iCub-main
Loading...
Searching...
No Matches
dataTypes.h
Go to the documentation of this file.
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
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
54
55typedef 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]);}
64
65typedef 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]);}
75
76typedef 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]);}
91
93//
95
96// 0xC2 : Acceleration & Angular Rate
97// BigEndian
98struct _C2_ {
99 uint8_t cmd;
102 uint32_t timer;
103 uint16_t checksum;
104} __attribute__((__packed__));
106
107// 0xC5 : Orientation Matrix
108// BigEndian
109struct _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
128
129// 0xCB : Acceleration, Angular Rate & Magnetometer Vectors
130// BigEndian
131struct _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
153
154// 0xCE : Euler Angles
155// BigEndian
156struct _CE_ {
157 uint8_t cmd;
159 uint32_t timer;
160 uint16_t checksum;
161} __attribute__((__packed__));
162typedef _CE_ eul_t;
163
164// 0xCF : Euler Angles & Angular Rate
165// BigEndian
166struct _CF_ {
167 uint8_t cmd;
170 uint32_t timer;
171 uint16_t checksum;
172} __attribute__((__packed__));
174
175// 0xDF : Quaternion
176// BigEndian
177struct _DF_ {
178 uint8_t cmd;
180 uint32_t timer;
181 uint16_t checksum;
182} __attribute__((__packed__));
183typedef _DF_ quat_t;
184
185
197
198
199typedef void*(*funptr_t)(data_3DM_GX3_t &);
200
201typedef struct {
202 uint8_t cmd;
203 uint8_t expSize;
206} imu_cmd_t;
207
209// herlper functions
211
212inline 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
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
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
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
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
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
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
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_ */
@ data
void * print_CE(data_3DM_GX3_t &data)
Definition dataTypes.h:372
unsigned short calc_checksum(uint8_t *buff, int len)
Definition dataTypes.h:212
void * print_DF(data_3DM_GX3_t &data)
Definition dataTypes.h:419
void * print_C2(data_3DM_GX3_t &data)
Definition dataTypes.h:232
_CF_ eul_angRate_t
Definition dataTypes.h:173
void * process_C2(data_3DM_GX3_t &data)
Definition dataTypes.h:221
void * process_CC(data_3DM_GX3_t &data)
Definition dataTypes.h:318
cmd
Definition dataTypes.h:30
@ CMD_RAW
Definition dataTypes.h:31
@ CMD_ACCEL_ANGRATE_MAG_ORIENT
Definition dataTypes.h:42
@ CMD_DELVEL_DELANG_MAG
Definition dataTypes.h:48
@ CMD_ATT_UPDATE
Definition dataTypes.h:36
@ CMD_EULER
Definition dataTypes.h:44
@ CMD_GYROSTAB_ANGRATE_MAG
Definition dataTypes.h:47
@ CMD_EULER_ANGRATE
Definition dataTypes.h:45
@ CMD_WRITE_ACCEL_BIAS
Definition dataTypes.h:39
@ CMD_ACCEL_ANGRATE_MAG
Definition dataTypes.h:41
@ CMD_WRITE_GYRO_BIAS
Definition dataTypes.h:40
@ CMD_TEMPERATURES
Definition dataTypes.h:46
@ CMD_ACCEL_ANGRATE_ORIENT
Definition dataTypes.h:38
@ CMD_QUATERNION
Definition dataTypes.h:50
@ CMD_ACCEL_ANGRATE
Definition dataTypes.h:32
@ CMD_MAG_VEC
Definition dataTypes.h:37
@ CMD_ORIENT
Definition dataTypes.h:35
@ CMD_DELVEL_DELANG
Definition dataTypes.h:33
@ CMD_CAPTURE_GYRO_BIAS
Definition dataTypes.h:43
@ CMD_SAMPLING_SETTING
Definition dataTypes.h:49
@ CMD_CONTINUOUS
Definition dataTypes.h:34
@ CMD_DEV_ID_STR
Definition dataTypes.h:51
@ CMD_STOP_CONTINUOUS
Definition dataTypes.h:52
void * print_CF(data_3DM_GX3_t &data)
Definition dataTypes.h:394
void * process_CE(data_3DM_GX3_t &data)
Definition dataTypes.h:362
void * print_CB(data_3DM_GX3_t &data)
Definition dataTypes.h:298
_C8_ acc_ang_orient_t
Definition dataTypes.h:127
uint16_t checksum
Definition dataTypes.h:4
void * process_C8(data_3DM_GX3_t &data)
Definition dataTypes.h:246
_CC_ acc_ang_mag_orient_t
Definition dataTypes.h:152
_4f_vect_t quat
Definition dataTypes.h:1
_C5_ orientMat_t
Definition dataTypes.h:115
void * process_CB(data_3DM_GX3_t &data)
Definition dataTypes.h:285
_DF_ quat_t
Definition dataTypes.h:183
_CB_ acc_ang_mag_t
Definition dataTypes.h:139
typedef __attribute__
void * process_CF(data_3DM_GX3_t &data)
Definition dataTypes.h:383
void *(* funptr_t)(data_3DM_GX3_t &)
Definition dataTypes.h:199
void * process_DF(data_3DM_GX3_t &data)
Definition dataTypes.h:409
_CE_ eul_t
Definition dataTypes.h:162
_C2_ acc_angRate_t
Definition dataTypes.h:105
void * print_C8(data_3DM_GX3_t &data)
Definition dataTypes.h:259
void * print_CC(data_3DM_GX3_t &data)
Definition dataTypes.h:332
uint8_t cmd
Definition dataTypes.h:99
uint16_t checksum
Definition dataTypes.h:103
uint32_t timer
Definition dataTypes.h:102
_3f_vect_t acc
Definition dataTypes.h:100
_3f_vect_t angRate
Definition dataTypes.h:101
_3f_matx_t orientMat
Definition dataTypes.h:111
uint32_t timer
Definition dataTypes.h:112
uint8_t cmd
Definition dataTypes.h:110
uint16_t checksum
Definition dataTypes.h:113
_3f_matx_t orientMat
Definition dataTypes.h:123
uint32_t timer
Definition dataTypes.h:124
_3f_vect_t acc
Definition dataTypes.h:121
uint16_t checksum
Definition dataTypes.h:125
uint8_t cmd
Definition dataTypes.h:120
_3f_vect_t angRate
Definition dataTypes.h:122
uint32_t timer
Definition dataTypes.h:136
uint16_t checksum
Definition dataTypes.h:137
uint8_t cmd
Definition dataTypes.h:132
_3f_vect_t mag
Definition dataTypes.h:135
_3f_vect_t angRate
Definition dataTypes.h:134
_3f_vect_t acc
Definition dataTypes.h:133
uint32_t timer
Definition dataTypes.h:149
_3f_matx_t orientMat
Definition dataTypes.h:148
uint16_t checksum
Definition dataTypes.h:150
uint8_t cmd
Definition dataTypes.h:144
_3f_vect_t mag
Definition dataTypes.h:147
_3f_vect_t angRate
Definition dataTypes.h:146
_3f_vect_t acc
Definition dataTypes.h:145
uint32_t timer
Definition dataTypes.h:159
uint8_t cmd
Definition dataTypes.h:157
uint16_t checksum
Definition dataTypes.h:160
_3f_vect_t eul
Definition dataTypes.h:158
uint8_t cmd
Definition dataTypes.h:167
uint32_t timer
Definition dataTypes.h:170
_3f_vect_t eul
Definition dataTypes.h:168
uint16_t checksum
Definition dataTypes.h:171
_3f_vect_t angRate
Definition dataTypes.h:169
uint8_t cmd
Definition dataTypes.h:178
uint32_t timer
Definition dataTypes.h:180
uint16_t checksum
Definition dataTypes.h:181
_4f_vect_t quat
Definition dataTypes.h:179
data_3DM_GX3_t data
Definition dataTypes.h:204
funptr_t process
Definition dataTypes.h:205
uint8_t cmd
Definition dataTypes.h:202
uint8_t expSize
Definition dataTypes.h:203
void swap_vect(void)
Definition dataTypes.h:89
float m13
Definition dataTypes.h:80
float m12
Definition dataTypes.h:79
float m21
Definition dataTypes.h:81
float m32
Definition dataTypes.h:85
float m11
Definition dataTypes.h:78
float m22
Definition dataTypes.h:82
float m33
Definition dataTypes.h:86
float m23
Definition dataTypes.h:83
float m31
Definition dataTypes.h:84
void swap_vect(void)
Definition dataTypes.h:62
float y
Definition dataTypes.h:58
float z
Definition dataTypes.h:59
float x
Definition dataTypes.h:57
float q1
Definition dataTypes.h:68
float q0
Definition dataTypes.h:67
void swap_vect(void)
Definition dataTypes.h:73
float q3
Definition dataTypes.h:70
float q2
Definition dataTypes.h:69
acc_ang_mag_orient_t aamom
Definition dataTypes.h:189
acc_angRate_t aa
Definition dataTypes.h:187
orientMat_t om
Definition dataTypes.h:191
eul_angRate_t ea
Definition dataTypes.h:192
acc_ang_orient_t aaom
Definition dataTypes.h:188
acc_ang_mag_t aam
Definition dataTypes.h:190