diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index c240696262..ec25f7012b 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -205,6 +205,7 @@
+
diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile
index 50a3a7f0ef..562b5087c1 100644
--- a/conf/autopilot/lisa_l_test_progs.makefile
+++ b/conf/autopilot/lisa_l_test_progs.makefile
@@ -277,6 +277,54 @@ test_imu_b2.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c $(SRC_BOOZ_ARCH)/peri
test_imu_b2.srcs += $(SRC_BOOZ)/peripherals/booz_ms2001.c $(SRC_BOOZ_ARCH)/peripherals/booz_ms2001_arch.c
+
+#
+# test IMU aspirin
+#
+test_imu_aspirin.ARCHDIR = $(ARCHI)
+test_imu_aspirin.TARGET = test_imu_aspirin
+test_imu_aspirin.TARGETDIR = test_imu_aspirin
+test_imu_aspirin.CFLAGS = -I$(SRC_LISA) -I$(ARCHI) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_imu_aspirin.srcs += $(SRC_BOOZ_TEST)/booz_test_imu.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+
+test_imu_aspirin.CFLAGS += -DUSE_LED
+test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c
+
+test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+test_imu_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+test_imu_aspirin.srcs += $(SRC_ARCH)/uart_hw.c
+
+test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
+test_imu_aspirin.srcs += downlink.c pprz_transport.c
+
+test_imu_aspirin.srcs += math/pprz_trig_int.c
+
+test_imu_aspirin.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/booz_imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
+test_imu_aspirin.srcs += $(SRC_BOOZ)/booz_imu.c \
+ $(SRC_BOOZ)/imu/booz_imu_aspirin.c \
+ $(SRC_BOOZ_ARCH)/imu/booz_imu_aspirin_arch.c
+
+test_imu_aspirin.CFLAGS += -DUSE_I2C2
+test_imu_aspirin.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
+test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
+test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+
+
+
+
+
+
+
+
+
#
# test hmc5843
#
@@ -362,4 +410,7 @@ test_adxl345.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_
test_adxl345.srcs += downlink.c pprz_transport.c
test_adxl345.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
-test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
\ No newline at end of file
+test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+
+
+
diff --git a/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.c b/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.c
index c14187a01e..33a2ea0aa5 100644
--- a/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.c
+++ b/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.c
@@ -223,18 +223,32 @@ void adxl345_start_reading_data(void) {
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
}
+/*
+ *
+ * Gyro data ready
+ *
+ */
void exti15_10_irq_handler(void) {
/* clear EXTI */
if(EXTI_GetITStatus(EXTI_Line14) != RESET)
EXTI_ClearITPendingBit(EXTI_Line14);
- i2c2.buf[0] = ITG3200_REG_GYRO_XOUT_H;
- i2c2_transceive(ITG3200_ADDR,1, 6, &imu_aspirin.i2c_done);
+ imu_aspirin.i2c_trans_gyro.type = I2CTransTxRx;
+ imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
+ imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
+ imu_aspirin.i2c_trans_gyro.len_w = 1;
+ imu_aspirin.i2c_trans_gyro.len_r = 6;
+ i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
imu_aspirin.status = AspirinStatusReadingGyro;
}
+/*
+ *
+ * Mag data ready
+ *
+ */
void exti9_5_irq_handler(void) {
/* clear EXTI */
@@ -245,6 +259,11 @@ void exti9_5_irq_handler(void) {
}
+/*
+ *
+ * Accel data ready
+ *
+ */
void exti2_irq_handler(void) {
/* clear EXTI */
@@ -255,6 +274,11 @@ void exti2_irq_handler(void) {
}
+/*
+ *
+ * Accel end of DMA transfert
+ *
+ */
void dma1_c4_irq_handler(void) {
Adxl345Unselect();
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
diff --git a/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.h b/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.h
index cd2f07ee67..979ec0f6ce 100644
--- a/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.h
+++ b/sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.h
@@ -10,23 +10,5 @@ extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_clear_rx_buf(void);
extern void adxl345_start_reading_data(void);
-#if 0
-#define OnI2CDone() { \
- switch (imu_aspirin.status) { \
- case AspirinStatusReadingGyro: \
- { \
- \
- int16_t gp = i2c2.buf[0]<<8 | i2c2.buf[1]; \
- int16_t gq = i2c2.buf[2]<<8 | i2c2.buf[3]; \
- int16_t gr = i2c2.buf[4]<<8 | i2c2.buf[5]; \
- RATES_ASSIGN(booz_imu.gyro_unscaled, gp, gq, gr); \
- /*if (abs(booz_imu.gyro_unscaled.p) > 32000 || abs(booz_imu.gyro_unscaled.q) > 32000) LED_ON(4);*/ \
- } \
- break; \
- default: \
- break; \
- } \
- }
-#endif
#endif /* BOOZ_IMU_ASPIRIN_ARCH_H */
diff --git a/sw/airborne/booz/imu/booz_imu_aspirin.c b/sw/airborne/booz/imu/booz_imu_aspirin.c
index 3a0e0da0a8..ca7d5773d6 100644
--- a/sw/airborne/booz/imu/booz_imu_aspirin.c
+++ b/sw/airborne/booz/imu/booz_imu_aspirin.c
@@ -13,7 +13,6 @@ static void configure_accel(void);
void booz_imu_impl_init(void) {
imu_aspirin.status = AspirinStatusUninit;
- imu_aspirin.i2c_done = FALSE;
imu_aspirin.gyro_available = FALSE;
imu_aspirin.gyro_available_blaaa = FALSE;
imu_aspirin.mag_ready_for_read = FALSE;
@@ -28,7 +27,7 @@ void booz_imu_impl_init(void) {
void booz_imu_periodic(void) {
if (imu_aspirin.status == AspirinStatusUninit) {
configure_gyro();
- // configure_mag();
+ configure_mag();
configure_accel();
imu_aspirin.status = AspirinStatusIdle;
}
@@ -40,47 +39,54 @@ void booz_imu_periodic(void) {
/* sends a serie of I2C commands to configure the ITG3200 gyro */
static void configure_gyro(void) {
+ struct i2c_transaction t;
+ t.type = I2CTransTx;
+ t.slave_addr = ITG3200_ADDR;
/* set gyro range to 2000deg/s and low pass at 256Hz */
- i2c2.buf[0] = ITG3200_REG_DLPF_FS;
- i2c2.buf[1] = (0x03<<3);
- i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
+ t.buf[0] = ITG3200_REG_DLPF_FS;
+ t.buf[1] = (0x03<<3);
+ t.len_w = 2;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
/* set sample rate to 533Hz */
- i2c2.buf[0] = ITG3200_REG_SMPLRT_DIV;
- i2c2.buf[1] = 0x0E;
- i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
+ t.buf[0] = ITG3200_REG_SMPLRT_DIV;
+ t.buf[1] = 0x0E;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
/* switch to gyroX clock */
- i2c2.buf[0] = ITG3200_REG_PWR_MGM;
- i2c2.buf[1] = 0x01;
- i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
+ t.buf[0] = ITG3200_REG_PWR_MGM;
+ t.buf[1] = 0x01;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
/* enable interrupt on data ready, idle hight */
- i2c2.buf[0] = ITG3200_REG_INT_CFG;
- i2c2.buf[1] = (0x01 | 0x01<<7);
- i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
-
+ t.buf[0] = ITG3200_REG_INT_CFG;
+ t.buf[1] = (0x01 | 0x01<<7);
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+
}
/* sends a serie of I2C commands to configure the ITG3200 gyro */
static void configure_mag(void) {
+ struct i2c_transaction t;
+ t.type = I2CTransTx;
+ t.slave_addr = HMC5843_ADDR;
/* set to rate to 50Hz */
- i2c2.buf[0] = HMC5843_REG_CFGA;
- i2c2.buf[1] = 0x00 | (0x06 << 2);
- i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
+ t.buf[0] = HMC5843_REG_CFGA;
+ t.buf[1] = 0x00 | (0x06 << 2);
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
/* set to gain to 1 Gauss */
- i2c2.buf[0] = HMC5843_REG_CFGB;
- i2c2.buf[1] = 0x01<<5;
- i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
+ t.buf[0] = HMC5843_REG_CFGB;
+ t.buf[1] = 0x01<<5;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
/* set to continuous mode */
- i2c2.buf[0] = HMC5843_REG_MODE;
- i2c2.buf[1] = 0x00;
- i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
- while (!imu_aspirin.i2c_done);
+ t.buf[0] = HMC5843_REG_MODE;
+ t.buf[1] = 0x00;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
}
diff --git a/sw/airborne/booz/imu/booz_imu_aspirin.h b/sw/airborne/booz/imu/booz_imu_aspirin.h
index 71d817aba7..e85d590045 100644
--- a/sw/airborne/booz/imu/booz_imu_aspirin.h
+++ b/sw/airborne/booz/imu/booz_imu_aspirin.h
@@ -27,9 +27,27 @@
#include "airframe.h"
#include "booz_imu.h"
-#include "peripherals/booz_itg3200.h"
-#include "peripherals/booz_hmc5843.h"
-#include "peripherals/booz_adxl345.h"
+#include "i2c.h"
+#include "booz/peripherals/booz_itg3200.h"
+#include "booz/peripherals/booz_hmc5843.h"
+#include "booz/peripherals/booz_adxl345.h"
+
+
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
enum AspirinStatus
{ AspirinStatusUninit,
@@ -40,7 +58,8 @@ enum AspirinStatus
struct BoozImuAspirin {
volatile enum AspirinStatus status;
- volatile uint8_t i2c_done;
+ struct i2c_transaction i2c_trans_gyro;
+ struct i2c_transaction i2c_trans_mag;
uint8_t gyro_available;
uint8_t gyro_available_blaaa;
uint8_t mag_available;
@@ -56,14 +75,18 @@ extern struct BoozImuAspirin imu_aspirin;
#define BoozImuEvent(_gyro_accel_handler, _mag_handler) { \
- if (imu_aspirin.status == AspirinStatusReadingGyro && imu_aspirin.i2c_done) { \
- int16_t gp = i2c2.buf[0]<<8 | i2c2.buf[1]; \
- int16_t gq = i2c2.buf[2]<<8 | i2c2.buf[3]; \
- int16_t gr = i2c2.buf[4]<<8 | i2c2.buf[5]; \
+ if (imu_aspirin.status == AspirinStatusReadingGyro && \
+ imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \
+ int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1]; \
+ int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3]; \
+ int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5]; \
RATES_ASSIGN(booz_imu.gyro_unscaled, gp, gq, gr); \
if (imu_aspirin.mag_ready_for_read ) { \
/* read mag */ \
- i2c2_receive(HMC5843_ADDR, 7, &imu_aspirin.i2c_done); \
+ imu_aspirin.i2c_trans_mag.type = I2CTransRx; \
+ imu_aspirin.i2c_trans_mag.slave_addr = HMC5843_ADDR; \
+ imu_aspirin.i2c_trans_mag.len_r = 7; \
+ i2c_submit(&i2c2,&imu_aspirin.i2c_trans_mag); \
imu_aspirin.mag_ready_for_read = FALSE; \
imu_aspirin.status = AspirinStatusReadingMag; \
} \
@@ -71,10 +94,11 @@ extern struct BoozImuAspirin imu_aspirin;
imu_aspirin.status = AspirinStatusIdle; \
} \
} \
- if (imu_aspirin.status == AspirinStatusReadingMag && imu_aspirin.i2c_done) { \
- int16_t mx = i2c2.buf[0]<<8 | i2c2.buf[1]; \
- int16_t my = i2c2.buf[2]<<8 | i2c2.buf[3]; \
- int16_t mz = i2c2.buf[4]<<8 | i2c2.buf[5]; \
+ if (imu_aspirin.status == AspirinStatusReadingMag && \
+ imu_aspirin.i2c_trans_mag.status == I2CTransSuccess) { \
+ int16_t mx = imu_aspirin.i2c_trans_mag.buf[0]<<8 | imu_aspirin.i2c_trans_mag.buf[1]; \
+ int16_t my = imu_aspirin.i2c_trans_mag.buf[2]<<8 | imu_aspirin.i2c_trans_mag.buf[3]; \
+ int16_t mz = imu_aspirin.i2c_trans_mag.buf[4]<<8 | imu_aspirin.i2c_trans_mag.buf[5]; \
VECT3_ASSIGN(booz_imu.mag_unscaled, mx, my, mz); \
imu_aspirin.mag_available = TRUE; \
imu_aspirin.status = AspirinStatusIdle; \
diff --git a/sw/airborne/lisa/test/lisa_test_aspirin.c b/sw/airborne/lisa/test/lisa_test_aspirin.c
index c47496426b..4be9f8d3e4 100644
--- a/sw/airborne/lisa/test/lisa_test_aspirin.c
+++ b/sw/airborne/lisa/test/lisa_test_aspirin.c
@@ -41,7 +41,6 @@ static inline void main_event_task( void );
static inline void main_init_hw(void);
-static uint8_t i2c_done;
static volatile uint8_t gyro_ready_for_read;
extern void exti2_irq_handler(void);
diff --git a/sw/airborne/lisa/test/lisa_test_hmc5843.c b/sw/airborne/lisa/test/lisa_test_hmc5843.c
index 255cc74a89..8432493869 100644
--- a/sw/airborne/lisa/test/lisa_test_hmc5843.c
+++ b/sw/airborne/lisa/test/lisa_test_hmc5843.c
@@ -42,8 +42,12 @@ static inline void main_periodic_task( void );
static inline void main_event_task( void );
static inline void main_init_hw(void);
+static void send_config(void);
static struct i2c_transaction i2c_trans;
+struct i2c_transaction t1;
+struct i2c_transaction t2;
+
#define INITIALIZED 6
static uint8_t mag_state = 0;
static volatile uint8_t mag_ready_for_read = FALSE;
@@ -89,7 +93,9 @@ static inline void main_periodic_task( void ) {
&i2c2_errors.unexpected_event_cnt,
&i2c2_errors.last_unexpected_event);
});
-
+ if (mag_state == 2) send_config();
+
+#if 0
switch (mag_state) {
case 2:
i2c_trans.type = I2CTransTx;
@@ -124,7 +130,7 @@ static inline void main_periodic_task( void ) {
default:
break;
}
-
+#endif
// if (mag_state == 4) mag_state=1;
if (mag_state < INITIALIZED) mag_state++;
@@ -161,9 +167,42 @@ static inline void main_event_task( void ) {
reading_mag = FALSE;
}
+}
+
+
+
+static void send_config(void) {
+
+ t1.type = I2CTransTx;
+ t1.slave_addr = HMC5843_ADDR;
+ t1.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz
+ t1.buf[1] = 0x00 | (0x06 << 2);
+ t1.len_w = 2;
+ i2c_submit(&i2c2,&t1);
+
+ t2.type = I2CTransTx;
+ t2.slave_addr = HMC5843_ADDR;
+ t2.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
+ t2.buf[1] = 0x01<<5;
+ t2.len_w = 2;
+ i2c_submit(&i2c2,&t2);
+
+ i2c_trans.type = I2CTransTx;
+ i2c_trans.slave_addr = HMC5843_ADDR;
+ i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode
+ i2c_trans.buf[1] = 0x00;
+ i2c_trans.len_w = 2;
+ i2c_submit(&i2c2,&i2c_trans);
}
+
+
+
+
+
+
+
static inline void main_init_hw( void ) {
/* set mag ss as floating input (on PC12) = shorted to I2C2 sda ----------*/
/* set mag reset as floating input (on PC13) = shorted to I2C2 scl ----------*/
diff --git a/sw/airborne/stm32/i2c_hw.c b/sw/airborne/stm32/i2c_hw.c
index e623e5e342..2b5d593926 100644
--- a/sw/airborne/stm32/i2c_hw.c
+++ b/sw/airborne/stm32/i2c_hw.c
@@ -702,7 +702,7 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
/* if peripheral is idle, start the transaction */
if (p->status == I2CIdle)
start_transaction(p);
-
+ /* else it will be started by the interrupt handler when the previous transactions completes */
__enable_irq();
return TRUE;