diff --git a/conf/airframes/AirborneCodeReorg/ChimuTinyFw.xml b/conf/airframes/AirborneCodeReorg/ChimuTinyFw.xml index a94b275a4d..cf6192a7d0 100644 --- a/conf/airframes/AirborneCodeReorg/ChimuTinyFw.xml +++ b/conf/airframes/AirborneCodeReorg/ChimuTinyFw.xml @@ -46,8 +46,6 @@ - - @@ -161,7 +159,7 @@ - + diff --git a/conf/modules/ins_chimu.xml b/conf/modules/ins_chimu_uart.xml similarity index 94% rename from conf/modules/ins_chimu.xml rename to conf/modules/ins_chimu_uart.xml index d40ee88e72..bd646d7167 100644 --- a/conf/modules/ins_chimu.xml +++ b/conf/modules/ins_chimu_uart.xml @@ -16,6 +16,5 @@ - diff --git a/sw/airborne/modules/gps_i2c/gps_i2c.c b/sw/airborne/modules/gps_i2c/gps_i2c.c index 8e0dbc7efd..591b36e3c2 100644 --- a/sw/airborne/modules/gps_i2c/gps_i2c.c +++ b/sw/airborne/modules/gps_i2c/gps_i2c.c @@ -49,8 +49,8 @@ bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit; } static uint8_t gps_i2c_status; -static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */ -static uint8_t data_buf_len; +//static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */ +//static uint8_t data_buf_len; void gps_i2c_init(void) { gps_i2c_status = GPS_I2C_STATUS_IDLE; diff --git a/sw/airborne/modules/ins/endian_functions.c b/sw/airborne/modules/ins/endian_functions.c deleted file mode 100644 index 8f6ffd9715..0000000000 --- a/sw/airborne/modules/ins/endian_functions.c +++ /dev/null @@ -1,74 +0,0 @@ -/********************************************************************************************** -* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved -* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION -* --------------------------------------------------------------------------------------------- -* -* Module: -* Endian Functions - Handles various low level endian swap functions -* -***********************************************************************************************/ -//----------------------------------------------------------------------------------- -// Includes -//----------------------------------------------------------------------------------- -#include "endian_functions.h" -//#include - - -short int ShortSwap( short int s ) -{ - unsigned char b1, b2; - - b1 = s & 255; - b2 = (s >> 8) & 255; - - return (b1 << 8) + b2; -} - -short int ShortNoSwap( short int s ) -{ - return s; -} - -int LongSwap (int i) -{ - unsigned char b1, b2, b3, b4; - - b1 = i & 255; - b2 = ( i >> 8 ) & 255; - b3 = ( i>>16 ) & 255; - b4 = ( i>>24 ) & 255; - - return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4; -} - -int LongNoSwap( int i ) -{ - return i; -} - -float FloatSwap( float f ) -{ - union - { - float f; - unsigned char b[4]; - } dat1, dat2; - - dat1.f = f; - dat2.b[0] = dat1.b[3]; - dat2.b[1] = dat1.b[2]; - dat2.b[2] = dat1.b[1]; - dat2.b[3] = dat1.b[0]; - return dat2.f; -} - -float FloatNoSwap( float f ) -{ - return f; -} - - - - - - diff --git a/sw/airborne/modules/ins/endian_functions.h b/sw/airborne/modules/ins/endian_functions.h deleted file mode 100644 index 924d4b2235..0000000000 --- a/sw/airborne/modules/ins/endian_functions.h +++ /dev/null @@ -1,26 +0,0 @@ -/********************************************************************************************** -* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved -* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION -* --------------------------------------------------------------------------------------------- -* -* Module: -* Endian Functions - Handles various low level endian swap functions -* -***********************************************************************************************/ -#ifndef ENDIAN_H -#define ENDIAN_H - - -short int ShortSwap( short int s ); - -short int ShortNoSwap( short int s ); - -int LongSwap (int i); - -int LongNoSwap( int i ); - -float FloatSwap( float f ); - -float FloatNoSwap( float f ); - -#endif diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index b131539f0b..6a0030ebed 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -29,7 +29,6 @@ Public Functions: CHIMU_Init Create component instance - CHIMU_Done Free component instance CHIMU_Parse Parse the RX byte stream message Applicable Documents: @@ -41,14 +40,75 @@ #include "imu_chimu.h" #include "string.h" -//#include "crc.h" -#include "endian_functions.h" -//#include "util.h" #include "math.h" +/*************************************************************************** + * Endianness Swapping Functions + */ -//---[Defines]------------------------------------------------------ +static float FloatSwap( float f ) +{ + union + { + float f; + unsigned char b[4]; + } dat1, dat2; + + dat1.f = f; + dat2.b[0] = dat1.b[3]; + dat2.b[1] = dat1.b[2]; + dat2.b[2] = dat1.b[1]; + dat2.b[3] = dat1.b[0]; + return dat2.f; +} + +/*************************************************************************** + * Cyclic Redundancy Checksum + */ + +static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long data_len) +{ + unsigned long i; // loop counter +#define POLY 0xEDB88320 // bit-reversed version of the poly 0x04C11DB7 + // Create the CRC "dividend" for polynomial arithmetic (binary arithmetic + // with no carries) + + unsigned char *CRC_input = (unsigned char*)data; + 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; + } + } + } + // Return the final remainder (CRC value) + return CRC_acc; +} + +/*************************************************************************** + * CHIMU Protocol Definition + */ + +// Lowlevel Protocol Decoding #define CHIMU_STATE_MACHINE_START 0 #define CHIMU_STATE_MACHINE_HEADER2 1 #define CHIMU_STATE_MACHINE_LEN 2 @@ -57,9 +117,7 @@ #define CHIMU_STATE_MACHINE_PAYLOAD 5 #define CHIMU_STATE_MACHINE_XSUM 6 - -//---[DEFINES for Message List]--------------------------------------- -//Message ID's that go TO the CHIMU +// Message ID's that go TO the CHIMU #define MSG00_PING 0x00 #define MSG01_BIAS 0x01 #define MSG02_DACMODE 0x02 @@ -79,8 +137,7 @@ #define MSG10_UARTSETTINGS 0x10 #define MSG11_SERIALNUMBER 0x11 -//Output message identifiers from the CHIMU unit -//---[Defines]------------------------------------------------------ +// Output message identifiers from the CHIMU unit #define CHIMU_Msg_0_Ping 0 #define CHIMU_Msg_1_IMU_Raw 1 #define CHIMU_Msg_2_IMU_FP 2 @@ -98,20 +155,9 @@ #define CHIMU_Msg_14_RefVector 14 #define CHIMU_Msg_15_SFCheck 15 - -//---[COM] defines -#define CHIMU_COM_ID_LOW 0x00 +// Communication Definitions #define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above -#define NP_MAX_CMD_LEN 8 // maximum command length (CHIMU address) -#define NP_MAX_DATA_LEN 256 // maximum data length -#define NP_MAX_CHAN 36 // maximum number of channels -#define NP_WAYPOINT_ID_LEN 32 // waypoint max string len -#define NP_XSUM_LEN 3 // chars in checksum string - -#define CHIMU_STANDARD 0x00 - - /*--------------------------------------------------------------------------- Name: CHIMU_Init @@ -155,11 +201,9 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) pstData->m_DeviceID = 0x01; //look at this later } - /*--------------------------------------------------------------------------- Name: CHIMU_Parse Abstract: Parse message, returns TRUE if new data. - Note: A typical sentence is constructed as: ---------------------------------------------------------------------------*/ unsigned char CHIMU_Parse( @@ -168,7 +212,6 @@ unsigned char CHIMU_Parse( CHIMU_PARSER_DATA *pstData) /* resulting data */ { - //long int i; char bUpdate = FALSE; switch (pstData->m_State) { @@ -215,7 +258,7 @@ unsigned char CHIMU_Parse( break; case CHIMU_STATE_MACHINE_ID: // Get ID pstData->m_MsgID = btData; // might be invalid, chgeck it out here: - if ( (pstData->m_MsgIDm_MsgID>CHIMU_COM_ID_HIGH)) + if ( pstData->m_MsgID>CHIMU_COM_ID_HIGH) { pstData->m_State = CHIMU_STATE_MACHINE_START; //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); @@ -230,8 +273,7 @@ unsigned char CHIMU_Parse( 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 { -// TODO Redo Checksum -// pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF); + 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; @@ -240,8 +282,7 @@ unsigned char CHIMU_Parse( case CHIMU_STATE_MACHINE_XSUM: // Verify pstData->m_ReceivedChecksum = btData; pstData->m_FullMessage[pstData->m_Index++]=btData; - // if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) - if (FALSE) + if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) { bUpdate = FALSE; //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c index 6a595d473a..5f397de6b3 100644 --- a/sw/airborne/modules/ins/ins_chimu_uart.c +++ b/sw/airborne/modules/ins/ins_chimu_uart.c @@ -49,7 +49,7 @@ void parse_ins_msg( void ) { if(CHIMU_DATA.m_MsgID==0x03) { - RunOnceEvery(25, LED_TOGGLE(3) ); + //RunOnceEvery(25, LED_TOGGLE(3) ); if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;