Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
imu_chimu.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 /*---------------------------------------------------------------------------
23  Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
24 
25  File: *.c
26 
27  Description: CHIMU Protocol Parser
28 
29 
30  Public Functions:
31  CHIMU_Init Create component instance
32  CHIMU_Parse Parse the RX byte stream message
33 
34  Applicable Documents:
35  CHIMU User Manual
36 
37  Adapted to paparazzi by C. De Wagter
38 
39  ---------------------------------------------------------------------------*/
40 
41 #include "imu_chimu.h"
42 #include "string.h"
43 #include "math.h"
44 
45 
46 /***************************************************************************
47  * Cyclic Redundancy Checksum
48  */
49 
50 static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long data_len)
51 {
52  unsigned long i; // loop counter
53 #define POLY 0xEDB88320 // bit-reversed version of the poly 0x04C11DB7
54  // Create the CRC "dividend" for polynomial arithmetic (binary arithmetic
55  // with no carries)
56 
57  unsigned char *CRC_input = (unsigned char *)data;
58  for (unsigned long j = data_len; j; --j) {
59 
60  CRC_acc = CRC_acc ^ *CRC_input++;
61  // "Divide" the poly into the dividend using CRC XOR subtraction
62  // CRC_acc holds the "remainder" of each divide
63  //
64  // Only complete this division for 8 bits since input is 1 byte
65  for (i = 0; i < 8; i++) {
66  // Check if the MSB is set (if MSB is 1, then the POLY can "divide"
67  // into the "dividend")
68  if ((CRC_acc & 0x00000001) == 0x00000001) {
69  // if so, shift the CRC value, and XOR "subtract" the poly
70  CRC_acc = CRC_acc >> 1;
71  CRC_acc ^= POLY;
72  } else {
73  // if not, just shift the CRC value
74  CRC_acc = CRC_acc >> 1;
75  }
76  }
77  }
78  // Return the final remainder (CRC value)
79  return CRC_acc;
80 }
81 
82 void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
83 {
84  data[buflen - 1] = (unsigned char)(UpdateCRC(0xFFFFFFFF , data , (unsigned long)(buflen - 1)) & 0xff) ;
85 }
86 
87 
88 /***************************************************************************
89  * CHIMU Protocol Definition
90  */
91 
92 // Lowlevel Protocol Decoding
93 #define CHIMU_STATE_MACHINE_START 0
94 #define CHIMU_STATE_MACHINE_HEADER2 1
95 #define CHIMU_STATE_MACHINE_LEN 2
96 #define CHIMU_STATE_MACHINE_DEVICE 3
97 #define CHIMU_STATE_MACHINE_ID 4
98 #define CHIMU_STATE_MACHINE_PAYLOAD 5
99 #define CHIMU_STATE_MACHINE_XSUM 6
100 
101 // Communication Definitions
102 #define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above
103 
104 /*---------------------------------------------------------------------------
105  Name: CHIMU_Init
106 
107  ---------------------------------------------------------------------------*/
108 void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
109 {
110  unsigned char i;
111  pstData->m_State = CHIMU_STATE_MACHINE_START;
112  pstData->m_Checksum = 0x00;
113  pstData->m_ReceivedChecksum = 0x00;
114  pstData->m_Index = 0;
115  pstData->m_PayloadIndex = 0;
116 
117  //Sensor data holder
118  pstData->m_sensor.cputemp = 0.0;
119  for (i = 0; i < 3; i++) {
120  pstData->m_sensor.acc[i] = 0.0;
121  pstData->m_sensor.rate[i] = 0.0;
122  pstData->m_sensor.mag[i] = 0.0;
123  }
124  pstData->m_sensor.spare1 = 0.0;
125  //Attitude data
126  pstData->m_attitude.euler.phi = 0.0;
127  pstData->m_attitude.euler.theta = 0.0;
128  pstData->m_attitude.euler.psi = 0.0;
129  //Attitude rate data
130  pstData->m_attrates.euler.phi = 0.0;
131  pstData->m_attrates.euler.theta = 0.0;
132  pstData->m_attrates.euler.psi = 0.0;
133 
134  for (i = 0; i < CHIMU_RX_BUFFERSIZE; i++) {
135  pstData->m_Payload[i] = 0x00;
136  pstData->m_FullMessage[i] = 0x00;
137  }
138  pstData->m_MsgLen = 0;
139  pstData->m_MsgID = 0;
140  pstData->m_TempDeviceID = 0;
141  pstData->m_DeviceID = 0x01; //look at this later
142 }
143 
144 /*---------------------------------------------------------------------------
145  Name: CHIMU_Parse
146  Abstract: Parse message, returns TRUE if new data.
147  ---------------------------------------------------------------------------*/
148 
149 unsigned char CHIMU_Parse(
150  unsigned char btData, /* input byte stream buffer */
151  unsigned char bInputType __attribute__((
152  unused)), /* for future use if special builds of CHIMU data are performed */
153  CHIMU_PARSER_DATA *pstData) /* resulting data */
154 {
155 
156  unsigned char bUpdate = 0;
157 
158  switch (pstData->m_State) {
159  case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE
160  if (btData == 0xAE) {
161  pstData->m_State = CHIMU_STATE_MACHINE_HEADER2;
162  pstData->m_Index = 0;
163  pstData->m_FullMessage[pstData->m_Index++] = btData;
164  } else {
165  ;;
166  }
167  bUpdate = 0;
168  break;
169  case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE
170  if (btData == 0xAE) {
171  pstData->m_State = CHIMU_STATE_MACHINE_LEN;
172  pstData->m_FullMessage[pstData->m_Index++] = btData;
173  } else {
174  pstData->m_State = CHIMU_STATE_MACHINE_START;
175  } //Fail to see header. Restart.
176  break;
177  case CHIMU_STATE_MACHINE_LEN: // Get chars to read
178  if (btData <= CHIMU_RX_BUFFERSIZE) {
179  pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size
180  pstData->m_FullMessage[pstData->m_Index++] = btData;
181  pstData->m_State = CHIMU_STATE_MACHINE_DEVICE;
182  } else {
183  pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart
184  //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
185  }
186  break;
187  case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk
188  if ((btData == pstData->m_DeviceID) || (btData == 0xAA)) {
189  //0xAA is global message
190  pstData->m_TempDeviceID = btData;
191  pstData->m_FullMessage[pstData->m_Index++] = btData;
192  pstData->m_State = CHIMU_STATE_MACHINE_ID;
193  } else {
194  pstData->m_State = CHIMU_STATE_MACHINE_START;
195  } //Fail to see correct device ID. Restart.
196  break;
197  case CHIMU_STATE_MACHINE_ID: // Get ID
198  pstData->m_MsgID = btData; // might be invalid, chgeck it out here:
199  if (pstData->m_MsgID > CHIMU_COM_ID_HIGH) {
200  pstData->m_State = CHIMU_STATE_MACHINE_START;
201  //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
202  } else {
203  pstData->m_FullMessage[pstData->m_Index++] = btData;
204  pstData->m_PayloadIndex = 0;
205  pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go...
206  }
207  break;
208  case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload
209  pstData->m_Payload[pstData->m_PayloadIndex++] = btData;
210  pstData->m_FullMessage[pstData->m_Index++] = btData;
211  if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) {
212  //Now we have the payload. Verify XSUM and then parse it next
213  pstData->m_Checksum = (unsigned char)((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage ,
214  (unsigned long)(pstData->m_MsgLen) + 5)) & 0xFF);
215  pstData->m_State = CHIMU_STATE_MACHINE_XSUM;
216  } else {
217  return 0;
218  }
219  break;
220  case CHIMU_STATE_MACHINE_XSUM: // Verify
221  pstData->m_ReceivedChecksum = btData;
222  pstData->m_FullMessage[pstData->m_Index++] = btData;
223  if (pstData->m_Checksum != pstData->m_ReceivedChecksum) {
224  bUpdate = 0;
225  //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
226  } else {
227  //Xsum passed, go parse it.
228  // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data.
229  bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData);
230  }
231  pstData->m_State = CHIMU_STATE_MACHINE_START;
232  break;
233  default:
234  pstData->m_State = CHIMU_STATE_MACHINE_START;
235  } /* End of SWITCH */
236  return (bUpdate);
237 }
238 
239 
241 // Process CHIMU sentence - Use the CHIMU address (*pCommand) and call the
242 // appropriate sentence data processor.
244 
245 static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude)
246 {
247  CHIMU_attitude_data ps;
248  ps = attitude;
249  float x, sqw, sqx, sqy, sqz, norm;
250  sqw = ps.q.s * ps.q.s;
251  sqx = ps.q.v.x * ps.q.v.x;
252  sqy = ps.q.v.y * ps.q.v.y;
253  sqz = ps.q.v.z * ps.q.v.z;
254  norm = sqrt(sqw + sqx + sqy + sqz);
255  //Normalize the quat
256  ps.q.s = ps.q.s / norm;
257  ps.q.v.x = ps.q.v.x / norm;
258  ps.q.v.y = ps.q.v.y / norm;
259  ps.q.v.z = ps.q.v.z / norm;
260  ps.euler.phi = atan2(2.0 * (ps.q.s * ps.q.v.x + ps.q.v.y * ps.q.v.z), (1 - 2 * (sqx + sqy)));
261  if (ps.euler.phi < 0) { ps.euler.phi = ps.euler.phi + 2 * M_PI; }
262  x = ((2.0 * (ps.q.s * ps.q.v.y - ps.q.v.z * ps.q.v.x)));
263  //Below needed in event normalization not done
264  if (x > 1.0) { x = 1.0; }
265  if (x < -1.0) { x = -1.0; }
266  //
267  if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5) {
268  ps.euler.theta = 2 * atan2(ps.q.v.x, ps.q.s);
269  } else if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) {
270  ps.euler.theta = -2 * atan2(ps.q.v.x, ps.q.s);
271  } else {
272  ps.euler.theta = asin(x);
273  }
274  ps.euler.psi = atan2(2.0 * (ps.q.s * ps.q.v.z + ps.q.v.x * ps.q.v.y), (1 - 2 * (sqy + sqz)));
275  if (ps.euler.psi < 0) {
276  ps.euler.psi = ps.euler.psi + (2 * M_PI);
277  }
278 
279  return (ps);
280 
281 }
282 
283 static unsigned char BitTest(unsigned char input, unsigned char n)
284 {
285  //Test a bit in n and return TRUE or FALSE
286  if (input & (1 << n)) { return 1; } else { return 0; }
287 }
288 unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID __attribute__((unused)), unsigned char *pPayloadData,
289  CHIMU_PARSER_DATA *pstData)
290 {
291  //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to
292  //CHIMU).
293 
294  //Any CHIMU messages coming from the ground should be ignored, as that byte stream goes up to CHIMU
295  // by itself. However, here we should decode CHIMU messages being received and
296  // a) pass them down to ground
297  // b) grab the data from the CHIMU for our own needs / purposes
298  int CHIMU_index = 0;
299  float sanity_check = 0.0;
300 
301  switch (pstData->m_MsgID) {
302  case CHIMU_Msg_0_Ping:
303  CHIMU_index = 0;
304  pstData->gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++;
305  pstData->gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++;
306  pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++;
307  pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index] << 8) & (0x0000FF00); CHIMU_index++;
308  pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++;
309 
310  return 1;
311  break;
312  case CHIMU_Msg_1_IMU_Raw:
313  break;
314  case CHIMU_Msg_2_IMU_FP:
315  CHIMU_index = 0;
316  memmove(&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));
317  CHIMU_index += (sizeof(pstData->m_sensor.cputemp));
318  pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp);
319  memmove(&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));
320  CHIMU_index += (sizeof(pstData->m_sensor.acc));
321  pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]);
322  pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]);
323  pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]);
324  memmove(&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));
325  CHIMU_index += (sizeof(pstData->m_sensor.rate));
326  pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]);
327  pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]);
328  pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]);
329  memmove(&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));
330  CHIMU_index += (sizeof(pstData->m_sensor.mag));
331  pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]);
332  pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]);
333  pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]);
334  memmove(&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));
335  CHIMU_index += (sizeof(pstData->m_sensor.spare1));
336  pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1);
337  return 1;
338  break;
339  case CHIMU_Msg_3_IMU_Attitude:
340  //Attitude message data from CHIMU
341  // Includes attitude and rates only, along with configuration bits
342  // All you need for control
343 
344  //Led_On(LED_RED);
345 
346  CHIMU_index = 0;
347  memmove(&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));
348  CHIMU_index += sizeof(pstData->m_attitude.euler);
349  pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi);
350  pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta);
351  pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi);
352  memmove(&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));
353  CHIMU_index += (sizeof(pstData->m_sensor.rate));
354  pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]);
355  pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]);
356  pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]);
357 
358  memmove(&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));
359  CHIMU_index += sizeof(pstData->m_attitude.q);
360  pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s);
361  pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x);
362  pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y);
363  pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z);
364 
365  memmove(&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));
366  CHIMU_index += sizeof(pstData->m_attitude.q);
367  pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s);
368  pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x);
369  pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y);
370  pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z);
371 
372  //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control
373  pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0];
374  pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1];
375  pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2];
376 
377  pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
378  pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
379  pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
380 
381  // TODO: Read configuration bits
382 
383  /* bC0_SPI_En = BitTest (gConfigInfo, 0);
384  bC1_HWCentrip_En = BitTest (gConfigInfo, 1);
385  bC2_TempCal_En = BitTest (gConfigInfo, 2);
386  bC3_RateOut_En = BitTest (gConfigInfo, 3);
387  bC4_TBD = BitTest (gConfigInfo, 4);
388  bC5_Quat_Est = BitTest (gConfigInfo, 5);
389  bC6_SWCentrip_En = BitTest (gConfigInfo, 6);
390  bC7_AllowHW_Override = BitTest (gConfigInfo, 7);
391  */
392  //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected
393  if (BitTest(pstData->gConfigInfo, 5) == TRUE) {
394  pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude));
395  }
396 
397  //NEW: Checks for bad attitude data (bad SPI maybe?)
398  // Only allow globals to contain updated data if it makes sense
399  sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s);
400  sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x);
401  sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y);
402  sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z);
403 
404  //Should be 1.0 (normalized quaternion)
405  if ((sanity_check > 0.8) && (sanity_check < 1.2)) {
406  // gAttitude = pstData->m_attitude;
407  // gAttRates = pstData->m_attrates;
408  // gSensor = pstData->m_sensor;
409  } else {
410  //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?)
411  }
412 
413  return 1;
414  break;
415  case CHIMU_Msg_4_BiasSF:
416  case CHIMU_Msg_5_BIT:
417  case CHIMU_Msg_6_MagCal:
418  case CHIMU_Msg_7_GyroBias:
419  case CHIMU_Msg_8_TempCal:
420  case CHIMU_Msg_9_DAC_Offsets:
421  case CHIMU_Msg_10_Res:
422  case CHIMU_Msg_11_Res:
423  case CHIMU_Msg_12_Res:
424  case CHIMU_Msg_13_Res:
425  case CHIMU_Msg_14_RefVector:
426  case CHIMU_Msg_15_SFCheck:
427  break;
428  default:
429  return 0;
430  break;
431  }
432  return 0;
433 }
CHIMU_Msg_13_Res
#define CHIMU_Msg_13_Res
Definition: imu_chimu.h:84
CHIMU_Init
void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:108
CHIMU_Msg_12_Res
#define CHIMU_Msg_12_Res
Definition: imu_chimu.h:83
CHIMU_Msg_4_BiasSF
#define CHIMU_Msg_4_BiasSF
Definition: imu_chimu.h:75
UpdateCRC
static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long data_len)
Definition: imu_chimu.c:50
CHIMU_PARSER_DATA::gCHIMU_SW_Exclaim
uint8_t gCHIMU_SW_Exclaim
Definition: imu_chimu.h:173
CHIMU_PARSER_DATA::m_Index
unsigned char m_Index
Definition: imu_chimu.h:160
CHIMU_PARSER_DATA
Definition: imu_chimu.h:156
CHIMU_Msg_7_GyroBias
#define CHIMU_Msg_7_GyroBias
Definition: imu_chimu.h:78
CHIMU_attitude_data
Definition: imu_chimu.h:134
CHIMU_STATE_MACHINE_XSUM
#define CHIMU_STATE_MACHINE_XSUM
Definition: imu_chimu.c:99
CHIMU_Msg_9_DAC_Offsets
#define CHIMU_Msg_9_DAC_Offsets
Definition: imu_chimu.h:80
CHIMU_PARSER_DATA::m_TempDeviceID
unsigned char m_TempDeviceID
Definition: imu_chimu.h:164
imu_chimu.h
CHIMU_attitude_data::q
CHIMU_Quaternion q
Definition: imu_chimu.h:136
CHIMU_sensor_data::mag
float mag[3]
Definition: imu_chimu.h:150
CHIMU_PARSER_DATA::m_MsgLen
unsigned char m_MsgLen
Definition: imu_chimu.h:163
CHIMU_sensor_data::acc
float acc[3]
Definition: imu_chimu.h:148
CHIMU_Msg_8_TempCal
#define CHIMU_Msg_8_TempCal
Definition: imu_chimu.h:79
CHIMU_PARSER_DATA::m_Payload
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]
Definition: imu_chimu.h:166
CHIMU_PARSER_DATA::gCHIMU_SW_Major
uint8_t gCHIMU_SW_Major
Definition: imu_chimu.h:174
CHIMU_STATE_MACHINE_DEVICE
#define CHIMU_STATE_MACHINE_DEVICE
Definition: imu_chimu.c:96
CHIMU_Euler::theta
float theta
Definition: imu_chimu.h:119
CHIMU_PARSER_DATA::gCalStatus
uint8_t gCalStatus
Definition: imu_chimu.h:179
CHIMU_STATE_MACHINE_HEADER2
#define CHIMU_STATE_MACHINE_HEADER2
Definition: imu_chimu.c:94
CHIMU_PARSER_DATA::gCHIMU_BIT
uint8_t gCHIMU_BIT
Definition: imu_chimu.h:180
attitude
#define attitude
Definition: cc2500_compat.h:79
CHIMU_STATE_MACHINE_START
#define CHIMU_STATE_MACHINE_START
Definition: imu_chimu.c:93
CHIMU_Msg_1_IMU_Raw
#define CHIMU_Msg_1_IMU_Raw
Definition: imu_chimu.h:72
CHIMU_RX_BUFFERSIZE
#define CHIMU_RX_BUFFERSIZE
Definition: imu_chimu.h:154
CHIMU_Msg_5_BIT
#define CHIMU_Msg_5_BIT
Definition: imu_chimu.h:76
CHIMU_Msg_0_Ping
#define CHIMU_Msg_0_Ping
Definition: imu_chimu.h:71
CHIMU_Msg_10_Res
#define CHIMU_Msg_10_Res
Definition: imu_chimu.h:81
CHIMU_PARSER_DATA::gCHIMU_SW_Minor
uint8_t gCHIMU_SW_Minor
Definition: imu_chimu.h:175
CHIMU_PARSER_DATA::m_State
unsigned char m_State
Definition: imu_chimu.h:157
CHIMU_Vector::x
float x
Definition: imu_chimu.h:124
CHIMU_PARSER_DATA::m_attrates
CHIMU_attitude_data m_attrates
Definition: imu_chimu.h:169
CHIMU_Msg_14_RefVector
#define CHIMU_Msg_14_RefVector
Definition: imu_chimu.h:85
CHIMU_Quaternion::s
float s
Definition: imu_chimu.h:130
CHIMU_PARSER_DATA::m_attitude
CHIMU_attitude_data m_attitude
Definition: imu_chimu.h:168
CHIMU_PARSER_DATA::m_PayloadIndex
unsigned char m_PayloadIndex
Definition: imu_chimu.h:161
CHIMU_PARSER_DATA::gConfigInfo
uint8_t gConfigInfo
Definition: imu_chimu.h:181
CHIMU_Vector::z
float z
Definition: imu_chimu.h:126
CHIMU_Msg_15_SFCheck
#define CHIMU_Msg_15_SFCheck
Definition: imu_chimu.h:86
CHIMU_STATE_MACHINE_ID
#define CHIMU_STATE_MACHINE_ID
Definition: imu_chimu.c:97
CHIMU_sensor_data::spare1
float spare1
Definition: imu_chimu.h:151
CHIMU_Msg_2_IMU_FP
#define CHIMU_Msg_2_IMU_FP
Definition: imu_chimu.h:73
CHIMU_sensor_data::rate
float rate[3]
Definition: imu_chimu.h:149
CHIMU_attitude_data::euler
CHIMU_Euler euler
Definition: imu_chimu.h:135
POLY
#define POLY
BitTest
static unsigned char BitTest(unsigned char input, unsigned char n)
Definition: imu_chimu.c:283
CHIMU_Msg_11_Res
#define CHIMU_Msg_11_Res
Definition: imu_chimu.h:82
CHIMU_Quaternion::v
CHIMU_Vector v
Definition: imu_chimu.h:131
CHIMU_PARSER_DATA::m_ReceivedChecksum
unsigned char m_ReceivedChecksum
Definition: imu_chimu.h:159
CHIMU_sensor_data::cputemp
float cputemp
Definition: imu_chimu.h:147
CHIMU_Euler::phi
float phi
Definition: imu_chimu.h:118
CHIMU_PARSER_DATA::m_FullMessage
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]
Definition: imu_chimu.h:167
CHIMU_STATE_MACHINE_PAYLOAD
#define CHIMU_STATE_MACHINE_PAYLOAD
Definition: imu_chimu.c:98
CHIMU_COM_ID_HIGH
#define CHIMU_COM_ID_HIGH
Definition: imu_chimu.c:102
FloatSwap
#define FloatSwap(X)
Definition: imu_chimu.h:112
CHIMU_ProcessMessage
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:288
CHIMU_PARSER_DATA::m_Checksum
unsigned char m_Checksum
Definition: imu_chimu.h:158
TRUE
#define TRUE
Definition: std.h:4
CHIMU_Parse
unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData)
Definition: imu_chimu.c:149
CHIMU_Euler::psi
float psi
Definition: imu_chimu.h:120
CHIMU_PARSER_DATA::m_DeviceID
unsigned char m_DeviceID
Definition: imu_chimu.h:165
CHIMU_Msg_3_IMU_Attitude
#define CHIMU_Msg_3_IMU_Attitude
Definition: imu_chimu.h:74
CHIMU_Msg_6_MagCal
#define CHIMU_Msg_6_MagCal
Definition: imu_chimu.h:77
CHIMU_PARSER_DATA::m_MsgID
unsigned char m_MsgID
Definition: imu_chimu.h:162
GetEulersFromQuat
static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude)
Definition: imu_chimu.c:245
CHIMU_STATE_MACHINE_LEN
#define CHIMU_STATE_MACHINE_LEN
Definition: imu_chimu.c:95
CHIMU_Checksum
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
Definition: imu_chimu.c:82
CHIMU_Vector::y
float y
Definition: imu_chimu.h:125
CHIMU_PARSER_DATA::gCHIMU_SW_SerialNumber
uint16_t gCHIMU_SW_SerialNumber
Definition: imu_chimu.h:176
CHIMU_PARSER_DATA::m_sensor
CHIMU_sensor_data m_sensor
Definition: imu_chimu.h:170