iCub-main
Loading...
Searching...
No Matches
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
34typedef 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
47typedef struct {
48 uint8_t frameControl;
49 uint8_t length;
50 uint8_t messageId;
51 uint8_t *payload;
52} __attribute__((__packed__))
53ST_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
62typedef enum{
66 nack
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
103inline 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
112inline 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
123inline 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
137inline 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
150inline 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
176inline 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
189inline 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
209inline 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
223inline 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
253inline 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
263inline 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
274inline 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
285inline 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
300inline 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
310inline 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_ */
FrameTypes
@ nack
@ control
@ data
@ ack
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
void * process_C2(data_3DM_GX3_t &data)
Definition dataTypes.h:221
void * process_CC(data_3DM_GX3_t &data)
Definition dataTypes.h:318
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
uint16_t checksum
Definition dataTypes.h:4
void * process_C8(data_3DM_GX3_t &data)
Definition dataTypes.h:246
_4f_vect_t quat
Definition dataTypes.h:1
void * process_CB(data_3DM_GX3_t &data)
Definition dataTypes.h:285
void * process_CF(data_3DM_GX3_t &data)
Definition dataTypes.h:383
void * process_DF(data_3DM_GX3_t &data)
Definition dataTypes.h:409
void * print_C8(data_3DM_GX3_t &data)
Definition dataTypes.h:259
void * print_CC(data_3DM_GX3_t &data)
Definition dataTypes.h:332
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:123
uint32_t timer
Definition dataTypes.h:124
_3f_vect_t acc
Definition dataTypes.h:121
uint16_t checksum
Definition dataTypes.h:125
_3f_vect_t angRate
Definition dataTypes.h:122
uint32_t timer
Definition dataTypes.h:136
uint16_t checksum
Definition dataTypes.h:137
_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
_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
uint16_t checksum
Definition dataTypes.h:160
_3f_vect_t eul
Definition dataTypes.h:158
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 * payload
uint8_t frameControl
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