diff --git a/conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml b/conf/airframes/CDW/ChimuLisaFw.xml
similarity index 90%
rename from conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml
rename to conf/airframes/CDW/ChimuLisaFw.xml
index af5645ce0f..8a6337cb4f 100644
--- a/conf/airframes/AirborneCodeReorg/ChimuLisaFw.xml
+++ b/conf/airframes/CDW/ChimuLisaFw.xml
@@ -16,9 +16,6 @@
-
-
-
@@ -49,6 +46,10 @@
+
+
+
+
@@ -94,20 +95,7 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/conf/airframes/CDW/ChimuTinyFw.xml b/conf/airframes/CDW/ChimuTinyFw.xml
new file mode 100644
index 0000000000..1e7249bf0a
--- /dev/null
+++ b/conf/airframes/CDW/ChimuTinyFw.xml
@@ -0,0 +1,198 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml
new file mode 100644
index 0000000000..a1fc625fec
--- /dev/null
+++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml
@@ -0,0 +1,192 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/AirborneCodeReorg/LisaFw.xml b/conf/airframes/CDW/LisaFw.xml
similarity index 100%
rename from conf/airframes/AirborneCodeReorg/LisaFw.xml
rename to conf/airframes/CDW/LisaFw.xml
diff --git a/conf/airframes/AirborneCodeReorg/TinyFw.xml b/conf/airframes/CDW/TinyFw.xml
similarity index 100%
rename from conf/airframes/AirborneCodeReorg/TinyFw.xml
rename to conf/airframes/CDW/TinyFw.xml
diff --git a/conf/autopilot/subsystems/fixedwing/spi_slave_hs.makefile b/conf/autopilot/subsystems/fixedwing/spi_slave_hs.makefile
new file mode 100644
index 0000000000..f9f6826f98
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/spi_slave_hs.makefile
@@ -0,0 +1,10 @@
+#generic spi driver
+$(TARGET).CFLAGS += -DUSE_SPI
+
+
+ifeq ($(ARCH), lpc21)
+$(TARGET).CFLAGS += -DSSP_VIC_SLOT=9
+else ifeq ($(ARCH), stm32)
+endif
+
+$(TARGET).srcs += mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_slave_hs_arch.c
diff --git a/conf/modules/gps_i2c.xml b/conf/modules/gps_i2c.xml
index dabbd3176c..2b012777c9 100644
--- a/conf/modules/gps_i2c.xml
+++ b/conf/modules/gps_i2c.xml
@@ -14,6 +14,8 @@ ap.CFLAGS += -DGPS_CONFIGURE -DGPS_PORT_ID=GPS_PORT_DDC
+
+
diff --git a/conf/modules/ins_chimu.xml b/conf/modules/ins_chimu.xml
deleted file mode 100644
index a7099512a6..0000000000
--- a/conf/modules/ins_chimu.xml
+++ /dev/null
@@ -1,48 +0,0 @@
-# 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/modules/ins_chimu_spi.xml b/conf/modules/ins_chimu_spi.xml
new file mode 100644
index 0000000000..8efbbfec24
--- /dev/null
+++ b/conf/modules/ins_chimu_spi.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ins_chimu_uart.xml b/conf/modules/ins_chimu_uart.xml
new file mode 100644
index 0000000000..e7ded2d0df
--- /dev/null
+++ b/conf/modules/ins_chimu_uart.xml
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
new file mode 100644
index 0000000000..21f112fb18
--- /dev/null
+++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
@@ -0,0 +1,194 @@
+/*
+ * $Id$
+ *
+ * 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.
+ */
+
+#include "spi_slave_hs_arch.h"
+#include "mcu_periph/spi.h"
+
+#include BOARD_CONFIG
+#include "interrupt_hw.h"
+#include "std.h"
+#include "mcu.h"
+#include "led.h"
+#include "LPC21xx.h"
+#include "ssp_hw.h"
+#include "pprz_debug.h"
+#include "armVIC.h"
+
+
+/* High Speed SPI Slave Circular Buffer */
+uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx;
+uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
+uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
+uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
+
+/* Prototypes */
+// void spi_init( void ); // -> declared in spi.h
+static void SSP_ISR(void) __attribute__((naked));
+
+/* SSPCR0 settings */
+#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
+//#define SSP_DDS 0x0F << 0 /* data size : 16 bits */
+#define SSP_FRF 0x00 << 4 /* frame format : SPI */
+#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
+#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
+#define SSP_SCR 0x00 << 8 /* serial clock rate : divide by 1 */
+
+#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
+
+/* SSPCR1 settings */
+#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
+#define SSP_SSE 0x00 << 1 /* SSP enable : enable later when init ready */
+#define SSP_MS 0x01 << 2 /* master slave mode : slave */
+#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
+
+#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
+
+/* SSPCPSR settings
+ * min value as master: 2
+ * min value as slave: 12
+ */
+#if (PCLK == 15000000)
+#define CPSDVSR 12
+#else
+
+#if (PCLK == 30000000)
+#define CPSDVSR 24
+#else
+
+#if (PCLK == 60000000)
+#define CPSDVSR 28
+#else
+
+#error unknown PCLK frequency
+#endif
+#endif
+#endif
+
+#define SSP_PINSEL1_SCK (2<<2)
+#define SSP_PINSEL1_MISO (2<<4)
+#define SSP_PINSEL1_MOSI (2<<6)
+#define SSP_PINSEL1_SSEL (2<<8)
+
+
+#define SSP_Write(X) SSPDR=(X)
+#define SSP_Read() SSPDR
+#define SSP_Status() SSPSR
+
+void spi_init(void) {
+
+ /* setup pins for SSP (SCK, MISO, MOSI) */
+ PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL;
+
+ /* setup SSP */
+ // Control Registers
+ SSPCR0 = SSPCR0_VAL;
+ SSPCR1 = SSPCR1_VAL;
+ // Clock Prescale Registers
+ SSPCPSR = CPSDVSR;
+
+ /* initialize interrupt vector */
+ VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
+ VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
+ _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
+ _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
+
+
+ // Enable SPI Slave
+ SetBit(SSPCR1, SSE);
+
+ // Enable Receive interrupt
+ SetBit(SSPIMSC, RXIM);
+
+}
+
+/*
+ * SSP Status:
+ *
+ * ROVR Read Overrun
+ * WCOL Write Collision (send new byte during a transfer in progress
+ * ABRT SSEL inactive before end of transfer
+ *
+ *
+ */
+
+
+static void SSP_ISR(void) {
+ ISR_ENTRY();
+
+ //LED_TOGGLE(3);
+
+ // If any TX bytes are pending
+ if (spi_slave_hs_tx_insert_idx != spi_slave_hs_tx_extract_idx)
+ {
+ uint8_t ret = spi_slave_hs_tx_buffer[spi_slave_hs_tx_extract_idx];
+ spi_slave_hs_tx_extract_idx = (spi_slave_hs_tx_extract_idx + 1)%SPI_SLAVE_HS_TX_BUFFER_SIZE;
+ SSP_Write(ret);
+ }
+ else
+ {
+ SSP_Write(0x00);
+ }
+
+
+ //do
+ {
+ uint16_t temp;
+
+ // calc next insert index & store character
+ temp = ( spi_slave_hs_rx_insert_idx + 1) % SPI_SLAVE_HS_RX_BUFFER_SIZE;
+ spi_slave_hs_rx_buffer[ spi_slave_hs_rx_insert_idx] = SSP_Read();
+
+ // check for more room in queue
+ if (temp != spi_slave_hs_rx_extract_idx)
+ spi_slave_hs_rx_insert_idx = temp; // update insert index
+
+ // else overrun
+ }
+ // while FIFO not empty
+ //while (SSPSR & RNE);
+
+/*
+ // loop until not more interrupt sources
+ while (((iid = U0IIR) & UIIR_NO_INT) == 0)
+ while (U0LSR & ULSR_THRE)
+ {
+ // check if more data to send
+ if (uart0_tx_insert_idx != uart0_tx_extract_idx)
+ {
+ U0THR = uart0_tx_buffer[uart0_tx_extract_idx];
+ uart0_tx_extract_idx++;
+ uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE;
+ }
+ else
+ {
+ // no
+ uart0_tx_running = 0; // clear running flag
+ break;
+ }
+ }
+
+*/
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
+
diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
new file mode 100644
index 0000000000..152c3da0d7
--- /dev/null
+++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
@@ -0,0 +1,77 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin
+ *
+ * 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.
+ */
+
+#ifndef SPI_SLAVE_HS_ARCH_H
+#define SPI_SLAVE_HS_ARCH_H
+
+/*
+
+ Highspeed SPI Slave Interface
+ SS on P0.20
+ Circular Buffer
+
+*/
+
+#include "std.h"
+
+
+#define SpiEnable() { \
+ SetBit(SSPCR1, SSE); \
+ }
+
+#define SpiDisable() { \
+ ClearBit(SSPCR1, SSE); \
+ }
+
+
+#define SPI_SLAVE_HS_RX_BUFFER_SIZE 256
+
+extern uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx;
+extern uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
+
+#define SpiSlaveChAvailable() (spi_slave_hs_rx_insert_idx != spi_slave_hs_rx_extract_idx)
+
+#define SpiSlaveGetch() ({\
+ uint8_t ret = spi_slave_hs_rx_buffer[spi_slave_hs_rx_extract_idx]; \
+ spi_slave_hs_rx_extract_idx = (spi_slave_hs_rx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE; \
+ ret; \
+})
+
+#define SPI_SLAVE_HS_TX_BUFFER_SIZE 64
+
+extern uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
+extern uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
+
+#define SpiSlaveTransmit(data) {\
+ uint8_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \
+ if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ \
+ { \
+ spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \
+ spi_slave_hs_tx_insert_idx = temp; \
+ } \
+}
+
+
+
+
+#endif
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 3049849fbc..653304e000 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -384,6 +384,32 @@ static void navigation_task( void ) {
*/
+static inline void attitude_loop( void ) {
+
+#ifdef USE_GYRO
+ gyro_update();
+#endif
+
+#ifdef USE_INFRARED
+ infrared_update();
+ estimator_update_state_infrared();
+#endif /* USE_INFRARED */
+ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
+ v_ctl_throttle_slew();
+ ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
+ ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
+
+ ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
+
+#if defined MCU_SPI_LINK
+ link_mcu_send();
+#elif defined INTER_MCU && defined SINGLE_MCU
+ /**Directly set the flag indicating to FBW that shared buffer is available*/
+ inter_mcu_received_ap = TRUE;
+#endif
+
+}
+
void periodic_task_ap( void ) {
static uint8_t _60Hz = 0;
@@ -474,35 +500,18 @@ void periodic_task_ap( void ) {
#error "Only 20 and 60 allowed for CONTROL_RATE"
#endif
-
#if CONTROL_RATE == 20
if (!_20Hz)
#endif
{
-#ifdef USE_GYRO
- gyro_update();
-#endif
-
-#ifdef USE_INFRARED
- infrared_update();
- estimator_update_state_infrared();
-#endif /* USE_INFRARED */
- h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
- v_ctl_throttle_slew();
- ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
- ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
- ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
-
-#if defined MCU_SPI_LINK
- link_mcu_send();
-#elif defined INTER_MCU && defined SINGLE_MCU
- /**Directly set the flag indicating to FBW that shared buffer is available*/
- inter_mcu_received_ap = TRUE;
+#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
+ attitude_loop();
#endif
}
+
modules_periodic_task();
}
@@ -621,6 +630,16 @@ void event_task_ap( void ) {
}
modules_event_task();
+
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+ if (new_ins_attitude > 0)
+ {
+ attitude_loop();
+ //LED_TOGGLE(3);
+ new_ins_attitude = 0;
+ }
+#endif
+
} /* event_task_ap() */
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 4db0b1860c..5aefd54855 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -92,14 +92,21 @@ static inline void set_failsafe_mode( void ) {
SetCommands(commands_failsafe);
}
+
+volatile uint8_t fbw_new_actuators = 0;
+
#ifdef RADIO_CONTROL
static inline void handle_rc_frame( void ) {
fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
if (fbw_mode == FBW_MODE_MANUAL)
+ {
SetCommandsFromRC(commands, radio_control.values);
+ fbw_new_actuators = 1;
+ }
}
#endif
+
/********** EVENT ************************************************************/
void event_task_fbw( void) {
@@ -129,11 +136,24 @@ void event_task_fbw( void) {
SetApOnlyCommands(ap_state->commands);
}
#endif
+ fbw_new_actuators = 1;
+
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
+#ifdef ACTUATORS
+ if (fbw_new_actuators > 0)
+ {
+ SetActuatorsFromCommands(commands);
+ fbw_new_actuators = 0;
+ }
+#endif
+
+
+
+
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
@@ -162,7 +182,9 @@ void periodic_task_fbw( void ) {
#ifdef INTER_MCU
inter_mcu_periodic_task();
if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
+ {
set_failsafe_mode();
+ }
#endif
#ifdef DOWNLINK
@@ -173,9 +195,4 @@ void periodic_task_fbw( void ) {
electrical_periodic();
}
-#ifdef ACTUATORS
- SetActuatorsFromCommands(commands);
-#endif
-
-
}
diff --git a/sw/airborne/modules/gps_i2c/gps_i2c.c b/sw/airborne/modules/gps_i2c/gps_i2c.c
index 040ab20ccb..23a312312e 100644
--- a/sw/airborne/modules/gps_i2c/gps_i2c.c
+++ b/sw/airborne/modules/gps_i2c/gps_i2c.c
@@ -20,10 +20,13 @@
*
*/
-#include "modules/gps_i2c.h"
+#include "gps_i2c.h"
#include "mcu_periph/i2c.h"
#include "subsystems/gps.h"
+struct i2c_transaction i2c_gps_trans;
+
+
uint8_t gps_i2c_rx_buf[GPS_I2C_BUF_SIZE];
uint8_t gps_i2c_rx_insert_idx, gps_i2c_rx_extract_idx;
uint8_t gps_i2c_tx_buf[GPS_I2C_BUF_SIZE];
@@ -46,11 +49,10 @@ 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) {
+void gps_i2c_init(void) {
gps_i2c_status = GPS_I2C_STATUS_IDLE;
gps_i2c_done = TRUE;
gps_i2c_data_ready_to_transmit = FALSE;
@@ -63,32 +65,34 @@ gps_i2c_init(void) {
#endif
}
-void
-gps_i2c_periodic(void) {
+void gps_i2c_periodic(void) {
+/*
if (gps_i2c_done && gps_i2c_status == GPS_I2C_STATUS_IDLE) {
i2c0_buf[0] = GPS_I2C_ADDR_NB_AVAIL_BYTES;
i2c0_transmit_no_stop(GPS_I2C_SLAVE_ADDR, 1, &gps_i2c_done);
gps_i2c_done = FALSE;
gps_i2c_status = GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES;
}
+*/
+
}
-void
-gps_i2c_event(void) {
- switch (gps_i2c_status) {
+void gps_i2c_event(void) {
+/*
+ * switch (gps_i2c_status) {
case GPS_I2C_STATUS_IDLE:
if (gps_i2c_data_ready_to_transmit) {
- /* Copy data from our buffer to the i2c buffer */
+ // Copy data from our buffer to the i2c buffer
uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN);
uint8_t i;
for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++)
i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx];
- /* Start i2c transmit */
+ // Start i2c transmit
i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done);
gps_i2c_done = FALSE;
- /* Reset flag if finished */
+ // Reset flag if finished
if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) {
gps_i2c_data_ready_to_transmit = FALSE;
gps_i2c_tx_insert_idx = 0;
@@ -138,4 +142,6 @@ gps_i2c_event(void) {
default:
return;
}
+*/
+
}
diff --git a/sw/airborne/modules/ins/endian_functions.c b/sw/airborne/modules/ins/endian_functions.c
deleted file mode 100644
index 6518b2fb42..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 f03d28cdca..61694472e8 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,81 @@
#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;
+}
+
+void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
+{
+ data[buflen-1] = (unsigned char) (UpdateCRC(0xFFFFFFFF , data , (unsigned long) (buflen - 1) ) & 0xff) ;
+}
+
+
+/***************************************************************************
+ * 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 +123,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 +143,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,21 +161,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 +206,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 +217,6 @@ unsigned char CHIMU_Parse(
CHIMU_PARSER_DATA *pstData) /* resulting data */
{
- //long int i;
char bUpdate = FALSE;
switch (pstData->m_State) {
@@ -215,7 +263,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 +278,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;
@@ -263,6 +310,55 @@ unsigned char CHIMU_Parse(
// appropriate sentence data processor.
///////////////////////////////////////////////////////////////////////////////
+static 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 *M_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 * M_PI);
+ }
+
+ return(ps);
+
+}
+
+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;
+}
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
@@ -278,11 +374,11 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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++;
+ 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:
@@ -319,7 +415,7 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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));
+ 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]);
@@ -341,14 +437,13 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1];
pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2];
-/*
- // TODO: Read configuration bits
+ pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
+ pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
+ pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
- gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++;
- gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++;
+// TODO: Read configuration bits
- gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++;
- bC0_SPI_En = BitTest (gConfigInfo, 0);
+/* bC0_SPI_En = BitTest (gConfigInfo, 0);
bC1_HWCentrip_En = BitTest (gConfigInfo, 1);
bC2_TempCal_En = BitTest (gConfigInfo, 2);
bC3_RateOut_En = BitTest (gConfigInfo, 3);
@@ -356,13 +451,12 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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)
+ 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
@@ -380,8 +474,6 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
{
//TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?)
}
-
- //Led_Off(LED_RED);
return TRUE;
break;
@@ -402,49 +494,6 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
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);
-
+ return FALSE;
}
diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h
index 75e9069d2f..3e1434daaa 100644
--- a/sw/airborne/modules/ins/imu_chimu.h
+++ b/sw/airborne/modules/ins/imu_chimu.h
@@ -66,41 +66,54 @@ typedef struct {
CHIMU_Quaternion q;
} CHIMU_attitude_data;
+#ifndef FALSE
#define FALSE (1==0)
+#endif
+#ifndef TRUE
#define TRUE (1==1)
+#endif
typedef struct {
- int cputemp;
- int acc[3];
- int rate[3];
- int mag[3];
- int spare1;
- int euler[3];
+ float cputemp;
+ float acc[3];
+ float rate[3];
+ float mag[3];
+ float spare1;
} 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_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;
+<<<<<<< HEAD
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
+=======
+ unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
+>>>>>>> paparazzi/CHIMU
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;
+
+ // Config
+ uint8_t gCalStatus;
+ uint8_t gCHIMU_BIT;
+ uint8_t gConfigInfo;
+
} CHIMU_PARSER_DATA;
/*---------------------------------------------------------------------------
@@ -116,8 +129,7 @@ unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData);
-CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude);
-
+void CHIMU_Checksum(unsigned char *data, unsigned char buflen);
#endif // CHIMU_DEFINED
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
new file mode 100644
index 0000000000..82bb0622a9
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -0,0 +1,143 @@
+/*
+C code to connect a CHIMU using uart
+*/
+
+
+#include
+
+// SPI
+#include "mcu_periph/spi.h"
+#include "mcu_periph/spi_slave_hs_arch.h"
+
+// Output
+#include "estimator.h"
+
+// For centripedal corrections
+#include "gps.h"
+
+// Telemetry
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "ins_module.h"
+#include "imu_chimu.h"
+
+
+CHIMU_PARSER_DATA CHIMU_DATA;
+
+INS_FORMAT ins_roll_neutral;
+INS_FORMAT ins_pitch_neutral;
+
+volatile uint8_t new_ins_attitude;
+
+void ins_init( void )
+{
+// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
+ uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
+// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
+ uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
+
+ CHIMU_Checksum(rate,12);
+
+
+
+ new_ins_attitude = 0;
+
+ ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+ ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+
+ CHIMU_Init(&CHIMU_DATA);
+
+ // Quat Filter
+ for (int i=0;i<7;i++)
+ {
+ InsSend1(quaternions[i]);
+ }
+ // Wait a bit (SPI send zero)
+ InsSend1(0);
+ InsSend1(0);
+ InsSend1(0);
+ InsSend1(0);
+ InsSend1(0);
+
+ // 50Hz data: attitude only
+ for (int i=0;i<12;i++)
+ {
+ InsSend1(rate[i]);
+ }
+
+}
+
+
+//float tempang = 0;
+
+void parse_ins_msg( void )
+{
+ 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) );
+ // LED_TOGGLE(3);
+ if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
+ {
+ CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
+ }
+ if (CHIMU_DATA.m_attrates.euler.phi > M_PI)
+ {
+ CHIMU_DATA.m_attrates.euler.phi -= 2 * M_PI;
+ }
+
+ LED_TOGGLE(3);
+/* if (CHIMU_DATA.m_attitude.euler.phi == tempang)
+ {
+ LED_ON(3);
+ }
+ else
+ {
+ LED_OFF(3);
+ }
+ tempang = CHIMU_DATA.m_attitude.euler.phi;
+*/
+ 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);
+ }
+ else if(CHIMU_DATA.m_MsgID==0x02)
+ {
+
+ RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
+
+ }
+ }
+ }
+}
+
+
+//Frequency defined in conf *.xml
+void ins_periodic_task( 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 };
+
+ // Fill X-speed
+
+ CHIMU_Checksum(centripedal,19);
+
+ for (int i=0;i<19;i++)
+ {
+ InsSend1(centripedal[i]);
+ }
+
+ // Downlink Send
+}
+
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
new file mode 100644
index 0000000000..1805c2ff93
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -0,0 +1,107 @@
+/*
+C code to connect a CHIMU using uart
+*/
+
+
+#include
+//#include "modules/ins/ins_chimu_uart.h"
+
+// Output
+#include "estimator.h"
+
+// For centripedal corrections
+#include "gps.h"
+
+// Telemetry
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "ins_module.h"
+#include "imu_chimu.h"
+
+CHIMU_PARSER_DATA CHIMU_DATA;
+
+INS_FORMAT ins_roll_neutral;
+INS_FORMAT ins_pitch_neutral;
+
+volatile uint8_t new_ins_attitude;
+
+void ins_init( void )
+{
+// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI
+ uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
+// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
+ uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
+
+ new_ins_attitude = 0;
+
+ ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+ ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+
+ CHIMU_Init(&CHIMU_DATA);
+
+ // Quat Filter
+ for (int i=0;i<7;i++)
+ {
+ InsUartSend1(quaternions[i]);
+ }
+ // 50Hz
+ for (int i=0;i<12;i++)
+ {
+ InsUartSend1(rate[i]);
+ }
+
+}
+
+float tempang = 0;
+
+void parse_ins_msg( void )
+{
+ 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) );
+ // LED_TOGGLE(3);
+ if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
+ {
+ CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
+ }
+
+ if (CHIMU_DATA.m_attitude.euler.phi == tempang)
+ {
+ LED_ON(3);
+ }
+ else
+ {
+ LED_OFF(3);
+ }
+ tempang = CHIMU_DATA.m_attitude.euler.phi;
+
+ EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
+ //EstimatorSetRate(ins_p,ins_q);
+
+ DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
+
+ }
+ }
+ }
+}
+
+
+//Frequency defined in conf *.xml
+void ins_periodic_task( void )
+{
+ // Downlink Send
+}
+
diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h
index 7c707d011e..aaa8bd1a36 100644
--- a/sw/airborne/modules/ins/ins_module.h
+++ b/sw/airborne/modules/ins/ins_module.h
@@ -62,7 +62,12 @@ extern INS_FORMAT ins_mx;
extern INS_FORMAT ins_my;
extern INS_FORMAT ins_mz;
+extern INS_FORMAT ins_roll_neutral;
+extern INS_FORMAT ins_pitch_neutral;
+
+
extern volatile uint8_t ins_msg_received;
+extern volatile uint8_t new_ins_attitude;
extern void ins_init( void );
extern void ins_periodic_task( void );
@@ -79,7 +84,8 @@ void parse_ins_buffer( uint8_t );
#define InsBuffer() InsLink(ChAvailable())
#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
-#define InsUartSend1(c) InsLink(Transmit(c))
+#define InsSend1(c) InsLink(Transmit(c))
+#define InsUartSend1(c) InsSend1(c)
#define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c))
#define InsUartRunning InsLink(TxRunning)