diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml
index d2ce549035..a2a56a4355 100644
--- a/conf/airframes/Poine/booz2_a1.xml
+++ b/conf/airframes/Poine/booz2_a1.xml
@@ -203,7 +203,8 @@
-
+
+
diff --git a/conf/airframes/Poine/booz2_a6.xml b/conf/airframes/Poine/booz2_a6.xml
index 0b2779af92..100b7771db 100644
--- a/conf/airframes/Poine/booz2_a6.xml
+++ b/conf/airframes/Poine/booz2_a6.xml
@@ -182,7 +182,8 @@
-
+
+
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
index 1eaf6c77b0..502bc22281 100644
--- a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
@@ -31,9 +31,8 @@ ap.srcs += $(SRC_BOOZ)/actuators/booz_actuators_mkk.c
ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
ifeq ($(ARCHI), arm7)
+ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
-ap.CFLAGS += -DI2C0_STOP_HANDLER=ActuatorsMkkI2cHandler
-ap.CFLAGS += -DI2C0_STOP_HANDLER_HEADER=\"actuators/booz_actuators_mkk.h\"
else ifeq ($(ARCHI), stm32)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
ap.CFLAGS += -DUSE_I2C1
diff --git a/sw/airborne/arm7/i2c_hw.c b/sw/airborne/arm7/i2c_hw.c
index 1f7e54d8eb..f8257b934f 100644
--- a/sw/airborne/arm7/i2c_hw.c
+++ b/sw/airborne/arm7/i2c_hw.c
@@ -24,12 +24,9 @@
#include "i2c.h"
-
-#include "led.h"
-
#include "std.h"
-
#include "interrupt_hw.h"
+#include BOARD_CONFIG
///////////////////
@@ -206,13 +203,27 @@ static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) {
#define I2C0_VIC_SLOT 9
#endif
+
void i2c0_ISR(void) __attribute__((naked));
+void i2c0_ISR(void) {
+ ISR_ENTRY();
+
+ uint32_t state = I2C0STAT;
+ I2cAutomaton(state,&i2c0);
+ I2cClearIT(i2c0.reg_addr);
+
+ VICVectAddr = 0x00000000; // clear this interrupt from the VIC
+ ISR_EXIT(); // recover registers and return
+}
+
/* SDA0 on P0.3 */
/* SCL0 on P0.2 */
void i2c0_hw_init ( void ) {
+ i2c0.reg_addr = I2C0;
+
/* set P0.2 and P0.3 to I2C0 */
PINSEL0 |= 1 << 4 | 1 << 6;
/* clear all flags */
@@ -230,21 +241,6 @@ void i2c0_hw_init ( void ) {
_VIC_ADDR(I2C0_VIC_SLOT) = (uint32_t)i2c0_ISR; // address of the ISR
}
-#define I2C_DATA_REG I2C0DAT
-#define I2C_STATUS_REG I2C0STAT
-
-void i2c0_ISR(void)
-{
- ISR_ENTRY();
-
- uint32_t state = I2C_STATUS_REG;
- I2c0Automaton(state);
- I2c0ClearIT();
-
- VICVectAddr = 0x00000000; // clear this interrupt from the VIC
- ISR_EXIT(); // recover registers and return
-}
-
#endif /* USE_I2C0 */
@@ -283,16 +279,25 @@ void i2c0_ISR(void)
#endif
#endif
-
-#define I2C1_DATA_REG I2C1DAT
-#define I2C1_STATUS_REG I2C1STAT
-
void i2c1_ISR(void) __attribute__((naked));
+void i2c1_ISR(void) {
+ ISR_ENTRY();
+
+ uint32_t state = I2C1STAT;
+ I2cAutomaton(state,&i2c1);
+ I2cClearIT(i2c1.reg_addr);
+
+ VICVectAddr = 0x00000000; // clear this interrupt from the VIC
+ ISR_EXIT(); // recover registers and return
+}
+
/* SDA1 on P0.14 */
/* SCL1 on P0.11 */
void i2c1_hw_init ( void ) {
+ i2c1.reg_addr = I2C1;
+
/* set P0.11 and P0.14 to I2C1 */
PINSEL0 |= 3 << 22 | 3 << 28;
/* clear all flags */
@@ -310,100 +315,8 @@ void i2c1_hw_init ( void ) {
_VIC_ADDR(I2C1_VIC_SLOT) = (uint32_t)i2c1_ISR; // address of the ISR
}
-void i2c1_ISR(void)
-{
- ISR_ENTRY();
-
- uint32_t state = I2C1_STATUS_REG;
- I2c1Automaton(state);
- I2c1ClearIT();
-
- VICVectAddr = 0x00000000; // clear this interrupt from the VIC
- ISR_EXIT(); // recover registers and return
-}
-
-
#endif /* USE_I2C1 */
-#ifdef USE_I2C2
-
-
-/* default clock speed 37.5KHz with our 15MHz PCLK
- I2C0_CLOCK = PCLK / (I2C0_SCLL + I2C0_SCLH) */
-#ifndef I2C0_SCLL
-#define I2C0_SCLL 200
-#endif
-
-#ifndef I2C0_SCLH
-#define I2C0_SCLH 200
-#endif
-
-/* adjust for other PCLKs */
-
-#if (PCLK == 15000000)
-#define I2C0_SCLL_D I2C0_SCLL
-#define I2C0_SCLH_D I2C0_SCLH
-#else
-
-#if (PCLK == 30000000)
-#define I2C0_SCLL_D (2*I2C0_SCLL)
-#define I2C0_SCLH_D (2*I2C0_SCLH)
-#else
-
-#if (PCLK == 60000000)
-#define I2C0_SCLL_D (4*I2C0_SCLL)
-#define I2C0_SCLH_D (4*I2C0_SCLH)
-#else
-
-#error unknown PCLK frequency
-#endif
-#endif
-#endif
-
-#ifndef I2C0_VIC_SLOT
-#define I2C0_VIC_SLOT 9
-#endif
-
-void i2c0_ISR(void) __attribute__((naked));
-
-
-/* SDA0 on P0.3 */
-/* SCL0 on P0.2 */
-void i2c2_hw_init ( void ) {
-
- i2c2.reg_addr = I2C0;
-
- /* set P0.2 and P0.3 to I2C0 */
- PINSEL0 |= 1 << 4 | 1 << 6;
- /* clear all flags */
- I2C0CONCLR = _BV(AAC) | _BV(SIC) | _BV(STAC) | _BV(I2ENC);
- /* enable I2C */
- I2C0CONSET = _BV(I2EN);
- /* set bitrate */
- I2C0SCLL = I2C0_SCLL_D;
- I2C0SCLH = I2C0_SCLH_D;
-
- // initialize the interrupt vector
- VICIntSelect &= ~VIC_BIT(VIC_I2C0); // I2C0 selected as IRQ
- VICIntEnable = VIC_BIT(VIC_I2C0); // I2C0 interrupt enabled
- _VIC_CNTL(I2C0_VIC_SLOT) = VIC_ENABLE | VIC_I2C0;
- _VIC_ADDR(I2C0_VIC_SLOT) = (uint32_t)i2c0_ISR; // address of the ISR
-}
-
-#define I2C_STATUS_REG I2C0STAT
-
-void i2c0_ISR(void)
-{
- ISR_ENTRY();
-
- uint32_t state = I2C_STATUS_REG;
- I2cAutomaton(state,&i2c2);
- I2cClearIT(i2c2.reg_addr);
-
- VICVectAddr = 0x00000000; // clear this interrupt from the VIC
- ISR_EXIT(); // recover registers and return
-}
-
bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
@@ -428,6 +341,5 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
return TRUE;
}
-#endif /* USE_I2C2 */
diff --git a/sw/airborne/arm7/i2c_hw.h b/sw/airborne/arm7/i2c_hw.h
index bc4c53b9a5..9bfe3cf39d 100644
--- a/sw/airborne/arm7/i2c_hw.h
+++ b/sw/airborne/arm7/i2c_hw.h
@@ -16,191 +16,19 @@
#define I2C_MR_DATA_NACK 0x58
-#define I2C_IDLE 0
-#define I2C_BUSY 1
-
-
-
#ifdef USE_I2C0
-#ifdef I2C0_STOP_HANDLER
-#include I2C0_STOP_HANDLER_HEADER
-#define I2c0StopHandler() I2C0_STOP_HANDLER()
-#else
-#define I2c0StopHandler() {}
-#endif /* I2C0_STOP_HANDLER */
-
-
extern void i2c0_hw_init(void);
-#define I2c0SendAck() { I2C0CONSET = _BV(AA); }
-#define I2c0Finished() { \
- if (i2c0_finished) *i2c0_finished = TRUE; \
- i2c0_status = I2C_IDLE; \
- I2c0StopHandler(); \
-}
-#define I2c0SendStop() { \
- I2C0CONSET = _BV(STO); \
- I2c0Finished(); \
- }
-#define I2c0SendStart() { I2C0CONSET = _BV(STA); }
-#define I2c0SendByte(b) { I2C_DATA_REG = b; }
-
-#define I2c0Receive(_ack) { \
- if (_ack) I2C0CONSET = _BV(AA); \
- else I2C0CONCLR = _BV(AAC); \
- }
-
-#define I2c0ClearStart() { I2C0CONCLR = _BV(STAC); }
-#define I2c0ClearIT() { I2C0CONCLR = _BV(SIC); }
-
-#define I2c0Automaton(state) { \
- switch (state) { \
- case I2C_START: \
- case I2C_RESTART: \
- I2c0SendByte(i2c0_slave_addr); \
- I2c0ClearStart(); \
- i2c0_index = 0; \
- break; \
- case I2C_MR_DATA_ACK: \
- if (i2c0_index < i2c0_len_r) { \
- i2c0_buf[i2c0_index] = I2C_DATA_REG; \
- i2c0_index++; \
- I2c0Receive(i2c0_index < i2c0_len_r - 1); \
- } \
- else { \
- /* error , we should have got NACK */ \
- I2c0SendStop(); \
- } \
- break; \
- case I2C_MR_SLA_ACK: /* At least one char */ \
- /* Wait and reply with ACK or NACK */ \
- I2c0Receive(i2c0_index < i2c0_len_r - 1); \
- break; \
- case I2C_MR_SLA_NACK: \
- case I2C_MT_SLA_NACK: \
- I2c0SendStart(); \
- break; \
- case I2C_MT_SLA_ACK: \
- case I2C_MT_DATA_ACK: \
- if (i2c0_index < i2c0_len_w) { \
- I2c0SendByte(i2c0_buf[i2c0_index]); \
- i2c0_index++; \
- } else { \
- if (i2c0_trx) { \
- i2c0_trx = 0; \
- i2c0_index = 0; \
- i2c0_slave_addr |= 1; \
- I2c0SendStart(); \
- } else { \
- if (i2c0_stop_after_transmit) { \
- I2c0SendStop(); \
- } else { \
- I2c0Finished(); \
- } \
- } \
- } \
- break; \
- case I2C_MR_DATA_NACK: \
- if (i2c0_index < i2c0_len_r) { \
- i2c0_buf[i2c0_index] = I2C_DATA_REG; \
- } \
- I2c0SendStop(); \
- break; \
- default: \
- I2c0SendStop(); \
- /* FIXME log error */ \
- } \
- } \
-
#endif /* USE_I2C0 */
+
#ifdef USE_I2C1
extern void i2c1_hw_init(void);
-#define I2c1StopHandler() {}
-
-#define I2c1SendAck() { I2C1CONSET = _BV(AA); }
-#define I2c1SendStop() { \
- I2C1CONSET = _BV(STO); \
- if (i2c1_finished) *i2c1_finished = TRUE; \
- i2c1_status = I2C_IDLE; \
- I2c1StopHandler(); \
- }
-#define I2c1SendStart() { I2C1CONSET = _BV(STA); }
-#define I2c1SendByte(b) { I2C1_DATA_REG = b; }
-
-#define I2c1Receive(_ack) { \
- if (_ack) I2C1CONSET = _BV(AA); \
- else I2C1CONCLR = _BV(AAC); \
- }
-
-#define I2c1ClearStart() { I2C1CONCLR = _BV(STAC); }
-#define I2c1ClearIT() { I2C1CONCLR = _BV(SIC); }
-
-#define I2c1Automaton(state) { \
- switch (state) { \
- case I2C_START: \
- case I2C_RESTART: \
- I2c1SendByte(i2c1_slave_addr); \
- I2c1ClearStart(); \
- i2c1_index = 0; \
- break; \
- case I2C_MR_DATA_ACK: \
- if (i2c1_index < i2c1_len_r) { \
- i2c1_buf[i2c1_index] = I2C1_DATA_REG; \
- i2c1_index++; \
- I2c1Receive(i2c1_index < i2c1_len_r - 1); \
- } \
- else { \
- /* error , we should have got NACK */ \
- I2c1SendStop(); \
- } \
- break; \
- case I2C_MR_SLA_ACK: /* At least one char */ \
- /* Wait and reply with ACK or NACK */ \
- I2c1Receive(i2c1_index < i2c1_len_r - 1); \
- break; \
- case I2C_MR_SLA_NACK: \
- case I2C_MT_SLA_NACK: \
- I2c1SendStart(); \
- break; \
- case I2C_MT_SLA_ACK: \
- case I2C_MT_DATA_ACK: \
- if (i2c1_index < i2c1_len_w) { \
- I2c1SendByte(i2c1_buf[i2c1_index]); \
- i2c1_index++; \
- } else { \
- if (i2c1_trx) { \
- i2c1_trx = 0; \
- i2c1_index = 0; \
- i2c1_slave_addr |= 1; \
- I2c1SendStart(); \
- } else { \
- I2c1SendStop(); \
- } \
- } \
- break; \
- case I2C_MR_DATA_NACK: \
- if (i2c1_index < i2c1_len_r) { \
- i2c1_buf[i2c1_index] = I2C1_DATA_REG; \
- } \
- I2c1SendStop(); \
- break; \
- default: \
- I2c1SendStop(); \
- /* LED_ON(2); FIXME log error */ \
- } \
- } \
-
#endif /* USE_I2C1 */
-#ifdef USE_I2C2
-
-extern void i2c2_hw_init(void);
-
-#endif
#endif /* I2C_HW_H */
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 ebd8e76d30..de0b15a410 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
@@ -239,7 +239,8 @@ void exti15_10_irq_handler(void) {
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;
- if (!i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro)) while(1);
+ // if (!i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro)) while(1);
+ i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
imu_aspirin.status = AspirinStatusReadingGyro;
}
diff --git a/sw/airborne/booz/imu/booz_imu_aspirin.c b/sw/airborne/booz/imu/booz_imu_aspirin.c
index ca7d5773d6..cc9dad2437 100644
--- a/sw/airborne/booz/imu/booz_imu_aspirin.c
+++ b/sw/airborne/booz/imu/booz_imu_aspirin.c
@@ -90,6 +90,20 @@ static void configure_mag(void) {
}
+
+static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
+ uint8_t max_retry = 8;
+ uint8_t nb_retry = 0;
+ do {
+ i2c_submit(&i2c2,&t);
+ while (t.status == I2CTransPending || t.status == I2CTransRunning);
+ if (t.status == I2CTransFailed)
+ nb_retry++;
+ }
+ while (t.status != I2CTransSuccess || nb_retry < max_retry);
+}
+
+
static void configure_accel(void) {
/* set data rate to 800Hz */
diff --git a/sw/airborne/booz/peripherals/booz_ami601.c b/sw/airborne/booz/peripherals/booz_ami601.c
index aca953e838..04a308b335 100644
--- a/sw/airborne/booz/peripherals/booz_ami601.c
+++ b/sw/airborne/booz/peripherals/booz_ami601.c
@@ -6,7 +6,7 @@ uint8_t ami601_foo3;
uint16_t ami601_values[AMI601_NB_CHAN];
volatile uint8_t ami601_status;
-volatile bool_t ami601_i2c_done;
+struct i2c_transaction ami601_i2c_trans;
volatile uint32_t ami601_nb_err;
void ami601_init( void ) {
diff --git a/sw/airborne/booz/peripherals/booz_ami601.h b/sw/airborne/booz/peripherals/booz_ami601.h
index 3ac4d30f59..11ac5e9695 100644
--- a/sw/airborne/booz/peripherals/booz_ami601.h
+++ b/sw/airborne/booz/peripherals/booz_ami601.h
@@ -22,7 +22,7 @@ extern uint8_t ami601_foo3;
#define AMI601_READING_MEASURE 3
#define AMI601_DATA_AVAILABLE 4
extern volatile uint8_t ami601_status;
-extern volatile bool_t ami601_i2c_done;
+extern struct i2c_transaction ami601_i2c_trans;
extern volatile uint32_t ami601_nb_err;
#define AMI601_SLAVE_ADDR 0x60
diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c
index d641e719c4..0585c26fa7 100644
--- a/sw/airborne/booz/test/booz_test_imu.c
+++ b/sw/airborne/booz/test/booz_test_imu.c
@@ -86,7 +86,7 @@ static inline void main_periodic_task( void ) {
&i2c2_errors.last_unexpected_event);
});
#endif
- booz_imu_periodic();
+ if (cpu_time_sec > 1) booz_imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();});
}
diff --git a/sw/airborne/i2c.c b/sw/airborne/i2c.c
index 589088044e..9036dc8004 100644
--- a/sw/airborne/i2c.c
+++ b/sw/airborne/i2c.c
@@ -1,74 +1,17 @@
-#include /* For NULL */
#include "i2c.h"
-#define I2C_RECEIVE 1
-
#ifdef USE_I2C0
-volatile uint8_t i2c0_status;
-volatile uint8_t i2c0_buf[I2C0_BUF_LEN];
-volatile uint16_t i2c0_len_r;
-volatile uint8_t i2c0_len_w;
-volatile uint8_t i2c0_index;
-volatile uint8_t i2c0_slave_addr;
-volatile bool_t i2c0_stop_after_transmit;
-volatile uint8_t i2c0_trx;
-
-volatile bool_t* i2c0_finished;
+struct i2c_periph i2c0;
void i2c0_init(void) {
- i2c0_status = I2C_IDLE;
+ i2c_init(&i2c0);
i2c0_hw_init();
- i2c0_finished = NULL;
}
-
-void i2c0_receive(uint8_t slave_addr, uint16_t len, volatile bool_t* finished) {
- i2c0_trx = 0;
- i2c0_len_r = len;
- i2c0_slave_addr = slave_addr | I2C_RECEIVE;
- i2c0_finished = finished;
- i2c0_status = I2C_BUSY;
- i2c0_stop_after_transmit = TRUE; /** Default "historic" behaviour */
- I2c0SendStart();
-}
-
-static inline void i2c0_init_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
- i2c0_trx = 0;
- i2c0_len_w = len;
- i2c0_slave_addr = slave_addr & ~I2C_RECEIVE;
- i2c0_finished = finished;
- i2c0_status = I2C_BUSY;
-}
-
-
-void i2c0_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
- i2c0_trx = 0;
- i2c0_init_transmit(slave_addr, len, finished);
- i2c0_stop_after_transmit = TRUE; /** Default "historic" behaviour */
- I2c0SendStart();
-}
-
-void i2c0_transmit_no_stop(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
- i2c0_trx = 0;
- i2c0_init_transmit(slave_addr, len, finished);
- i2c0_stop_after_transmit = FALSE; /** Default "historic" behaviour */
- I2c0SendStart();
-}
-
-void i2c0_transceive(uint8_t slave_addr, uint8_t len_w, uint16_t len_r, volatile bool_t* finished) {
- i2c0_trx = 1;
- i2c0_len_w = len_w;
- i2c0_len_r = len_r;
- i2c0_slave_addr = slave_addr & ~I2C_RECEIVE;
- i2c0_finished = finished;
- i2c0_status = I2C_BUSY;
- I2c0SendStart();
-}
-
-
#endif /* USE_I2C0 */
+
#ifdef USE_I2C1
struct i2c_periph i2c1;
@@ -83,8 +26,6 @@ void i2c1_init(void) {
#ifdef USE_I2C2
-#include "booz/booz2_debug.h"
-
struct i2c_periph i2c2;
void i2c2_init(void) {
diff --git a/sw/airborne/i2c.h b/sw/airborne/i2c.h
index 2f28a04431..8c1bc91352 100644
--- a/sw/airborne/i2c.h
+++ b/sw/airborne/i2c.h
@@ -102,31 +102,13 @@ struct i2c_errors {
#ifdef USE_I2C0
+extern struct i2c_periph i2c0;
extern void i2c0_init(void);
-extern void i2c0_receive(uint8_t slave_addr, uint16_t len, volatile bool_t* finished);
-extern void i2c0_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
-extern void i2c0_transmit_no_stop(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
-extern void i2c0_transceive(uint8_t slave_addr, uint8_t len_w, uint16_t len_r, volatile bool_t* finished);
-
-extern volatile uint8_t i2c0_status;
-extern volatile bool_t i2c0_stop_after_transmit;
-
-
-#ifndef I2C0_BUF_LEN
-#define I2C0_BUF_LEN 32
-#endif
-
-extern volatile uint8_t i2c0_buf[I2C0_BUF_LEN];
-extern volatile uint16_t i2c0_len_r;
-extern volatile uint8_t i2c0_len_w;
-extern volatile uint8_t i2c0_index;
-extern volatile uint8_t i2c0_slave_addr;
-extern volatile uint8_t i2c0_trx;
-
-extern volatile bool_t* i2c0_finished;
#endif /* USE_I2C0 */
+
+
#ifdef USE_I2C1
extern struct i2c_periph i2c1;