diff --git a/conf/modules/ins_chimu_spi.xml b/conf/modules/ins_chimu_spi.xml index db4f90c226..3e4151fb65 100644 --- a/conf/modules/ins_chimu_spi.xml +++ b/conf/modules/ins_chimu_spi.xml @@ -4,9 +4,9 @@
+
- diff --git a/conf/modules/ins_chimu_uart.xml b/conf/modules/ins_chimu_uart.xml index f6c69cecbc..aa85a4caa1 100644 --- a/conf/modules/ins_chimu_uart.xml +++ b/conf/modules/ins_chimu_uart.xml @@ -10,6 +10,7 @@ For older CHIMU v1.0 you should define CHIMU_BIG_ENDIAN
+
diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index 8ee2f11cc7..4699a4f6d7 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -20,23 +20,23 @@ */ /*--------------------------------------------------------------------------- - Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. + Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. - File: *.c + File: *.c - Description: CHIMU Protocol Parser + Description: CHIMU Protocol Parser - Public Functions: - CHIMU_Init Create component instance - CHIMU_Parse Parse the RX byte stream message + Public Functions: + CHIMU_Init Create component instance + CHIMU_Parse Parse the RX byte stream message - Applicable Documents: - CHIMU User Manual + Applicable Documents: + CHIMU User Manual - Adapted to paparazzi by C. De Wagter + Adapted to paparazzi by C. De Wagter ----------------------------------------------------------------------------*/ + ---------------------------------------------------------------------------*/ #include "imu_chimu.h" #include "string.h" @@ -55,29 +55,25 @@ static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long // with no carries) unsigned char *CRC_input = (unsigned char*)data; - for (unsigned long j = data_len; j; --j) - { + for (unsigned long j = data_len; j; --j) { CRC_acc = CRC_acc ^ *CRC_input++; // "Divide" the poly into the dividend using CRC XOR subtraction // CRC_acc holds the "remainder" of each divide // // Only complete this division for 8 bits since input is 1 byte - for (i = 0; i < 8; i++) - { - // Check if the MSB is set (if MSB is 1, then the POLY can "divide" - // into the "dividend") - if ((CRC_acc & 0x00000001) == 0x00000001) - { - // if so, shift the CRC value, and XOR "subtract" the poly - CRC_acc = CRC_acc >> 1; - CRC_acc ^= POLY; - } - else - { - // if not, just shift the CRC value - CRC_acc = CRC_acc >> 1; - } + for (i = 0; i < 8; i++) { + // Check if the MSB is set (if MSB is 1, then the POLY can "divide" + // into the "dividend") + if ((CRC_acc & 0x00000001) == 0x00000001) { + // if so, shift the CRC value, and XOR "subtract" the poly + CRC_acc = CRC_acc >> 1; + CRC_acc ^= POLY; + } + else { + // if not, just shift the CRC value + CRC_acc = CRC_acc >> 1; + } } } // Return the final remainder (CRC value) @@ -95,8 +91,8 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen) */ // Lowlevel Protocol Decoding -#define CHIMU_STATE_MACHINE_START 0 -#define CHIMU_STATE_MACHINE_HEADER2 1 +#define CHIMU_STATE_MACHINE_START 0 +#define CHIMU_STATE_MACHINE_HEADER2 1 #define CHIMU_STATE_MACHINE_LEN 2 #define CHIMU_STATE_MACHINE_DEVICE 3 #define CHIMU_STATE_MACHINE_ID 4 @@ -107,9 +103,9 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen) #define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above /*--------------------------------------------------------------------------- - Name: CHIMU_Init + Name: CHIMU_Init ----------------------------------------------------------------------------*/ + ---------------------------------------------------------------------------*/ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) { unsigned char i; @@ -121,8 +117,7 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) //Sensor data holder pstData->m_sensor.cputemp=0.0; - for (i=0;i<3;i++) - { + for (i=0;i<3;i++) { pstData->m_sensor.acc[i]=0.0; pstData->m_sensor.rate[i]=0.0; pstData->m_sensor.mag[i]=0.0; @@ -137,8 +132,7 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) pstData->m_attrates.euler.theta = 0.0; pstData->m_attrates.euler.psi = 0.0; - for (i=0; im_Payload[i]= 0x00; pstData->m_FullMessage[i]= 0x00; } @@ -149,100 +143,95 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) } /*--------------------------------------------------------------------------- - Name: CHIMU_Parse - Abstract: Parse message, returns TRUE if new data. ----------------------------------------------------------------------------*/ + Name: CHIMU_Parse + Abstract: Parse message, returns TRUE if new data. + ---------------------------------------------------------------------------*/ unsigned char CHIMU_Parse( - unsigned char btData, /* input byte stream buffer */ - unsigned char bInputType, /* for future use if special builds of CHIMU data are performed */ - CHIMU_PARSER_DATA *pstData) /* resulting data */ + unsigned char btData, /* input byte stream buffer */ + unsigned char bInputType, /* for future use if special builds of CHIMU data are performed */ + CHIMU_PARSER_DATA *pstData) /* resulting data */ { - char bUpdate = FALSE; + char bUpdate = FALSE; - switch (pstData->m_State) { - case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE - if(btData == 0xAE) - { - pstData->m_State = CHIMU_STATE_MACHINE_HEADER2; - pstData->m_Index = 0; - pstData->m_FullMessage[pstData->m_Index++] = btData; - } else { - ;; - } - bUpdate = FALSE; - break; - case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE - if(btData == 0xAE) - { - pstData->m_State = CHIMU_STATE_MACHINE_LEN; - pstData->m_FullMessage[pstData->m_Index++]=btData; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; - } //Fail to see header. Restart. - break; - case CHIMU_STATE_MACHINE_LEN: // Get chars to read - if( btData <= CHIMU_RX_BUFFERSIZE) - { - pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_State = CHIMU_STATE_MACHINE_DEVICE; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } - break; - case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk - if( (btData == pstData->m_DeviceID) || (btData == 0xAA)) - { //0xAA is global message - pstData->m_TempDeviceID = btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_State = CHIMU_STATE_MACHINE_ID; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; - } //Fail to see correct device ID. Restart. - break; - case CHIMU_STATE_MACHINE_ID: // Get ID - pstData->m_MsgID = btData; // might be invalid, chgeck it out here: - if ( pstData->m_MsgID>CHIMU_COM_ID_HIGH) - { - pstData->m_State = CHIMU_STATE_MACHINE_START; - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } else { - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_PayloadIndex = 0; - pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go... - } - break; - case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload - pstData->m_Payload[pstData->m_PayloadIndex++]= btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) //Now we have the payload. Verify XSUM and then parse it next - { - pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF); - pstData->m_State = CHIMU_STATE_MACHINE_XSUM; - } else { - return FALSE; - } - break; - case CHIMU_STATE_MACHINE_XSUM: // Verify - pstData->m_ReceivedChecksum = btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) - { - bUpdate = FALSE; - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } else { - //Xsum passed, go parse it. - // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data. - bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData); - } - pstData->m_State = CHIMU_STATE_MACHINE_START; - break; - default: - pstData->m_State = CHIMU_STATE_MACHINE_START; - } /* End of SWITCH */ + switch (pstData->m_State) { + case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE + if(btData == 0xAE) { + pstData->m_State = CHIMU_STATE_MACHINE_HEADER2; + pstData->m_Index = 0; + pstData->m_FullMessage[pstData->m_Index++] = btData; + } else { + ;; + } + bUpdate = FALSE; + break; + case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE + if(btData == 0xAE) { + pstData->m_State = CHIMU_STATE_MACHINE_LEN; + pstData->m_FullMessage[pstData->m_Index++]=btData; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; + } //Fail to see header. Restart. + break; + case CHIMU_STATE_MACHINE_LEN: // Get chars to read + if( btData <= CHIMU_RX_BUFFERSIZE) { + pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size + pstData->m_FullMessage[pstData->m_Index++]=btData; + pstData->m_State = CHIMU_STATE_MACHINE_DEVICE; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } + break; + case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk + if( (btData == pstData->m_DeviceID) || (btData == 0xAA)) { + //0xAA is global message + pstData->m_TempDeviceID = btData; + pstData->m_FullMessage[pstData->m_Index++]=btData; + pstData->m_State = CHIMU_STATE_MACHINE_ID; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; + } //Fail to see correct device ID. Restart. + break; + case CHIMU_STATE_MACHINE_ID: // Get ID + pstData->m_MsgID = btData; // might be invalid, chgeck it out here: + if ( pstData->m_MsgID>CHIMU_COM_ID_HIGH) { + pstData->m_State = CHIMU_STATE_MACHINE_START; + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } else { + pstData->m_FullMessage[pstData->m_Index++]=btData; + pstData->m_PayloadIndex = 0; + pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go... + } + break; + case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload + pstData->m_Payload[pstData->m_PayloadIndex++]= btData; + pstData->m_FullMessage[pstData->m_Index++]=btData; + if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) { + //Now we have the payload. Verify XSUM and then parse it next + pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF); + pstData->m_State = CHIMU_STATE_MACHINE_XSUM; + } else { + return FALSE; + } + break; + case CHIMU_STATE_MACHINE_XSUM: // Verify + pstData->m_ReceivedChecksum = btData; + pstData->m_FullMessage[pstData->m_Index++]=btData; + if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) { + bUpdate = FALSE; + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } else { + //Xsum passed, go parse it. + // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data. + bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData); + } + pstData->m_State = CHIMU_STATE_MACHINE_START; + break; + default: + pstData->m_State = CHIMU_STATE_MACHINE_START; + } /* End of SWITCH */ return (bUpdate); } @@ -274,23 +263,20 @@ static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) if (x > 1.0) x = 1.0; if (x < -1.0) x = -1.0; // - if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5) - { - ps.euler.theta = 2 *atan2(ps.q.v.x, ps.q.s); - } - else - if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) - { - ps.euler.theta = -2 *atan2(ps.q.v.x, ps.q.s); - } - else{ - ps.euler.theta = asin(x); - } + if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5) { + ps.euler.theta = 2 *atan2(ps.q.v.x, ps.q.s); + } + else + if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) { + ps.euler.theta = -2 *atan2(ps.q.v.x, ps.q.s); + } + else{ + ps.euler.theta = asin(x); + } 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))); - if (ps.euler.psi < 0) - { - ps.euler.psi = ps.euler.psi + (2 * M_PI); - } + if (ps.euler.psi < 0) { + ps.euler.psi = ps.euler.psi + (2 * M_PI); + } return(ps); @@ -298,145 +284,143 @@ static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) static unsigned char BitTest (unsigned char input, unsigned char n) { -//Test a bit in n and return TRUE or FALSE - if ( input & (1 << n)) return TRUE; else return FALSE; + //Test a bit in n and return TRUE or FALSE + if ( input & (1 << n)) return TRUE; else return FALSE; } unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData) { - //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to - //CHIMU). + //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to + //CHIMU). - //Any CHIMU messages coming from the ground should be ignored, as that byte stream goes up to CHIMU - // by itself. However, here we should decode CHIMU messages being received and - // a) pass them down to ground - // b) grab the data from the CHIMU for our own needs / purposes - int CHIMU_index =0; - float sanity_check=0.0; + //Any CHIMU messages coming from the ground should be ignored, as that byte stream goes up to CHIMU + // by itself. However, here we should decode CHIMU messages being received and + // a) pass them down to ground + // b) grab the data from the CHIMU for our own needs / purposes + int CHIMU_index =0; + float sanity_check=0.0; - switch (pstData->m_MsgID){ - case CHIMU_Msg_0_Ping: - CHIMU_index = 0; - pstData->gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++; - pstData->gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++; - pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; - pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; - pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; + switch (pstData->m_MsgID){ + case CHIMU_Msg_0_Ping: + CHIMU_index = 0; + pstData->gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++; + pstData->gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++; + pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; + pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; + pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; - return TRUE; - break; - case CHIMU_Msg_1_IMU_Raw: - break; - case CHIMU_Msg_2_IMU_FP: - CHIMU_index = 0; - memmove (&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));CHIMU_index += (sizeof(pstData->m_sensor.cputemp)); - pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp); - memmove (&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));CHIMU_index += (sizeof(pstData->m_sensor.acc)); - pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]); - pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]); - pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]); - memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); - pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); - pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); - pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); - memmove (&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));CHIMU_index += (sizeof(pstData->m_sensor.mag)); - pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]); - pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]); - pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]); - memmove (&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));CHIMU_index += (sizeof(pstData->m_sensor.spare1)); - pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1); - return TRUE; - break; - case CHIMU_Msg_3_IMU_Attitude: - //Attitude message data from CHIMU - // Includes attitude and rates only, along with configuration bits - // All you need for control + return TRUE; + break; + case CHIMU_Msg_1_IMU_Raw: + break; + case CHIMU_Msg_2_IMU_FP: + CHIMU_index = 0; + memmove (&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));CHIMU_index += (sizeof(pstData->m_sensor.cputemp)); + pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp); + memmove (&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));CHIMU_index += (sizeof(pstData->m_sensor.acc)); + pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]); + pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]); + pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]); + memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); + pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); + pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); + pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); + memmove (&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));CHIMU_index += (sizeof(pstData->m_sensor.mag)); + pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]); + pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]); + pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]); + memmove (&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));CHIMU_index += (sizeof(pstData->m_sensor.spare1)); + pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1); + return TRUE; + break; + case CHIMU_Msg_3_IMU_Attitude: + //Attitude message data from CHIMU + // Includes attitude and rates only, along with configuration bits + // All you need for control - //Led_On(LED_RED); + //Led_On(LED_RED); - CHIMU_index = 0; - memmove (&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));CHIMU_index += sizeof(pstData->m_attitude.euler); - pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi); - pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta); - pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi); - memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate)); CHIMU_index += (sizeof(pstData->m_sensor.rate)); - pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); - pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); - pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); + CHIMU_index = 0; + memmove (&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));CHIMU_index += sizeof(pstData->m_attitude.euler); + pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi); + pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta); + pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi); + memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate)); CHIMU_index += (sizeof(pstData->m_sensor.rate)); + pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); + pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); + pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); - memmove (&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));CHIMU_index += sizeof(pstData->m_attitude.q); - pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s); - pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x); - pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y); - pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z); + memmove (&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));CHIMU_index += sizeof(pstData->m_attitude.q); + pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s); + pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x); + pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y); + pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z); - memmove (&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));CHIMU_index += sizeof(pstData->m_attitude.q); - pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s); - pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x); - pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y); - pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z); + memmove (&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));CHIMU_index += sizeof(pstData->m_attitude.q); + pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s); + pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x); + pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y); + pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z); - //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control - pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0]; - pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; - pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; + //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control + pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0]; + pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; + pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; - pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; - pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; - pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; + pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; + pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; + pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; -// TODO: Read configuration bits + // TODO: Read configuration bits -/* bC0_SPI_En = BitTest (gConfigInfo, 0); - bC1_HWCentrip_En = BitTest (gConfigInfo, 1); - bC2_TempCal_En = BitTest (gConfigInfo, 2); - bC3_RateOut_En = BitTest (gConfigInfo, 3); - bC4_TBD = BitTest (gConfigInfo, 4); - bC5_Quat_Est = BitTest (gConfigInfo, 5); - bC6_SWCentrip_En = BitTest (gConfigInfo, 6); - bC7_AllowHW_Override = BitTest (gConfigInfo, 7); -*/ - //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected - if(BitTest (pstData->gConfigInfo, 5) == TRUE) - { - pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude)); - } + /* bC0_SPI_En = BitTest (gConfigInfo, 0); + bC1_HWCentrip_En = BitTest (gConfigInfo, 1); + bC2_TempCal_En = BitTest (gConfigInfo, 2); + bC3_RateOut_En = BitTest (gConfigInfo, 3); + bC4_TBD = BitTest (gConfigInfo, 4); + bC5_Quat_Est = BitTest (gConfigInfo, 5); + bC6_SWCentrip_En = BitTest (gConfigInfo, 6); + bC7_AllowHW_Override = BitTest (gConfigInfo, 7); + */ + //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected + if(BitTest (pstData->gConfigInfo, 5) == TRUE) + { + pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude)); + } - //NEW: Checks for bad attitude data (bad SPI maybe?) - // Only allow globals to contain updated data if it makes sense - sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); - sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x); - sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y); - sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z); + //NEW: Checks for bad attitude data (bad SPI maybe?) + // Only allow globals to contain updated data if it makes sense + sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); + sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x); + sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y); + sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z); - if ((sanity_check > 0.8) && (sanity_check < 1.2)) //Should be 1.0 (normalized quaternion) - { -// gAttitude = pstData->m_attitude; -// gAttRates = pstData->m_attrates; -// gSensor = pstData->m_sensor; - } else - { - //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?) - } + //Should be 1.0 (normalized quaternion) + if ((sanity_check > 0.8) && (sanity_check < 1.2)) { + // gAttitude = pstData->m_attitude; + // gAttRates = pstData->m_attrates; + // gSensor = pstData->m_sensor; + } else { + //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?) + } - return TRUE; - break; - case CHIMU_Msg_4_BiasSF: - case CHIMU_Msg_5_BIT: - case CHIMU_Msg_6_MagCal: - case CHIMU_Msg_7_GyroBias: - case CHIMU_Msg_8_TempCal: - case CHIMU_Msg_9_DAC_Offsets: - case CHIMU_Msg_10_Res: - case CHIMU_Msg_11_Res: - case CHIMU_Msg_12_Res: - case CHIMU_Msg_13_Res: - case CHIMU_Msg_14_RefVector: - case CHIMU_Msg_15_SFCheck: - break; - default: - return FALSE; - break; - } - return FALSE; + return TRUE; + break; + case CHIMU_Msg_4_BiasSF: + case CHIMU_Msg_5_BIT: + case CHIMU_Msg_6_MagCal: + case CHIMU_Msg_7_GyroBias: + case CHIMU_Msg_8_TempCal: + case CHIMU_Msg_9_DAC_Offsets: + case CHIMU_Msg_10_Res: + case CHIMU_Msg_11_Res: + case CHIMU_Msg_12_Res: + case CHIMU_Msg_13_Res: + case CHIMU_Msg_14_RefVector: + case CHIMU_Msg_15_SFCheck: + break; + default: + return FALSE; + break; + } + return FALSE; } - diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index d246bf6a6f..328a2c1e68 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -20,23 +20,23 @@ */ /*--------------------------------------------------------------------------- - Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. + Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. - File: *.c + File: *.c - Description: CHIMU Protocol Parser + Description: CHIMU Protocol Parser - Public Functions: - CHIMU_Init Create component instance - CHIMU_Done Free component instance - CHIMU_Parse Parse the RX byte stream message + Public Functions: + CHIMU_Init Create component instance + CHIMU_Done Free component instance + CHIMU_Parse Parse the RX byte stream message - Applicable Documents: - CHIMU parsing documentation + Applicable Documents: + CHIMU parsing documentation ----------------------------------------------------------------------------*/ + ---------------------------------------------------------------------------*/ #include "paparazzi.h" @@ -116,25 +116,25 @@ static inline float FloatSwap( float f ) typedef struct { - float phi; - float theta; - float psi; + float phi; + float theta; + float psi; } CHIMU_Euler; typedef struct { - float x; - float y; - float z; + float x; + float y; + float z; } CHIMU_Vector; typedef struct { - float s; - CHIMU_Vector v; + float s; + CHIMU_Vector v; } CHIMU_Quaternion; typedef struct { - CHIMU_Euler euler; - CHIMU_Quaternion q; + CHIMU_Euler euler; + CHIMU_Quaternion q; } CHIMU_attitude_data; #ifndef FALSE @@ -145,53 +145,53 @@ typedef struct { #endif typedef struct { - float cputemp; - float acc[3]; - float rate[3]; - float mag[3]; - float spare1; + float cputemp; + float acc[3]; + float rate[3]; + float mag[3]; + float spare1; } CHIMU_sensor_data; #define CHIMU_RX_BUFFERSIZE 128 typedef struct { - unsigned char m_State; // Current state protocol parser is in - unsigned char m_Checksum; // Calculated CHIMU sentence checksum - unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) - unsigned char m_Index; // Index used for command and data - unsigned char m_PayloadIndex; - unsigned char m_MsgID; - unsigned char m_MsgLen; - unsigned char m_TempDeviceID; - unsigned char m_DeviceID; - unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data - unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data - CHIMU_attitude_data m_attitude; - CHIMU_attitude_data m_attrates; - CHIMU_sensor_data m_sensor; + unsigned char m_State; // Current state protocol parser is in + unsigned char m_Checksum; // Calculated CHIMU sentence checksum + unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) + unsigned char m_Index; // Index used for command and data + unsigned char m_PayloadIndex; + unsigned char m_MsgID; + unsigned char m_MsgLen; + unsigned char m_TempDeviceID; + unsigned char m_DeviceID; + unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data + unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data + CHIMU_attitude_data m_attitude; + CHIMU_attitude_data m_attrates; + CHIMU_sensor_data m_sensor; - // Ping data - uint8_t gCHIMU_SW_Exclaim; - uint8_t gCHIMU_SW_Major; - uint8_t gCHIMU_SW_Minor; - uint16_t gCHIMU_SW_SerialNumber; + // Ping data + uint8_t gCHIMU_SW_Exclaim; + uint8_t gCHIMU_SW_Major; + uint8_t gCHIMU_SW_Minor; + uint16_t gCHIMU_SW_SerialNumber; - // Config - uint8_t gCalStatus; - uint8_t gCHIMU_BIT; - uint8_t gConfigInfo; + // Config + uint8_t gCalStatus; + uint8_t gCHIMU_BIT; + uint8_t gConfigInfo; } CHIMU_PARSER_DATA; /*--------------------------------------------------------------------------- - Name: CHIMU_Init ----------------------------------------------------------------------------*/ + Name: CHIMU_Init + ---------------------------------------------------------------------------*/ void CHIMU_Init(CHIMU_PARSER_DATA *pstData); /*--------------------------------------------------------------------------- - Name: CHIMU_Parse - Abstract: Parse message input test mode, returns TRUE if new data. ----------------------------------------------------------------------------*/ + Name: CHIMU_Parse + Abstract: Parse message input test mode, returns TRUE if new data. + ---------------------------------------------------------------------------*/ unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData); unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData); @@ -199,4 +199,3 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa void CHIMU_Checksum(unsigned char *data, unsigned char buflen); #endif // CHIMU_DEFINED - diff --git a/sw/airborne/modules/ins/ins_chimu.h b/sw/airborne/modules/ins/ins_chimu.h new file mode 100644 index 0000000000..a612e28f02 --- /dev/null +++ b/sw/airborne/modules/ins/ins_chimu.h @@ -0,0 +1,6 @@ +#ifndef INS_CHIMU_H +#define INS_CHIMU_H + +extern void ahrs_update_gps( void ); + +#endif diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c index 01d441fcab..4ff59a66b4 100644 --- a/sw/airborne/modules/ins/ins_chimu_spi.c +++ b/sw/airborne/modules/ins/ins_chimu_spi.c @@ -1,5 +1,5 @@ /* -C code to connect a CHIMU using uart + C code to connect a CHIMU using uart */ @@ -71,28 +71,23 @@ void ins_init( void ) void parse_ins_msg( void ) { - while (InsLink(ChAvailable())) - { + while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); - if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) - { - RunOnceEvery(25, LED_TOGGLE(3) ); - if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) - { - new_ins_attitude = 1; - if (CHIMU_DATA.m_attitude.euler.phi > M_PI) - { - CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; - } + if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { + RunOnceEvery(25, LED_TOGGLE(3) ); + if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) { + new_ins_attitude = 1; + if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { + CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; + } - EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); - EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r + EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); + EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r } - else if(CHIMU_DATA.m_MsgID==0x02) - { + else if(CHIMU_DATA.m_MsgID==0x02) { - RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); + RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); } } @@ -100,16 +95,14 @@ void parse_ins_msg( void ) } -//Frequency defined in conf *.xml -void ins_periodic_task( void ) +void ahrs_update_gps( void ) { // Send SW Centripetal Corrections uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 }; float gps_speed = 0; - if (gps.fix == GPS_FIX_3D) - { + if (gps.fix == GPS_FIX_3D) { gps_speed = gps.speed_3d/100.; } gps_speed = FloatSwap(gps_speed); @@ -123,3 +116,9 @@ void ins_periodic_task( void ) // Downlink Send } + +//Frequency defined in conf *.xml +void ins_periodic_task( void ) +{ +} + diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c index 44cc695f7b..17be49bce8 100644 --- a/sw/airborne/modules/ins/ins_chimu_uart.c +++ b/sw/airborne/modules/ins/ins_chimu_uart.c @@ -1,5 +1,5 @@ /* -C code to connect a CHIMU using uart + C code to connect a CHIMU using uart */ @@ -39,8 +39,8 @@ void ins_init( void ) uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI -// uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI -// uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI + // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI + // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI new_ins_attitude = 0; @@ -51,15 +51,13 @@ void ins_init( void ) CHIMU_Init(&CHIMU_DATA); // Request Software version - for (int i=0;i<7;i++) - { + for (int i=0;i<7;i++) { InsUartSend1(ping[i]); } // Quat Filter - for (int i=0;i<7;i++) - { + for (int i=0;i<7;i++) { InsUartSend1(quaternions[i]); } @@ -72,25 +70,21 @@ void ins_init( void ) void parse_ins_msg( void ) { - while (InsLink(ChAvailable())) - { + while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); - if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) - { - if(CHIMU_DATA.m_MsgID==0x03) - { - new_ins_attitude = 1; - RunOnceEvery(25, LED_TOGGLE(3) ); - if (CHIMU_DATA.m_attitude.euler.phi > M_PI) - { - CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; - } + if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { + if(CHIMU_DATA.m_MsgID==0x03) { + new_ins_attitude = 1; + RunOnceEvery(25, LED_TOGGLE(3) ); + if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { + CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; + } - EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); - //EstimatorSetRate(ins_p,ins_q,ins_r); + EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); + //EstimatorSetRate(ins_p,ins_q,ins_r); - DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); + DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); } } @@ -104,3 +98,7 @@ void ins_periodic_task( void ) // Downlink Send } +void ahrs_update_gps( void ) +{ + +}