iCub-main
ST_M1_dataType.h
Go to the documentation of this file.
1 /* Copyright (C) 2015 iCub Facility, Istituto Italiano di Tecnologia
2  * Author: Alberto Cardellino
3  * email: alberto.cardellino@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found at
9  * http://www.robotcub.org/icub/license/gpl.txt
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15  */
16 
17 #ifndef _ST_M1_DATATYPES_
18 #define _ST_M1_DATATYPES_
19 
20 #include <byteswap.h>
21 #include <string>
22 #include <map>
23 #include <unistd.h>
24 #include <stdint.h>
25 #include <stdio.h>
26 
27 #define counter_base 3
28 #define accel_base 5
29 #define gyro_base 11
30 #define magneto_base 17
31 #define temperat_base 23
32 #define quaternion_base 37
33 
34 typedef struct {
35  // uint16_t counter;
36  int16_t accel[3];
37  int16_t gyro[3];
38  int16_t magn[3];
39  int16_t temp;
40  uint32_t euler_raw[3];
41  uint32_t quat[4];
42 } __attribute__ ((__packed__)) Pippo;
43 
44 
45 
46 
47 typedef struct {
48  uint8_t frameControl;
49  uint8_t length;
50  uint8_t messageId;
51  uint8_t *payload;
52 } __attribute__((__packed__))
53 ST_IMU_Frame;
54 
55 // Mask for Frame Control byte fields
56 #define FRAME_TYPE_MASK OXC0
57 #define ACK_MASK OX20
58 #define LF_MF_MASK OX10
59 #define FRAME_VERSION_MASK OX0C
60 #define QOS_MASK OX03
61 
62 typedef enum{
63  control = 0,
65  ack,
67 } FrameTypes;
68 
69 
70 // list of available Communication control commands
71 #define iNEMO_Connect 0x00
72 #define iNEMO_Disconnect 0x01
73 #define iNEMO_Reset_Board 0x02
74 #define iNEMO_Enter_DFU_Mode 0x00
75 #define iNEMO_Trace 0x07
76 #define iNEMO_Led_Control 0x08
77 
78 
79 // QoS possible values
80 #define NORMAL_PRIORITY 0X00
81 #define MEDIUM_PRIORITY 0X01
82 #define HIGH_PRIORITY 0X02
83 #define FRU 0X03
84 
85 
86 // list of available Board information commands
87 // TBD
88 
89 // list of available Sensor setting commands
90 
91 // Sensor_Type list
92 
93 // etc etc etc...
94 
95 
96 
97 #if 0
98 // 0xC5 : Orientation Matrix
100 // herlper functions
102 
103 inline unsigned short calc_checksum(uint8_t *buff, int len)
104 {
105  unsigned short checksum = 0;
106  for (int i = 0; i < len; i++) { checksum += buff[i]; }
107  return checksum;
108 }
109 
110 
111 
112 inline void * process_C2(data_3DM_GX3_t &data)
113 {
114  acc_angRate_t * aa = &data.aa;
115  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_angRate_t)-2);
116  aa->acc.swap_vect();
117  aa->angRate.swap_vect();
118  aa->timer = bswap_32(aa->timer);
119  aa->checksum = bswap_16(aa->checksum);
120  return (void*)(checksum == aa->checksum);
121 }
122 
123 inline void * print_C2(data_3DM_GX3_t &data)
124 {
125  acc_angRate_t * aa = &data.aa;
126  printf("Accel - Ang rate [0xC2]\n");
127  printf("\t%f %f %f\n\t%f %f %f\n",
128  aa->acc.x,
129  aa->acc.y,
130  aa->acc.z,
131  aa->angRate.x,
132  aa->angRate.y,
133  aa->angRate.z);
134  return 0;
135 }
136 
137 inline void * process_C8(data_3DM_GX3_t &data)
138 {
139  acc_ang_orient_t * aaom = &data.aaom;
140  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_ang_orient_t)-2);
141  aaom->acc.swap_vect();
142  aaom->angRate.swap_vect();
143  aaom->orientMat.swap_vect();
144  aaom->timer = bswap_32(aaom->timer);
145  aaom->checksum = bswap_16(aaom->checksum);
146  //printf("0x%02X 0x%02X\n", checksum, aaom->checksum);
147  return (void*)(checksum == aaom->checksum);
148 }
149 
150 inline void * print_C8(data_3DM_GX3_t &data)
151 {
152  acc_ang_orient_t * aaom = &data.aaom;
153 
154  printf("Accel - Ang rate - Mag - Orient M [0xCC]\n");
155  printf("\t%f %f %f\n",
156  aaom->acc.x,
157  aaom->acc.y,
158  aaom->acc.z);
159  printf("\t%f %f %f\n",
160  aaom->angRate.x,
161  aaom->angRate.y,
162  aaom->angRate.z);
163  printf("\t[%f %f %f\n\t%f %f %f\n\t%f %f %f]\n",
164  aaom->orientMat.m11,
165  aaom->orientMat.m12,
166  aaom->orientMat.m13,
167  aaom->orientMat.m21,
168  aaom->orientMat.m22,
169  aaom->orientMat.m23,
170  aaom->orientMat.m31,
171  aaom->orientMat.m32,
172  aaom->orientMat.m33);
173  return 0;
174 }
175 
176 inline void * process_CB(data_3DM_GX3_t &data)
177 {
178  acc_ang_mag_t * aam = &data.aam;
179  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_ang_mag_t)-2);
180  aam->acc.swap_vect();
181  aam->angRate.swap_vect();
182  aam->mag.swap_vect();
183  aam->timer = bswap_32(aam->timer);
184  aam->checksum = bswap_16(aam->checksum);
185  //printf("0x%02X 0x%02X\n", checksum, aamom->checksum);
186  return (void*)(checksum == aam->checksum);
187 }
188 
189 inline void * print_CB(data_3DM_GX3_t &data)
190 {
191  acc_ang_mag_t * aam = &data.aam;
192 
193  printf("Accel - Ang rate - Mag - Orient M [0xCC]\n");
194  printf("\t%f %f %f\n",
195  aam->acc.x,
196  aam->acc.y,
197  aam->acc.z);
198  printf("\t%f %f %f\n",
199  aam->angRate.x,
200  aam->angRate.y,
201  aam->angRate.z);
202  printf("\t%f %f %f\n",
203  aam->mag.x,
204  aam->mag.y,
205  aam->mag.z);
206  return 0;
207 }
208 
209 inline void * process_CC(data_3DM_GX3_t &data)
210 {
211  acc_ang_mag_orient_t * aamom = &data.aamom;
212  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_ang_mag_orient_t)-2);
213  aamom->acc.swap_vect();
214  aamom->angRate.swap_vect();
215  aamom->mag.swap_vect();
216  aamom->orientMat.swap_vect();
217  aamom->timer = bswap_32(aamom->timer);
218  aamom->checksum = bswap_16(aamom->checksum);
219  //printf("0x%02X 0x%02X\n", checksum, aamom->checksum);
220  return (void*)(checksum == aamom->checksum);
221 }
222 
223 inline void * print_CC(data_3DM_GX3_t &data)
224 {
225  acc_ang_mag_orient_t * aamom = &data.aamom;
226 
227  printf("Accel - Ang rate - Mag - Orient M [0xCC]\n");
228  printf("\t%f %f %f\n",
229  aamom->acc.x,
230  aamom->acc.y,
231  aamom->acc.z);
232  printf("\t%f %f %f\n",
233  aamom->angRate.x,
234  aamom->angRate.y,
235  aamom->angRate.z);
236  printf("\t%f %f %f\n",
237  aamom->mag.x,
238  aamom->mag.y,
239  aamom->mag.z);
240  printf("\t[%f %f %f\n\t%f %f %f\n\t%f %f %f]\n",
241  aamom->orientMat.m11,
242  aamom->orientMat.m12,
243  aamom->orientMat.m13,
244  aamom->orientMat.m21,
245  aamom->orientMat.m22,
246  aamom->orientMat.m23,
247  aamom->orientMat.m31,
248  aamom->orientMat.m32,
249  aamom->orientMat.m33);
250  return 0;
251 }
252 
253 inline void * process_CE(data_3DM_GX3_t &data)
254 {
255  eul_t * eu = &data.eu;
256  unsigned short checksum = calc_checksum(data.buffer, sizeof(eul_t)-2);
257  eu->eul.swap_vect();
258  eu->timer = bswap_32(eu->timer);
259  eu->checksum = bswap_16(eu->checksum);
260  return (void*)(checksum == eu->checksum);
261 }
262 
263 inline void * print_CE(data_3DM_GX3_t &data)
264 {
265  eul_t * eu = &data.eu;
266  printf("Euler ang [0xCE]\n");
267  printf("\t%f %f %f\n",
268  eu->eul.x,
269  eu->eul.y,
270  eu->eul.z);
271  return 0;
272 }
273 
274 inline void * process_CF(data_3DM_GX3_t &data)
275 {
276  eul_angRate_t * ea = &data.ea;
277  unsigned short checksum = calc_checksum(data.buffer, sizeof(eul_angRate_t)-2);
278  ea->eul.swap_vect();
279  ea->angRate.swap_vect();
280  ea->timer = bswap_32(ea->timer);
281  ea->checksum = bswap_16(ea->checksum);
282  return (void*)(checksum == ea->checksum);
283 }
284 
285 inline void * print_CF(data_3DM_GX3_t &data)
286 {
287  eul_angRate_t * ea = &data.ea;
288  printf("Euler ang [0xCE]\n");
289  printf("\t%f %f %f\n",
290  ea->eul.x,
291  ea->eul.y,
292  ea->eul.z);
293  printf("\t%f %f %f\n",
294  ea->angRate.x,
295  ea->angRate.y,
296  ea->angRate.z);
297  return 0;
298 }
299 
300 inline void * process_DF(data_3DM_GX3_t &data)
301 {
302  quat_t * quat = &data.quat;
303  unsigned short checksum = calc_checksum(data.buffer, sizeof(quat_t)-2);
304  quat->quat.swap_vect();
305  quat->timer = bswap_32(quat->timer);
306  quat->checksum = bswap_16(quat->checksum);
307  return (void*)(checksum == quat->checksum);
308 }
309 
310 inline void * print_DF(data_3DM_GX3_t &data)
311 {
312  quat_t * quat = &data.quat;
313  printf("Quaternion [0xDF]\n");
314  printf("\t%f %f %f %f\n",
315  quat->quat.q0,
316  quat->quat.q1,
317  quat->quat.q2,
318  quat->quat.q3);
319  return 0;
320 }
321 #endif
322 
323 #endif /* _ST_M1_DATATYPES_ */
eul_angRate_t ea
Definition: dataTypes.h:192
unsigned short calc_checksum(uint8_t *buff, int len)
Definition: dataTypes.h:212
void * process_CB(data_3DM_GX3_t &data)
Definition: dataTypes.h:285
float m22
Definition: dataTypes.h:82
_3f_vect_t angRate
Definition: dataTypes.h:101
void * print_CE(data_3DM_GX3_t &data)
Definition: dataTypes.h:372
float m23
Definition: dataTypes.h:83
uint16_t checksum
Definition: dataTypes.h:103
_3f_vect_t eul
Definition: dataTypes.h:158
uint32_t timer
Definition: dataTypes.h:180
_3f_vect_t acc
Definition: dataTypes.h:133
_3f_vect_t acc
Definition: dataTypes.h:145
Definition: dataTypes.h:98
_3f_matx_t orientMat
Definition: dataTypes.h:123
uint32_t timer
Definition: dataTypes.h:124
_3f_vect_t angRate
Definition: dataTypes.h:169
_3f_vect_t acc
Definition: dataTypes.h:121
void * print_CC(data_3DM_GX3_t &data)
Definition: dataTypes.h:332
float m33
Definition: dataTypes.h:86
void swap_vect(void)
Definition: dataTypes.h:89
float m31
Definition: dataTypes.h:84
_3f_matx_t orientMat
Definition: dataTypes.h:148
uint16_t checksum
Definition: dataTypes.h:160
_3f_vect_t mag
Definition: dataTypes.h:147
void * process_C8(data_3DM_GX3_t &data)
Definition: dataTypes.h:246
float q1
Definition: dataTypes.h:68
_3f_vect_t angRate
Definition: dataTypes.h:146
uint32_t timer
Definition: dataTypes.h:136
_4f_vect_t quat
Definition: dataTypes.h:175
uint16_t checksum
Definition: dataTypes.h:125
float m12
Definition: dataTypes.h:79
float q2
Definition: dataTypes.h:69
_3f_vect_t mag
Definition: dataTypes.h:135
_4f_vect_t quat
Definition: dataTypes.h:179
uint8_t messageId
uint32_t timer
Definition: dataTypes.h:159
FrameTypes
float m13
Definition: dataTypes.h:80
uint32_t timer
Definition: dataTypes.h:102
float m21
Definition: dataTypes.h:81
void * process_C2(data_3DM_GX3_t &data)
Definition: dataTypes.h:221
void * print_C8(data_3DM_GX3_t &data)
Definition: dataTypes.h:259
acc_angRate_t aa
Definition: dataTypes.h:187
void * print_DF(data_3DM_GX3_t &data)
Definition: dataTypes.h:419
float x
Definition: dataTypes.h:57
acc_ang_mag_orient_t aamom
Definition: dataTypes.h:189
void * print_CB(data_3DM_GX3_t &data)
Definition: dataTypes.h:298
_3f_vect_t angRate
Definition: dataTypes.h:134
uint32_t timer
Definition: dataTypes.h:170
float m11
Definition: dataTypes.h:78
void * print_C2(data_3DM_GX3_t &data)
Definition: dataTypes.h:232
uint16_t checksum
Definition: dataTypes.h:181
void * print_CF(data_3DM_GX3_t &data)
Definition: dataTypes.h:394
acc_ang_mag_t aam
Definition: dataTypes.h:190
acc_ang_orient_t aaom
Definition: dataTypes.h:188
float q3
Definition: dataTypes.h:70
_3f_vect_t angRate
Definition: dataTypes.h:122
void swap_vect(void)
Definition: dataTypes.h:62
float m32
Definition: dataTypes.h:85
void * process_DF(data_3DM_GX3_t &data)
Definition: dataTypes.h:409
void * process_CE(data_3DM_GX3_t &data)
Definition: dataTypes.h:362
uint16_t checksum
Definition: dataTypes.h:137
uint16_t checksum
Definition: dataTypes.h:150
float q0
Definition: dataTypes.h:67
typedef __attribute__
uint16_t checksum
Definition: dataTypes.h:126
void swap_vect(void)
Definition: dataTypes.h:73
uint16_t checksum
Definition: dataTypes.h:171
void * process_CC(data_3DM_GX3_t &data)
Definition: dataTypes.h:318
uint8_t * payload
float y
Definition: dataTypes.h:58
uint32_t timer
Definition: dataTypes.h:149
float z
Definition: dataTypes.h:59
uint8_t buffer[80]
Definition: dataTypes.h:195
uint8_t frameControl
_3f_vect_t eul
Definition: dataTypes.h:168
void * process_CF(data_3DM_GX3_t &data)
Definition: dataTypes.h:383
_3f_vect_t acc
Definition: dataTypes.h:100