diff --git a/conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml b/conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml
new file mode 100644
index 0000000000..af5645ce0f
--- /dev/null
+++ b/conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml
@@ -0,0 +1,184 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml
index ae607c28b4..d9e489ef66 100644
--- a/conf/airframes/TU_Delft/yapa_xsens.xml
+++ b/conf/airframes/TU_Delft/yapa_xsens.xml
@@ -7,85 +7,85 @@
-
-
-
+
+
+
+
+
-
-
-
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -148,18 +148,16 @@
-
-
-
-
-
-
+
@@ -169,22 +167,29 @@
-
-
-
+
--->
+
+
-
+
+
+
+
@@ -193,17 +198,16 @@
-
+
-
-
-
+
+
-
+
diff --git a/conf/modules/ins_chimu.xml b/conf/modules/ins_chimu.xml
new file mode 100644
index 0000000000..a7099512a6
--- /dev/null
+++ b/conf/modules/ins_chimu.xml
@@ -0,0 +1,48 @@
+# attitude via IR sensors
+
+#
+# CHIMU using UART
+#
+
+ap.CFLAGS += -DUSE_$(CHIMU_PORT)
+ap.CFLAGS += -D$(CHIMU_PORT)_BAUD=B115200
+
+#
+# CHIMU Status LED
+#
+
+ifneq ($(GPS_LED),none)
+ ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_chimu.c
+
+
+
+#
+# LPC only has one ADC
+#
+ifeq ($(ARCH), lpc21)
+ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1) -DUSE_$(ADC_IR1)
+ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2) -DUSE_$(ADC_IR2)
+ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP) -DUSE_$(ADC_IR_TOP)
+endif
+
+#
+# On STM32 let's hardwire infrared sensors to AD1 for now
+#
+ifeq ($(ARCH), stm32)
+ap.CFLAGS += -DUSE_AD1
+ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1_CHAN) -DUSE_AD1_$(ADC_IR1)
+ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2_CHAN) -DUSE_AD1_$(ADC_IR2)
+ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP_CHAN) -DUSE_AD1_$(ADC_IR_TOP)
+endif
+
+ap.CFLAGS += -DADC_CHANNEL_IR_NB_SAMPLES=$(ADC_IR_NB_SAMPLES)
+
+$(TARGET).CFLAGS += -DUSE_INFRARED
+$(TARGET).srcs += subsystems/sensors/infrared.c
+$(TARGET).srcs += subsystems/sensors/infrared_adc.c
+
+sim.srcs += $(SRC_ARCH)/sim_ir.c
+jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c
diff --git a/conf/radios/R617FS_5ch_neg.xml b/conf/radios/R617FS_5ch_neg.xml
new file mode 100644
index 0000000000..c368ca499b
--- /dev/null
+++ b/conf/radios/R617FS_5ch_neg.xml
@@ -0,0 +1,52 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/ins/endian_functions.c b/sw/airborne/modules/ins/endian_functions.c
new file mode 100644
index 0000000000..8b81eee19d
--- /dev/null
+++ b/sw/airborne/modules/ins/endian_functions.c
@@ -0,0 +1,74 @@
+/**********************************************************************************************
+* 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
new file mode 100644
index 0000000000..03ab794571
--- /dev/null
+++ b/sw/airborne/modules/ins/endian_functions.h
@@ -0,0 +1,26 @@
+/**********************************************************************************************
+* 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
new file mode 100644
index 0000000000..f2edc5a3c7
--- /dev/null
+++ b/sw/airborne/modules/ins/imu_chimu.c
@@ -0,0 +1,450 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*---------------------------------------------------------------------------
+ Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
+
+ File: *.c
+
+ Description: CHIMU Protocol Parser
+
+
+ Public Functions:
+ CHIMU_Init Create component instance
+ CHIMU_Done Free component instance
+ CHIMU_Parse Parse the RX byte stream message
+
+ Applicable Documents:
+ CHIMU User Manual
+
+ Adapted to paparazzi by C. De Wagter
+
+---------------------------------------------------------------------------*/
+
+#include "imu_chimu.h"
+#include "string.h"
+//#include "crc.h"
+#include "endian_functions.h"
+//#include "util.h"
+#include "math.h"
+
+
+
+//---[Defines]------------------------------------------------------
+#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
+#define CHIMU_STATE_MACHINE_PAYLOAD 5
+#define CHIMU_STATE_MACHINE_XSUM 6
+
+
+//---[DEFINES for Message List]---------------------------------------
+//Message ID's that go TO the CHIMU
+#define MSG00_PING 0x00
+#define MSG01_BIAS 0x01
+#define MSG02_DACMODE 0x02
+#define MSG03_CALACC 0x03
+#define MSG04_CALMAG 0x04
+#define MSG05_CALRATE 0x05
+#define MSG06_CONFIGCLR 0x06
+#define MSG07_CONFIGSET 0x07
+#define MSG08_SAVEGYROBIAS 0x08
+#define MSG09_ESTIMATOR 0x09
+#define MSG0A_SFCHECK 0x0A
+#define MSG0B_CENTRIP 0x0B
+#define MSG0C_INITGYROS 0x0C
+#define MSG0D_DEVICEID 0x0D
+#define MSG0E_REFVECTOR 0x0E
+#define MSG0F_RESET 0x0F
+#define MSG10_UARTSETTINGS 0x10
+#define MSG11_SERIALNUMBER 0x11
+
+//Output message identifiers from the CHIMU unit
+//---[Defines]------------------------------------------------------
+#define CHIMU_Msg_0_Ping 0
+#define CHIMU_Msg_1_IMU_Raw 1
+#define CHIMU_Msg_2_IMU_FP 2
+#define CHIMU_Msg_3_IMU_Attitude 3
+#define CHIMU_Msg_4_BiasSF 4
+#define CHIMU_Msg_5_BIT 5
+#define CHIMU_Msg_6_MagCal 6
+#define CHIMU_Msg_7_GyroBias 7
+#define CHIMU_Msg_8_TempCal 8
+#define CHIMU_Msg_9_DAC_Offsets 9
+#define CHIMU_Msg_10_Res 10
+#define CHIMU_Msg_11_Res 11
+#define CHIMU_Msg_12_Res 12
+#define CHIMU_Msg_13_Res 13
+#define CHIMU_Msg_14_RefVector 14
+#define CHIMU_Msg_15_SFCheck 15
+
+
+//---[COM] defines
+#define CHIMU_COM_ID_LOW 0x00
+#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
+
+---------------------------------------------------------------------------*/
+void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
+{
+ unsigned char i;
+ pstData->m_State = CHIMU_STATE_MACHINE_START;
+ pstData->m_Checksum = 0x00;
+ pstData->m_ReceivedChecksum = 0x00;
+ pstData->m_Index = 0;
+ pstData->m_PayloadIndex = 0;
+
+ //Sensor data holder
+ pstData->m_sensor.cputemp=0.0;
+ 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;
+ }
+ pstData->m_sensor.spare1=0.0;
+ //Attitude data
+ pstData->m_attitude.euler.phi = 0.0;
+ pstData->m_attitude.euler.theta = 0.0;
+ pstData->m_attitude.euler.psi = 0.0;
+ //Attitude rate data
+ pstData->m_attrates.euler.phi = 0.0;
+ 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;
+ }
+ pstData->m_MsgLen = 0;
+ pstData->m_MsgID = 0;
+ pstData->m_TempDeviceID =0;
+ 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(
+ 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 */
+{
+
+ //long int i;
+ 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_MsgIDm_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
+ {
+// TODO Redo Checksum
+// 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);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+// Process CHIMU sentence - Use the CHIMU address (*pCommand) and call the
+// appropriate sentence data processor.
+///////////////////////////////////////////////////////////////////////////////
+
+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).
+
+ //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;
+ gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++;
+ gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++;
+ gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++;
+ gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++;
+ 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
+
+ //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]);
+
+ 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);
+
+ //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];
+
+/*
+ // TODO: Read configuration bits
+
+ gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
+ gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
+
+ gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
+ 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(bC5_Quat_Est == 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);
+
+ 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?)
+ }
+
+ //Led_Off(LED_RED);
+
+ 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;
+ }
+}
+
+CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude)
+{
+ CHIMU_attitude_data ps;
+ ps = attitude;
+ float x, sqw,sqx,sqy,sqz,norm;
+ sqw = ps.q.s * ps.q.s;
+ sqx = ps.q.v.x * ps.q.v.x;
+ sqy = ps.q.v.y * ps.q.v.y;
+ sqz = ps.q.v.z * ps.q.v.z;
+ norm = sqrt(sqw + sqx + sqy + sqz);
+ //Normalize the quat
+ ps.q.s = ps.q.s / norm;
+ ps.q.v.x = ps.q.v.x / norm;
+ ps.q.v.y = ps.q.v.y / norm;
+ ps.q.v.z = ps.q.v.z / norm;
+ 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)));
+ if (ps.euler.phi < 0) ps.euler.phi = ps.euler.phi + 2 *PI;
+ x = ((2.0 * (ps.q.s * ps.q.v.y - ps.q.v.z * ps.q.v.x)));
+ //Below needed in event normalization not done
+ 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);
+ }
+ 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 * PI);
+ }
+
+ return(ps);
+
+}
+
diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h
new file mode 100644
index 0000000000..a19fb2c3fc
--- /dev/null
+++ b/sw/airborne/modules/ins/imu_chimu.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2011 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*---------------------------------------------------------------------------
+ Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
+
+ File: *.c
+
+ Description: CHIMU Protocol Parser
+
+
+ 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
+
+
+---------------------------------------------------------------------------*/
+
+#include "paparazzi.h"
+
+//---[Defines]------------------------------------------------------
+#ifndef CHIMU_DEFINED_H
+#define CHIMU_DEFINED_H
+
+typedef struct {
+ float phi;
+ float theta;
+ float psi;
+} CHIMU_Euler;
+
+typedef struct {
+ float x;
+ float y;
+ float z;
+} CHIMU_Vector;
+
+typedef struct {
+ float s;
+ CHIMU_Vector v;
+} CHIMU_Quaternion;
+
+typedef struct {
+ CHIMU_Euler euler;
+ CHIMU_Quaternion q;
+} CHIMU_attitude_data;
+
+#define FALSE (1==0)
+#define TRUE (1==1)
+
+typedef struct {
+ int cputemp;
+ int acc[3];
+ int rate[3];
+ int mag[3];
+ int spare1;
+ int euler[3];
+} CHIMU_sensor_data;
+
+extern uint8_t gCHIMU_SW_Exclaim;
+extern char gCHIMU_SW_Major;
+extern char gCHIMU_SW_Minor;
+extern uint16_t gCHIMU_SW_SerialNumber;
+
+#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;
+
+} CHIMU_PARSER_DATA;
+
+/*---------------------------------------------------------------------------
+ Name: CHIMU_Init
+---------------------------------------------------------------------------*/
+void CHIMU_Init(CHIMU_PARSER_DATA *pstData);
+
+/*---------------------------------------------------------------------------
+ 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);
+
+CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude);
+
+
+#endif // CHIMU_DEFINED
+