50 static unsigned long UpdateCRC(
unsigned long CRC_acc,
void *data,
unsigned long data_len)
53 #define POLY 0xEDB88320
57 unsigned char *CRC_input = (
unsigned char *)data;
58 for (
unsigned long j = data_len; j; --j) {
60 CRC_acc = CRC_acc ^ *CRC_input++;
65 for (i = 0; i < 8; i++) {
68 if ((CRC_acc & 0x00000001) == 0x00000001) {
70 CRC_acc = CRC_acc >> 1;
74 CRC_acc = CRC_acc >> 1;
84 data[buflen - 1] = (
unsigned char)(
UpdateCRC(0xFFFFFFFF , data , (
unsigned long)(buflen - 1)) & 0xff) ;
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
102 #define CHIMU_COM_ID_HIGH 0x1F
119 for (i = 0; i < 3; i++) {
150 unsigned char btData,
151 unsigned char bInputType __attribute__((
156 unsigned char bUpdate = 0;
160 if (btData == 0xAE) {
170 if (btData == 0xAE) {
188 if ((btData == pstData->
m_DeviceID) || (btData == 0xAA)) {
214 (
unsigned long)(pstData->
m_MsgLen) + 5)) & 0xFF);
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);
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)));
262 x = ((2.0 * (ps.
q.
s * ps.
q.
v.
y - ps.
q.
v.
z * ps.
q.
v.
x)));
264 if (x > 1.0) { x = 1.0; }
265 if (x < -1.0) { x = -1.0; }
267 if ((ps.
q.
v.
x * ps.
q.
v.
y + ps.
q.
v.
z * ps.
q.
s) == 0.5) {
269 }
else if ((ps.
q.
v.
x * ps.
q.
v.
y + ps.
q.
v.
z * ps.
q.
s) == -0.5) {
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)));
283 static unsigned char BitTest(
unsigned char input,
unsigned char n)
286 if (input & (1 << n)) {
return 1; }
else {
return 0; }
299 float sanity_check = 0.0;
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 ++;
405 if ((sanity_check > 0.8) && (sanity_check < 1.2)) {
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData)
static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long data_len)
#define CHIMU_STATE_MACHINE_HEADER2
#define CHIMU_COM_ID_HIGH
#define CHIMU_STATE_MACHINE_ID
void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData)
void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
#define CHIMU_STATE_MACHINE_XSUM
#define CHIMU_STATE_MACHINE_START
static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude)
#define CHIMU_STATE_MACHINE_LEN
#define CHIMU_STATE_MACHINE_PAYLOAD
static unsigned char BitTest(unsigned char input, unsigned char n)
#define CHIMU_STATE_MACHINE_DEVICE
CHIMU_attitude_data m_attitude
#define CHIMU_Msg_14_RefVector
#define CHIMU_Msg_1_IMU_Raw
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]
CHIMU_attitude_data m_attrates
#define CHIMU_Msg_6_MagCal
#define CHIMU_Msg_8_TempCal
CHIMU_sensor_data m_sensor
#define CHIMU_Msg_9_DAC_Offsets
unsigned char m_ReceivedChecksum
#define CHIMU_RX_BUFFERSIZE
#define CHIMU_Msg_3_IMU_Attitude
#define CHIMU_Msg_4_BiasSF
#define CHIMU_Msg_15_SFCheck
unsigned char m_TempDeviceID
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]
unsigned char m_PayloadIndex
#define CHIMU_Msg_2_IMU_FP
uint8_t gCHIMU_SW_Exclaim
#define CHIMU_Msg_7_GyroBias
uint16_t gCHIMU_SW_SerialNumber