iCub-main
dataTypes.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /* Copyright (C) 2013 iCub Facility, Istituto Italiano di Tecnologia
4  * Author: Alberto Cardellino
5  * email: alberto.cardellino@iit.it
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17  */
18 #ifndef DATATYPES_H_
19 #define DATATYPES_H_
20 
21 #include <byteswap.h>
22 #include <string>
23 #include <map>
24 #include <unistd.h>
25 #include <stdint.h>
26 #include <stdio.h>
27 
28 
29 // Enumeration of possible IMU commands
30 enum cmd {
31  CMD_RAW = 0xC1,
35  CMD_ORIENT = 0xC5,
37  CMD_MAG_VEC = 0xC7,
44  CMD_EULER = 0xCE,
53 };
54 
55 typedef union {
56  struct {
57  float x;
58  float y;
59  float z;
60  };
61  uint32_t _bytes[3];
62  void swap_vect(void) { for (int i=0; i<3; i++) _bytes[i] = bswap_32(_bytes[i]);}
63 } _3f_vect_t;
64 
65 typedef union {
66  struct {
67  float q0;
68  float q1;
69  float q2;
70  float q3;
71  };
72  uint32_t _bytes[4];
73  void swap_vect(void) { for (int i=0; i<4; i++) _bytes[i] = bswap_32(_bytes[i]);}
74 } _4f_vect_t;
75 
76 typedef union {
77  struct {
78  float m11;
79  float m12;
80  float m13;
81  float m21;
82  float m22;
83  float m23;
84  float m31;
85  float m32;
86  float m33;
87  };
88  uint32_t _bytes[9];
89  void swap_vect(void) { for (int i=0; i<9; i++) _bytes[i] = bswap_32(_bytes[i]);}
90 } _3f_matx_t;
91 
93 //
95 
96 // 0xC2 : Acceleration & Angular Rate
97 // BigEndian
98 struct _C2_ {
99  uint8_t cmd;
102  uint32_t timer;
103  uint16_t checksum;
104 } __attribute__((__packed__));
106 
107 // 0xC5 : Orientation Matrix
108 // BigEndian
109 struct _C5_ {
110  uint8_t cmd;
112  uint32_t timer;
113  uint16_t checksum;
114 } __attribute__((__packed__));
116 
117 // 0xC8 : Acceleration, Angular Rate & Orientation Matrix
118 // BigEndian
119 struct _C8_ {
120  uint8_t cmd;
124  uint32_t timer;
125  uint16_t checksum;
126 } __attribute__((__packed__));
128 
129 // 0xCB : Acceleration, Angular Rate & Magnetometer Vectors
130 // BigEndian
131 struct _CB_ {
132  uint8_t cmd;
136  uint32_t timer;
137  uint16_t checksum;
138 } __attribute__((__packed__));
140 
141 // 0xCC : Acceleration, Angular Rate & Magnetometer Vectors & Orientation Matrix
142 // BigEndian
143 struct _CC_ {
144  uint8_t cmd;
149  uint32_t timer;
150  uint16_t checksum;
151 } __attribute__((__packed__));
153 
154 // 0xCE : Euler Angles
155 // BigEndian
156 struct _CE_ {
157  uint8_t cmd;
159  uint32_t timer;
160  uint16_t checksum;
161 } __attribute__((__packed__));
162 typedef _CE_ eul_t;
163 
164 // 0xCF : Euler Angles & Angular Rate
165 // BigEndian
166 struct _CF_ {
167  uint8_t cmd;
170  uint32_t timer;
171  uint16_t checksum;
172 } __attribute__((__packed__));
174 
175 // 0xDF : Quaternion
176 // BigEndian
177 struct _DF_ {
178  uint8_t cmd;
180  uint32_t timer;
181  uint16_t checksum;
182 } __attribute__((__packed__));
183 typedef _DF_ quat_t;
184 
185 
186 typedef union {
187  acc_angRate_t aa;
188  acc_ang_orient_t aaom;
189  acc_ang_mag_orient_t aamom;
190  acc_ang_mag_t aam;
191  orientMat_t om;
192  eul_angRate_t ea;
193  eul_t eu;
194  quat_t quat;
195  uint8_t buffer[80];
197 
198 
199 typedef void*(*funptr_t)(data_3DM_GX3_t &);
200 
201 typedef struct {
202  uint8_t cmd;
203  uint8_t expSize;
206 } imu_cmd_t;
207 
209 // herlper functions
211 
212 inline unsigned short calc_checksum(uint8_t *buff, int len)
213 {
214  unsigned short checksum = 0;
215  for (int i = 0; i < len; i++) { checksum += buff[i]; }
216  return checksum;
217 }
218 
219 
220 
222 {
223  acc_angRate_t * aa = &data.aa;
224  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_angRate_t)-2);
225  aa->acc.swap_vect();
226  aa->angRate.swap_vect();
227  aa->timer = bswap_32(aa->timer);
228  aa->checksum = bswap_16(aa->checksum);
229  return (void*)(checksum == aa->checksum);
230 }
231 
232 inline void * print_C2(data_3DM_GX3_t &data)
233 {
234  acc_angRate_t * aa = &data.aa;
235  printf("Accel - Ang rate [0xC2]\n");
236  printf("\t%f %f %f\n\t%f %f %f\n",
237  aa->acc.x,
238  aa->acc.y,
239  aa->acc.z,
240  aa->angRate.x,
241  aa->angRate.y,
242  aa->angRate.z);
243  return 0;
244 }
245 
247 {
248  acc_ang_orient_t * aaom = &data.aaom;
249  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_ang_orient_t)-2);
250  aaom->acc.swap_vect();
251  aaom->angRate.swap_vect();
252  aaom->orientMat.swap_vect();
253  aaom->timer = bswap_32(aaom->timer);
254  aaom->checksum = bswap_16(aaom->checksum);
255  //printf("0x%02X 0x%02X\n", checksum, aaom->checksum);
256  return (void*)(checksum == aaom->checksum);
257 }
258 
259 inline void * print_C8(data_3DM_GX3_t &data)
260 {
261  acc_ang_orient_t * aaom = &data.aaom;
262 
263  printf("Accel - Ang rate - Mag - Orient M [0xCC]\n");
264  printf("\t%f %f %f\n",
265  aaom->acc.x,
266  aaom->acc.y,
267  aaom->acc.z);
268  printf("\t%f %f %f\n",
269  aaom->angRate.x,
270  aaom->angRate.y,
271  aaom->angRate.z);
272  printf("\t[%f %f %f\n\t%f %f %f\n\t%f %f %f]\n",
273  aaom->orientMat.m11,
274  aaom->orientMat.m12,
275  aaom->orientMat.m13,
276  aaom->orientMat.m21,
277  aaom->orientMat.m22,
278  aaom->orientMat.m23,
279  aaom->orientMat.m31,
280  aaom->orientMat.m32,
281  aaom->orientMat.m33);
282  return 0;
283 }
284 
286 {
287  acc_ang_mag_t * aam = &data.aam;
288  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_ang_mag_t)-2);
289  aam->acc.swap_vect();
290  aam->angRate.swap_vect();
291  aam->mag.swap_vect();
292  aam->timer = bswap_32(aam->timer);
293  aam->checksum = bswap_16(aam->checksum);
294  //printf("0x%02X 0x%02X\n", checksum, aamom->checksum);
295  return (void*)(checksum == aam->checksum);
296 }
297 
298 inline void * print_CB(data_3DM_GX3_t &data)
299 {
300  acc_ang_mag_t * aam = &data.aam;
301 
302  printf("Accel - Ang rate - Mag - Orient M [0xCC]\n");
303  printf("\t%f %f %f\n",
304  aam->acc.x,
305  aam->acc.y,
306  aam->acc.z);
307  printf("\t%f %f %f\n",
308  aam->angRate.x,
309  aam->angRate.y,
310  aam->angRate.z);
311  printf("\t%f %f %f\n",
312  aam->mag.x,
313  aam->mag.y,
314  aam->mag.z);
315  return 0;
316 }
317 
319 {
320  acc_ang_mag_orient_t * aamom = &data.aamom;
321  unsigned short checksum = calc_checksum(data.buffer, sizeof(acc_ang_mag_orient_t)-2);
322  aamom->acc.swap_vect();
323  aamom->angRate.swap_vect();
324  aamom->mag.swap_vect();
325  aamom->orientMat.swap_vect();
326  aamom->timer = bswap_32(aamom->timer);
327  aamom->checksum = bswap_16(aamom->checksum);
328  //printf("0x%02X 0x%02X\n", checksum, aamom->checksum);
329  return (void*)(checksum == aamom->checksum);
330 }
331 
332 inline void * print_CC(data_3DM_GX3_t &data)
333 {
334  acc_ang_mag_orient_t * aamom = &data.aamom;
335 
336  printf("Accel - Ang rate - Mag - Orient M [0xCC]\n");
337  printf("\t%f %f %f\n",
338  aamom->acc.x,
339  aamom->acc.y,
340  aamom->acc.z);
341  printf("\t%f %f %f\n",
342  aamom->angRate.x,
343  aamom->angRate.y,
344  aamom->angRate.z);
345  printf("\t%f %f %f\n",
346  aamom->mag.x,
347  aamom->mag.y,
348  aamom->mag.z);
349  printf("\t[%f %f %f\n\t%f %f %f\n\t%f %f %f]\n",
350  aamom->orientMat.m11,
351  aamom->orientMat.m12,
352  aamom->orientMat.m13,
353  aamom->orientMat.m21,
354  aamom->orientMat.m22,
355  aamom->orientMat.m23,
356  aamom->orientMat.m31,
357  aamom->orientMat.m32,
358  aamom->orientMat.m33);
359  return 0;
360 }
361 
363 {
364  eul_t * eu = &data.eu;
365  unsigned short checksum = calc_checksum(data.buffer, sizeof(eul_t)-2);
366  eu->eul.swap_vect();
367  eu->timer = bswap_32(eu->timer);
368  eu->checksum = bswap_16(eu->checksum);
369  return (void*)(checksum == eu->checksum);
370 }
371 
372 inline void * print_CE(data_3DM_GX3_t &data)
373 {
374  eul_t * eu = &data.eu;
375  printf("Euler ang [0xCE]\n");
376  printf("\t%f %f %f\n",
377  eu->eul.x,
378  eu->eul.y,
379  eu->eul.z);
380  return 0;
381 }
382 
384 {
385  eul_angRate_t * ea = &data.ea;
386  unsigned short checksum = calc_checksum(data.buffer, sizeof(eul_angRate_t)-2);
387  ea->eul.swap_vect();
388  ea->angRate.swap_vect();
389  ea->timer = bswap_32(ea->timer);
390  ea->checksum = bswap_16(ea->checksum);
391  return (void*)(checksum == ea->checksum);
392 }
393 
394 inline void * print_CF(data_3DM_GX3_t &data)
395 {
396  eul_angRate_t * ea = &data.ea;
397  printf("Euler ang [0xCE]\n");
398  printf("\t%f %f %f\n",
399  ea->eul.x,
400  ea->eul.y,
401  ea->eul.z);
402  printf("\t%f %f %f\n",
403  ea->angRate.x,
404  ea->angRate.y,
405  ea->angRate.z);
406  return 0;
407 }
408 
410 {
411  quat_t * quat = &data.quat;
412  unsigned short checksum = calc_checksum(data.buffer, sizeof(quat_t)-2);
413  quat->quat.swap_vect();
414  quat->timer = bswap_32(quat->timer);
415  quat->checksum = bswap_16(quat->checksum);
416  return (void*)(checksum == quat->checksum);
417 }
418 
419 inline void * print_DF(data_3DM_GX3_t &data)
420 {
421  quat_t * quat = &data.quat;
422  printf("Quaternion [0xDF]\n");
423  printf("\t%f %f %f %f\n",
424  quat->quat.q0,
425  quat->quat.q1,
426  quat->quat.q2,
427  quat->quat.q3);
428  return 0;
429 }
430 
431 
432 #endif /* DATATYPES_H_ */
eul_angRate_t ea
Definition: dataTypes.h:192
unsigned short calc_checksum(uint8_t *buff, int len)
Definition: dataTypes.h:212
_C8_ acc_ang_orient_t
Definition: dataTypes.h:127
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
data_3DM_GX3_t data
Definition: dataTypes.h:204
uint8_t cmd
Definition: dataTypes.h:110
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
_DF_ quat_t
Definition: dataTypes.h:183
_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
uint16_t checksum
Definition: dataTypes.h:113
_3f_matx_t orientMat
Definition: dataTypes.h:111
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
void *(* funptr_t)(data_3DM_GX3_t &)
Definition: dataTypes.h:199
_3f_matx_t orientMat
Definition: dataTypes.h:107
_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
uint32_t timer
Definition: dataTypes.h:112
uint8_t cmd
Definition: dataTypes.h:178
uint16_t checksum
Definition: dataTypes.h:125
float m12
Definition: dataTypes.h:79
float q2
Definition: dataTypes.h:69
funptr_t process
Definition: dataTypes.h:205
_3f_vect_t mag
Definition: dataTypes.h:135
_4f_vect_t quat
Definition: dataTypes.h:179
cmd
Definition: dataTypes.h:30
uint8_t cmd
Definition: dataTypes.h:144
orientMat_t om
Definition: dataTypes.h:191
uint8_t cmd
Definition: dataTypes.h:99
uint32_t timer
Definition: dataTypes.h:159
_CE_ eul_t
Definition: dataTypes.h:162
_C2_ acc_angRate_t
Definition: dataTypes.h:105
float m13
Definition: dataTypes.h:80
uint32_t timer
Definition: dataTypes.h:102
float m21
Definition: dataTypes.h:81
_CB_ acc_ang_mag_t
Definition: dataTypes.h:139
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
uint8_t cmd
Definition: dataTypes.h:157
float x
Definition: dataTypes.h:57
acc_ang_mag_orient_t aamom
Definition: dataTypes.h:189
_CC_ acc_ang_mag_orient_t
Definition: dataTypes.h:152
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
uint8_t cmd
Definition: dataTypes.h:120
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
uint8_t expSize
Definition: dataTypes.h:203
_C5_ orientMat_t
Definition: dataTypes.h:115
acc_ang_orient_t aaom
Definition: dataTypes.h:188
float q3
Definition: dataTypes.h:70
uint8_t cmd
Definition: dataTypes.h:132
_3f_vect_t angRate
Definition: dataTypes.h:122
void swap_vect(void)
Definition: dataTypes.h:62
uint8_t cmd
Definition: dataTypes.h:167
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__
void swap_vect(void)
Definition: dataTypes.h:73
uint8_t cmd
Definition: dataTypes.h:202
uint16_t checksum
Definition: dataTypes.h:171
void * process_CC(data_3DM_GX3_t &data)
Definition: dataTypes.h:318
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
_CF_ eul_angRate_t
Definition: dataTypes.h:173
_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