diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index 93cf767bf8..434b4f457a 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -41,6 +41,8 @@ ap.CFLAGS += -DCONFIG=$(BOARD_CFG)
 ap.srcs += $(SRC_BOOZ)/booz2_main.c
 ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
 # -DTIME_LED=1
+ap.CFLAGS += -DPERIPHERALS_AUTO_INIT
+
 ap.CFLAGS += -DLED
 ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 
diff --git a/conf/autopilot/booz2_imu_b2v1.makefile b/conf/autopilot/booz2_imu_b2v1.makefile
index 1e3146dd64..24ff0d77e9 100644
--- a/conf/autopilot/booz2_imu_b2v1.makefile
+++ b/conf/autopilot/booz2_imu_b2v1.makefile
@@ -47,7 +47,7 @@
 #
 
 # imu Booz2 v1
-ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\"
+ap.CFLAGS += -DBOOZ2_IMU_TYPE_H=\"booz2_imu_b2.h\"
 ap.CFLAGS += -DSSP_VIC_SLOT=9
 ap.srcs += $(SRC_BOOZ)/booz2_imu_b2.c $(SRC_BOOZ_ARCH)/booz2_imu_b2_hw.c
 ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
@@ -56,12 +56,11 @@ ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2
 #ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 ap.CFLAGS += -DUSE_AMI601
 ap.srcs += AMI601.c
-ap.CFLAGS += -DFLOAT_T=float
 ap.srcs += $(SRC_BOOZ)/booz2_imu.c
 
 
 
-sim.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\"
+sim.CFLAGS += -DBOOZ2_IMU_TYPE_H=\"booz2_imu_b2.h\"
 sim.srcs += $(SRC_BOOZ)/booz2_imu.c \
 	    $(SRC_BOOZ)/booz2_imu_b2.c \
             $(SRC_BOOZ_SIM)/booz2_imu_b2_hw.c \
diff --git a/conf/autopilot/booz2_imu_b2v1_1.makefile b/conf/autopilot/booz2_imu_b2v1_1.makefile
index d143fff336..d483cb63f1 100644
--- a/conf/autopilot/booz2_imu_b2v1_1.makefile
+++ b/conf/autopilot/booz2_imu_b2v1_1.makefile
@@ -47,14 +47,13 @@
 #
 
 # imu Booz2 v1.1
-ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\"
+ap.CFLAGS += -DBOOZ2_IMU_TYPE_H=\"booz2_imu_b2.h\"
 ap.CFLAGS += -DSSP_VIC_SLOT=9 -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
 ap.srcs += $(SRC_BOOZ)/booz2_imu_b2.c $(SRC_BOOZ_ARCH)/booz2_imu_b2_hw.c
 ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
 ap.srcs += $(SRC_BOOZ)/booz2_max1168.c $(SRC_BOOZ_ARCH)/booz2_max1168_hw.c
 ap.CFLAGS += -DUSE_MICROMAG -DMICROMAG_DRDY_VIC_SLOT=11
 ap.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
-ap.CFLAGS += -DFLOAT_T=float
 ap.srcs += $(SRC_BOOZ)/booz2_imu.c
 
 
diff --git a/conf/autopilot/booz2_imu_crista.makefile b/conf/autopilot/booz2_imu_crista.makefile
index a597ffc8d2..d5704d6fd7 100644
--- a/conf/autopilot/booz2_imu_crista.makefile
+++ b/conf/autopilot/booz2_imu_crista.makefile
@@ -48,22 +48,19 @@
 
 
 
-ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
+ap.CFLAGS += -DBOOZ2_IMU_TYPE_H=\"booz2_imu_crista.h\"
 ap.srcs += $(SRC_BOOZ)/booz2_imu_crista.c $(SRC_BOOZ_ARCH)/booz2_imu_crista_hw.c
 ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
 ap.CFLAGS += -DUSE_AMI601
 ap.srcs += AMI601.c
-ap.CFLAGS += -DFLOAT_T=float
 ap.srcs += $(SRC_BOOZ)/booz2_imu.c
 
 
 
-sim.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
+sim.CFLAGS += -DBOOZ2_IMU_TYPE_H=\"booz2_imu_crista.h\"
 sim.srcs += $(SRC_BOOZ)/booz2_imu.c \
 	    $(SRC_BOOZ)/booz2_imu_crista.c \
 
 sim.CFLAGS += -DUSE_I2C1
-#  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
-#sim.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 sim.CFLAGS += -DUSE_AMI601
 sim.srcs += AMI601.c
\ No newline at end of file
diff --git a/sw/airborne/arm7/i2c_hw.c b/sw/airborne/arm7/i2c_hw.c
index ce526646aa..58bb9baace 100644
--- a/sw/airborne/arm7/i2c_hw.c
+++ b/sw/airborne/arm7/i2c_hw.c
@@ -31,6 +31,9 @@
 
 #include "interrupt_hw.h"
 
+
+#ifdef USE_I2C0
+
 /* default clock speed 37.5KHz with our 15MHz PCLK 
    I2C0_CLOCK = PCLK / (I2C0_SCLL + I2C0_SCLH)     */
 #ifndef I2C0_SCLL
@@ -41,37 +44,21 @@
 #define I2C0_SCLH 200
 #endif
 
-/* default clock speed 37.5KHz with our 15MHz PCLK 
-   I2C1_CLOCK = PCLK / (I2C1_SCLL + I2C1_SCLH)     */
-#ifndef I2C1_SCLL
-#define I2C1_SCLL 200
-#endif
-
-#ifndef I2C1_SCLH
-#define I2C1_SCLH 200
-#endif
-
 /* adjust for other PCLKs */
 
 #if (PCLK == 15000000)
 #define I2C0_SCLL_D I2C0_SCLL
 #define I2C0_SCLH_D I2C0_SCLH
-#define I2C1_SCLL_D I2C1_SCLL
-#define I2C1_SCLH_D I2C1_SCLH
 #else
 
 #if (PCLK == 30000000)
 #define I2C0_SCLL_D (2*I2C0_SCLL)
 #define I2C0_SCLH_D (2*I2C0_SCLH)
-#define I2C1_SCLL_D (2*I2C1_SCLL)
-#define I2C1_SCLH_D (2*I2C1_SCLH)
 #else
 
 #if (PCLK == 60000000)
 #define I2C0_SCLL_D (4*I2C0_SCLL)
 #define I2C0_SCLH_D (4*I2C0_SCLH)
-#define I2C1_SCLL_D (4*I2C1_SCLL)
-#define I2C1_SCLH_D (4*I2C1_SCLH)
 #else
 
 #error unknown PCLK frequency
@@ -115,15 +102,52 @@ void i2c0_ISR(void)
   ISR_ENTRY();
 
   uint32_t state = I2C_STATUS_REG;
-  I2cAutomaton(state);
-  I2cClearIT();
+  I2c0Automaton(state);
+  I2c0ClearIT();
   
   VICVectAddr = 0x00000000;             // clear this interrupt from the VIC
   ISR_EXIT();                           // recover registers and return
 }
 
+#endif /* USE_I2C0 */
+
+
+
 #ifdef USE_I2C1
 
+/* default clock speed 37.5KHz with our 15MHz PCLK 
+   I2C1_CLOCK = PCLK / (I2C1_SCLL + I2C1_SCLH)     */
+#ifndef I2C1_SCLL
+#define I2C1_SCLL 200
+#endif
+
+#ifndef I2C1_SCLH
+#define I2C1_SCLH 200
+#endif
+
+/* adjust for other PCLKs */
+
+#if (PCLK == 15000000)
+#define I2C1_SCLL_D I2C1_SCLL
+#define I2C1_SCLH_D I2C1_SCLH
+#else
+
+#if (PCLK == 30000000)
+#define I2C1_SCLL_D (2*I2C1_SCLL)
+#define I2C1_SCLH_D (2*I2C1_SCLH)
+#else
+
+#if (PCLK == 60000000)
+#define I2C1_SCLL_D (4*I2C1_SCLL)
+#define I2C1_SCLH_D (4*I2C1_SCLH)
+#else
+
+#error unknown PCLK frequency
+#endif
+#endif
+#endif
+
+
 #define I2C1_DATA_REG   I2C1DAT
 #define I2C1_STATUS_REG I2C1STAT
 
diff --git a/sw/airborne/arm7/i2c_hw.h b/sw/airborne/arm7/i2c_hw.h
index d8df1ae7a7..9351e71075 100644
--- a/sw/airborne/arm7/i2c_hw.h
+++ b/sw/airborne/arm7/i2c_hw.h
@@ -4,17 +4,18 @@
 
 #include "LPC21xx.h"
 
+#ifdef USE_I2C0
 
 #ifdef USE_BUSS_TWI_BLMC
 #include "actuators_buss_twi_blmc_hw.h"
-#define I2cStopHandler() ActuatorsBussTwiBlmcNext()
+#define I2c0StopHandler() ActuatorsBussTwiBlmcNext()
 #else
 
 #ifdef USE_BUSS_TWI_BLMC_MOTOR
 #include "buss_twi_blmc_hw.h"
-#define I2cStopHandler() BussTwiBlmcNext()
+#define I2c0StopHandler() BussTwiBlmcNext()
 #else
-#define I2cStopHandler() {}
+#define I2c0StopHandler() {}
 #endif
 
 #endif
@@ -23,23 +24,25 @@
 
 extern void i2c0_hw_init(void);
 
-#define I2cSendAck()   { I2C0CONSET = _BV(AA); }
-#define I2cSendStop()  {						\
+#define I2c0SendAck()   { I2C0CONSET = _BV(AA); }
+#define I2c0SendStop()  {						\
     I2C0CONSET = _BV(STO);						\
-    if (i2c_finished) *i2c_finished = TRUE;				\
-    i2c_status = I2C_IDLE;						\
-    I2cStopHandler();							\
+    if (i2c0_finished) *i2c0_finished = TRUE;				\
+    i2c0_status = I2C_IDLE;						\
+    I2c0StopHandler();							\
   }
-#define I2cSendStart() { I2C0CONSET = _BV(STA); }
-#define I2cSendByte(b) { I2C_DATA_REG = b; }
+#define I2c0SendStart() { I2C0CONSET = _BV(STA); }
+#define I2c0SendByte(b) { I2C_DATA_REG = b; }
 
-#define I2cReceive(_ack) {	    \
+#define I2c0Receive(_ack) {	    \
     if (_ack) I2C0CONSET = _BV(AA); \
     else I2C0CONCLR = _BV(AAC);	    \
   }
 
-#define I2cClearStart() { I2C0CONCLR = _BV(STAC); }
-#define I2cClearIT() { I2C0CONCLR = _BV(SIC); }
+#define I2c0ClearStart() { I2C0CONCLR = _BV(STAC); }
+#define I2c0ClearIT() { I2C0CONCLR = _BV(SIC); }
+
+#endif /* USE_I2C0 */
 
 
 #ifdef USE_I2C1
diff --git a/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c b/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c
index 97a860d279..10a51fed3f 100644
--- a/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c
+++ b/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c
@@ -63,43 +63,43 @@ void asctec_twi_controller_send() {
     switch (actuators_asctec_twi_blmc_command) {
       
     case MB_TWI_CONTROLLER_COMMAND_TEST :
-      i2c_buf[0] = 251;
-      i2c_buf[1] = actuators_asctec_twi_blmc_addr;
-      i2c_buf[2] = 0;
-      i2c_buf[3] = 231 + actuators_asctec_twi_blmc_addr;
+      i2c0_buf[0] = 251;
+      i2c0_buf[1] = actuators_asctec_twi_blmc_addr;
+      i2c0_buf[2] = 0;
+      i2c0_buf[3] = 231 + actuators_asctec_twi_blmc_addr;
       mb_twi_i2c_done = FALSE;
-      i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+      i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
       break;
       
     case MB_TWI_CONTROLLER_COMMAND_REVERSE :
-      i2c_buf[0] = 254;
-      i2c_buf[1] = actuators_asctec_twi_blmc_addr;
-      i2c_buf[2] = 0;
-      i2c_buf[3] = 234 + actuators_asctec_twi_blmc_addr;
+      i2c0_buf[0] = 254;
+      i2c0_buf[1] = actuators_asctec_twi_blmc_addr;
+      i2c0_buf[2] = 0;
+      i2c0_buf[3] = 234 + actuators_asctec_twi_blmc_addr;
       mb_twi_i2c_done = FALSE;
-      i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+      i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
       break;
       
     case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
-      i2c_buf[0] = 250;
-      i2c_buf[1] = actuators_asctec_twi_blmc_addr;
-      i2c_buf[2] = actuators_asctec_twi_blmc_new_addr;
-      i2c_buf[3] = 230 + actuators_asctec_twi_blmc_addr + 
+      i2c0_buf[0] = 250;
+      i2c0_buf[1] = actuators_asctec_twi_blmc_addr;
+      i2c0_buf[2] = actuators_asctec_twi_blmc_new_addr;
+      i2c0_buf[3] = 230 + actuators_asctec_twi_blmc_addr + 
 	actuators_asctec_twi_blmc_new_addr;
       actuators_asctec_twi_blmc_addr = actuators_asctec_twi_blmc_new_addr;
       mb_twi_i2c_done = FALSE;
-      i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+      i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
       break;
       
     }
     actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE;
   }
   else {
-    i2c_buf[0] = 100 + asctec_twi_blmc_motor_power[SERVO_PITCH];
-    i2c_buf[1] = 100 + asctec_twi_blmc_motor_power[SERVO_ROLL];
-    i2c_buf[2] = 100 + asctec_twi_blmc_motor_power[SERVO_YAW];
-    i2c_buf[3] = asctec_twi_blmc_motor_power[SERVO_THRUST];
+    i2c0_buf[0] = 100 + asctec_twi_blmc_motor_power[SERVO_PITCH];
+    i2c0_buf[1] = 100 + asctec_twi_blmc_motor_power[SERVO_ROLL];
+    i2c0_buf[2] = 100 + asctec_twi_blmc_motor_power[SERVO_YAW];
+    i2c0_buf[3] = asctec_twi_blmc_motor_power[SERVO_THRUST];
     mb_twi_i2c_done = FALSE;
-    i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
+    i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
   }
 }
diff --git a/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.h b/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.h
index 4dd6cba73c..ec69e15144 100644
--- a/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.h
+++ b/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.h
@@ -102,9 +102,9 @@ extern const uint8_t buss_twi_blmc_addr[];
       buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;	\
   }
 
-#define ActuatorsBussTwiBlmcSend() {					             \
-    i2c_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx];		             \
-    i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \
+#define ActuatorsBussTwiBlmcSend() {					              \
+    i2c0_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx];		              \
+    i2c0_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \
   }
 
 
diff --git a/sw/airborne/booz/arm7/booz2_imu_crista_hw.c b/sw/airborne/booz/arm7/booz2_imu_crista_hw.c
index eb16aa2450..c934a0d932 100644
--- a/sw/airborne/booz/arm7/booz2_imu_crista_hw.c
+++ b/sw/airborne/booz/arm7/booz2_imu_crista_hw.c
@@ -26,8 +26,7 @@
 #include "LPC21xx.h"
 #include "armVIC.h"
 #include CONFIG
-#include "led.h"
-#include "spi_hw.h"
+#include "ssp_hw.h"
 
 #define ADS8344_SS_IODIR IO0DIR
 #define ADS8344_SS_IOSET IO0SET
@@ -93,19 +92,17 @@ static inline void read_values( void ) {
 
 static inline void send_request( void ) {
   uint8_t control = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE;
-
-  SSPDR = control;
-  SSPDR = 0;
-  SSPDR = 0;
-  SSPDR = 0;
+  SSP_Send(control);
+  SSP_Send(0);
+  SSP_Send(0);
+  SSP_Send(0);
 }
 
 void ADS8344_start( void ) {
-  LED_ON(2);
   ADS8344Select();
-  SpiClearRti();
-  SpiEnableRti();
-  SpiEnable();
+  SSP_ClearRti();
+  SSP_EnableRti();
+  SSP_Enable();
   send_request();
 }
 
@@ -116,14 +113,13 @@ void SPI1_ISR(void) {
  if (channel > 7-1) {
    channel = 0;
    ADS8344_available = TRUE;
-   LED_OFF(2); 
    ADS8344Unselect();    
  }
  else {
    send_request();
  }
 
- SpiClearRti();
+ SSP_ClearRti();
 
  VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
  ISR_EXIT();
diff --git a/sw/airborne/booz/booz2_imu.h b/sw/airborne/booz/booz2_imu.h
index ed5345313a..3fd6f208e1 100644
--- a/sw/airborne/booz/booz2_imu.h
+++ b/sw/airborne/booz/booz2_imu.h
@@ -26,7 +26,7 @@
 
 #include "pprz_algebra_int.h"
 
-#include BOOZ2_IMU_TYPE
+#include BOOZ2_IMU_TYPE_H
 
 struct BoozImu {
   struct Int32Rates gyro;
diff --git a/sw/airborne/booz/booz2_imu_crista.c b/sw/airborne/booz/booz2_imu_crista.c
index 57fd0bd5db..f75a00d524 100644
--- a/sw/airborne/booz/booz2_imu_crista.c
+++ b/sw/airborne/booz/booz2_imu_crista.c
@@ -21,6 +21,7 @@
  * Boston, MA 02111-1307, USA. 
  */
 
+#include "booz2_imu.h"
 #include "booz2_imu_crista.h"
 
 void booz2_imu_impl_init(void) {
@@ -29,10 +30,17 @@ void booz2_imu_impl_init(void) {
 
   booz2_imu_crista_hw_init();
 
+#ifdef USE_AMI601
+  ami601_init();
+#endif
+
 }
 
 void booz2_imu_periodic(void) {
 
   Booz2ImuCristaHwPeriodic();
+#ifdef USE_AMI601
+  RunOnceEvery(10, { ami601_read(); });
+#endif
 
 }
diff --git a/sw/airborne/booz/booz2_imu_crista.h b/sw/airborne/booz/booz2_imu_crista.h
index ffdf7c8247..bc344286aa 100644
--- a/sw/airborne/booz/booz2_imu_crista.h
+++ b/sw/airborne/booz/booz2_imu_crista.h
@@ -47,7 +47,25 @@ extern bool_t ADS8344_available;
       /* spare 3, temp 7 */						\
       _gyro_accel_handler();						\
     }									\
+    Booz2ImuMagEvent(_mag_handler);					\
   }
 
+#ifdef USE_AMI601
+#include "AMI601.h"
+#define foo_handler() {}
+#define Booz2ImuMagEvent(_mag_handler) {				\
+    AMI601Event(foo_handler);						\
+    if (ami601_status == AMI601_DATA_AVAILABLE) {			\
+      booz_imu.mag_unscaled.x = ami601_val[IMU_MAG_X_CHAN];		\
+      booz_imu.mag_unscaled.y = ami601_val[IMU_MAG_Y_CHAN];		\
+      booz_imu.mag_unscaled.z = ami601_val[IMU_MAG_Z_CHAN];		\
+      ami601_status = AMI601_IDLE;					\
+      _mag_handler();							\
+    }									\
+  }
+#else
+#define Booz2ImuMagEvent(_mag_handler) {}
+#endif
+
 #endif /* BOOZ2_IMU_CRISTA_H */
 
diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 180d6e6e46..b22409fd3c 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -88,7 +88,7 @@ STATIC_INLINE void booz2_main_init( void ) {
   sys_time_init();
   led_init();
   uart0_init();
-  i2c_init();
+  i2c0_init();
   actuators_init();
 #if defined USE_BOOZ2_SERVOS_DIRECT
   booz2_servos_direct_init();
@@ -106,7 +106,7 @@ STATIC_INLINE void booz2_main_init( void ) {
   booz2_imu_init();
 #ifdef USE_AMI601
   i2c1_init();
-  ami601_init();
+  //  ami601_init();
 #endif
 
   booz_fms_init();
diff --git a/sw/airborne/booz/booz_radio_control_spektrum.h b/sw/airborne/booz/booz_radio_control_spektrum.h
index f53ec80a3b..dc0fb77b6b 100644
--- a/sw/airborne/booz/booz_radio_control_spektrum.h
+++ b/sw/airborne/booz/booz_radio_control_spektrum.h
@@ -76,10 +76,11 @@ extern const int16_t rc_spk_throw[RADIO_CONTROL_NB_CHANNEL];
           radio_control.status = RADIO_CONTROL_OK;			\
 	  uint8_t i;							\
 	  for (i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) {			\
-	    radio_control.values[i]  = rc_spk_parser_buf[2*i]<<8;	\
-	    radio_control.values[i] += rc_spk_parser_buf[2*i+1];	\
-	    radio_control.values[i] &= 0x03FF;				\
-	    radio_control.values[i] -= 512;				\
+	    const int16_t tmp = (rc_spk_parser_buf[2*i]<<8) +		\
+	                         rc_spk_parser_buf[2*i+1];		\
+	    const int16_t chan = (tmp&0xFC00) >> 10;			\
+	    const int16_t val  = (tmp&0x03FF) - 512;			\
+	    radio_control.values[i] = val;				\
 	    radio_control.values[i] *= rc_spk_throw[i];			\
 	    if (i==RADIO_CONTROL_THROTTLE) {				\
 	      radio_control.values[i] += MAX_PPRZ;			\
diff --git a/sw/airborne/booz/booz_radio_control_spektrum_dx7se.h b/sw/airborne/booz/booz_radio_control_spektrum_dx7se.h
index 3f2bf47b19..5c8c772e13 100644
--- a/sw/airborne/booz/booz_radio_control_spektrum_dx7se.h
+++ b/sw/airborne/booz/booz_radio_control_spektrum_dx7se.h
@@ -41,6 +41,12 @@
                         MAX_PPRZ/MAX_SPK, \
                         MAX_PPRZ/MAX_SPK }
 
-
+/*
+  aileron 1
+  elevator 2
+  rudder 3
+  gear 4
+  throttle 5
+*/
 
 #endif /* BOOZ_RADIO_CONTROL_SPEKTRUM_DX7SE_H */
diff --git a/sw/airborne/booz/test/booz2_test_ami.c b/sw/airborne/booz/test/booz2_test_ami.c
index 2761bd7a3e..c2c61193b4 100644
--- a/sw/airborne/booz/test/booz2_test_ami.c
+++ b/sw/airborne/booz/test/booz2_test_ami.c
@@ -62,7 +62,7 @@ static inline void main_init( void ) {
   //  LED_ON(6);
   //  LED_ON(7);
 
-  uart1_init_tx();
+  uart1_init();
   
 #if 0
   /* set P0.11 and P0.14 to I2C1 */
diff --git a/sw/airborne/dpicco.c b/sw/airborne/dpicco.c
index 6b5bd80536..0ec312e11c 100644
--- a/sw/airborne/dpicco.c
+++ b/sw/airborne/dpicco.c
@@ -40,18 +40,18 @@ void dpicco_periodic( void ) {
       /* init first read */
       dpicco_status = DPICCO_MEASURING_RD;
       dpicco_i2c_done = FALSE;
-      i2c_receive(DPICCO_SLAVE_ADDR, 4, &dpicco_i2c_done);
+      i2c0_receive(DPICCO_SLAVE_ADDR, 4, &dpicco_i2c_done);
       
 //      LED_ON(2);
     }
     else if (dpicco_status == DPICCO_MEASURING_RD) {
       /* get data */
-      dpicco_val[0] = (i2c_buf[0]<<8) | i2c_buf[1];
-      dpicco_val[1] = (i2c_buf[2]<<8) | i2c_buf[3];
+      dpicco_val[0] = (i2c0_buf[0]<<8) | i2c0_buf[1];
+      dpicco_val[1] = (i2c0_buf[2]<<8) | i2c0_buf[3];
       
       /* start next one right away */
       dpicco_i2c_done = FALSE;
-      i2c_receive(DPICCO_SLAVE_ADDR, 4, &dpicco_i2c_done);
+      i2c0_receive(DPICCO_SLAVE_ADDR, 4, &dpicco_i2c_done);
       
 //      LED_TOGGLE(2);
     }
diff --git a/sw/airborne/enose.c b/sw/airborne/enose.c
index 4f8d833172..3eda06d965 100644
--- a/sw/airborne/enose.c
+++ b/sw/airborne/enose.c
@@ -50,31 +50,31 @@ void enose_periodic( void ) {
   if (enose_i2c_done) {
     if (enose_conf_requested) {
       const uint8_t msg[] = { ENOSE_PWM_ADDR, enose_heat[0], enose_heat[1], enose_heat[2] };
-      memcpy((void*)i2c_buf, msg, sizeof(msg));
-      i2c_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done);
+      memcpy((void*)i2c0_buf, msg, sizeof(msg));
+      i2c0_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done);
       enose_i2c_done = FALSE;
       enose_conf_requested = FALSE;
     }
     else if (enose_status == ENOSE_IDLE) {
       enose_status = ENOSE_MEASURING_WR;
       const uint8_t msg[] = { ENOSE_DATA_ADDR };  
-      memcpy((void*)i2c_buf, msg, sizeof(msg));
-      i2c_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done);
+      memcpy((void*)i2c0_buf, msg, sizeof(msg));
+      i2c0_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done);
       enose_i2c_done = FALSE;
     }
     else if (enose_status == ENOSE_MEASURING_WR) {
       enose_status = ENOSE_MEASURING_RD;
-      i2c_receive(ENOSE_SLAVE_ADDR, 6, &enose_i2c_done);
+      i2c0_receive(ENOSE_SLAVE_ADDR, 6, &enose_i2c_done);
       enose_i2c_done = FALSE;
     }
     else if (enose_status == ENOSE_MEASURING_RD) {
-      uint16_t val = (i2c_buf[0]<<8) | i2c_buf[1];
+      uint16_t val = (i2c0_buf[0]<<8) | i2c0_buf[1];
       if (val < 5000) 
 	enose_val[0] = val;
-      val = (i2c_buf[2]<<8) | i2c_buf[3];
+      val = (i2c0_buf[2]<<8) | i2c0_buf[3];
       if (val < 5000) 
 	enose_val[1] = val;
-      val = (i2c_buf[4]<<8) | i2c_buf[5];
+      val = (i2c0_buf[4]<<8) | i2c0_buf[5];
       if (val < 5000) 
 	enose_val[2] = val;
       enose_status = ENOSE_IDLE;
diff --git a/sw/airborne/fms/Makefile b/sw/airborne/fms/Makefile
index 0ffa7c187c..4cfd2fa054 100644
--- a/sw/airborne/fms/Makefile
+++ b/sw/airborne/fms/Makefile
@@ -17,10 +17,28 @@ TT_LDFLAGS = -levent
 TT_CFLAGS += -DDOWNLINK
 TT_CFLAGS += -DDOWNLINK_TRANSPORT=UdpTransport
 TT_SRCS += fms_network.c udp_transport.c ../downlink.c
-test_telemetry: $(TT_SRCS)
-	$(CC) $(TT_CFLAGS) -o $@ $^ $(TT_LDFLAGS)
+#test_telemetry: $(TT_SRCS)
+#	$(CC) $(TT_CFLAGS) -o $@ $^ $(TT_LDFLAGS)
 
+# http://groups.google.com/group/beagleboard/browse_frm/thread/15d9488c1ec314ef/ca50640b9c51cef2
+# http://www.nabble.com/Overo-oe-SPI-questions-td22194463.html
+# http://docwiki.gumstix.org/Sample_code/C/SPI
+# http://www.nabble.com/SPI-Bus-td23307852.html
+# http://groups.google.com/group/beagleboard/browse_thread/thread/42988f0e14db0f01/3200cce7400bb6b8?lnk=gst&q=mcspi#3200cce7400bb6b8
+#<s4wrxttcs2> "export PATH=${PATH}:/home/gumstix/oe/tmp/staging/i686-linux/usr/bin"
+#<s4wrxttcs2> I just use "export PATH=${PATH}:/home/gumstix/oe/tmp/cross/armv7a/bin" and
+#<s4wrxttcs2> or "make ARCH=arm CROSS_COMPILE=arm-angstrom-linux-gnueabi-" if I have a makefile
 
+CROSS_CC=/home/poine/overo-oe/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-gcc
+
+CROSS_CFLAGS  = -Wall
+CROSS_LDFLAGS = 
+
+test_telemetry: test_telemetry.c
+	$(CROSS_CC) $(CROSS_CFLAGS) -o $@ $^ $(CROSS_LDFLAGS)
+
+test_spi: test_spi.c
+	$(CROSS_CC) $(CROSS_CFLAGS) -o $@ $^ $(CROSS_LDFLAGS)
 
 
 
diff --git a/sw/airborne/i2c.c b/sw/airborne/i2c.c
index e13ad6327b..100a11b08a 100644
--- a/sw/airborne/i2c.c
+++ b/sw/airborne/i2c.c
@@ -3,38 +3,40 @@
 
 #define I2C_RECEIVE     1
 
-volatile uint8_t i2c_status;
-volatile uint8_t i2c_buf[I2C_BUF_LEN];
-volatile uint8_t i2c_len;
-volatile uint8_t i2c_index;
-volatile uint8_t i2c_slave_addr;
+#ifdef USE_I2C0
 
-volatile bool_t* i2c_finished;
+volatile uint8_t i2c0_status;
+volatile uint8_t i2c0_buf[I2C0_BUF_LEN];
+volatile uint8_t i2c0_len;
+volatile uint8_t i2c0_index;
+volatile uint8_t i2c0_slave_addr;
 
-void i2c_init(void) {
-  i2c_status = I2C_IDLE;
+volatile bool_t* i2c0_finished;
+
+void i2c0_init(void) {
+  i2c0_status = I2C_IDLE;
   i2c0_hw_init();
-  i2c_finished = NULL;
+  i2c0_finished = NULL;
 }
 
 
-void i2c_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
-  i2c_len = len;
-  i2c_slave_addr = slave_addr | I2C_RECEIVE;
-  i2c_finished = finished;
-  i2c_status = I2C_BUSY;
-  I2cSendStart();
+void i2c0_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
+  i2c0_len = len;
+  i2c0_slave_addr = slave_addr | I2C_RECEIVE;
+  i2c0_finished = finished;
+  i2c0_status = I2C_BUSY;
+  I2c0SendStart();
 }
 
-void i2c_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
-  i2c_len = len;
-  i2c_slave_addr = slave_addr & ~I2C_RECEIVE;
-  i2c_finished = finished;
-  i2c_status = I2C_BUSY;
-  I2cSendStart();
+void i2c0_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
+  i2c0_len = len;
+  i2c0_slave_addr = slave_addr & ~I2C_RECEIVE;
+  i2c0_finished = finished;
+  i2c0_status = I2C_BUSY;
+  I2c0SendStart();
 }
 
-
+#endif /* USE_I2C0 */
 
 #ifdef USE_I2C1
 
@@ -69,7 +71,4 @@ void i2c1_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished) {
   I2c1SendStart();
 }
 
-
-
-
 #endif /* USE_I2C1 */
diff --git a/sw/airborne/i2c.h b/sw/airborne/i2c.h
index b3ad73a2c8..529483c902 100644
--- a/sw/airborne/i2c.h
+++ b/sw/airborne/i2c.h
@@ -3,13 +3,7 @@
 
 #include "std.h"
 
-//#ifndef SITL
 #include "i2c_hw.h"
-//#endif
-
-extern void i2c_init(void);
-extern void i2c_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
-extern void i2c_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
 
 #define I2C_START        0x08
 #define I2C_RESTART      0x10
@@ -25,67 +19,74 @@ extern void i2c_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finis
 #define I2C_IDLE 0
 #define I2C_BUSY 1
 
-extern volatile uint8_t i2c_status;
+#ifdef USE_I2C0
 
-#ifndef I2C_BUF_LEN
-#define I2C_BUF_LEN 16
+extern void i2c0_init(void);
+extern void i2c0_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
+extern void i2c0_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
+
+extern volatile uint8_t i2c0_status;
+
+#ifndef I2C0_BUF_LEN
+#define I2C0_BUF_LEN 16
 #endif
 
-extern volatile uint8_t i2c_buf[I2C_BUF_LEN];
-extern volatile uint8_t i2c_len;
-extern volatile uint8_t i2c_index;
-extern volatile uint8_t i2c_slave_addr;
+extern volatile uint8_t i2c0_buf[I2C0_BUF_LEN];
+extern volatile uint8_t i2c0_len;
+extern volatile uint8_t i2c0_index;
+extern volatile uint8_t i2c0_slave_addr;
 
-extern volatile bool_t* i2c_finished;
+extern volatile bool_t* i2c0_finished;
 
-#define I2cAutomaton(state) {						\
+#define I2c0Automaton(state) {						\
     switch (state) {							\
     case I2C_START:							\
     case I2C_RESTART:							\
-      I2cSendByte(i2c_slave_addr);					\
-      I2cClearStart();							\
-      i2c_index = 0;							\
+      I2c0SendByte(i2c0_slave_addr);					\
+      I2c0ClearStart();							\
+      i2c0_index = 0;							\
       break;								\
     case I2C_MR_DATA_ACK:						\
-      if (i2c_index < i2c_len) {					\
-	i2c_buf[i2c_index] = I2C_DATA_REG;				\
-	i2c_index++;							\
-	I2cReceive(i2c_index < i2c_len - 1);				\
+      if (i2c0_index < i2c0_len) {					\
+	i2c0_buf[i2c0_index] = I2C_DATA_REG;				\
+	i2c0_index++;							\
+	I2c0Receive(i2c0_index < i2c0_len - 1);				\
       }									\
       else {								\
 	/* error , we should have got NACK */				\
-	I2cSendStop();							\
+	I2c0SendStop();							\
       }									\
       break;								\
     case I2C_MR_SLA_ACK: /* At least one char */			\
       /* Wait and reply with ACK or NACK */				\
-      I2cReceive(i2c_index < i2c_len - 1);				\
+      I2c0Receive(i2c0_index < i2c0_len - 1);				\
       break;								\
     case I2C_MR_SLA_NACK:						\
     case I2C_MT_SLA_NACK:						\
-      I2cSendStart();							\
+      I2c0SendStart();							\
       break;								\
     case I2C_MT_SLA_ACK:						\
     case I2C_MT_DATA_ACK:						\
-      if (i2c_index < i2c_len) {					\
-	I2cSendByte(i2c_buf[i2c_index]);				\
-	i2c_index++;							\
+      if (i2c0_index < i2c0_len) {					\
+	I2c0SendByte(i2c0_buf[i2c0_index]);				\
+	i2c0_index++;							\
       } else {								\
-	I2cSendStop();							\
+	I2c0SendStop();							\
       }									\
       break;								\
     case I2C_MR_DATA_NACK:						\
-      if (i2c_index < i2c_len) {					\
-	i2c_buf[i2c_index] = I2C_DATA_REG;				\
+      if (i2c0_index < i2c0_len) {					\
+	i2c0_buf[i2c0_index] = I2C_DATA_REG;				\
       }									\
-      I2cSendStop();							\
+      I2c0SendStop();							\
       break;								\
     default:								\
-      I2cSendStop();							\
-      /* LED_ON(2); FIXME log error */					\
+      I2c0SendStop();							\
+      /* FIXME log error */						\
     }									\
   }									\
-   
+
+#endif /* USE_I2C0 */   
 
 #ifdef USE_I2C1
 
diff --git a/sw/airborne/srf08.c b/sw/airborne/srf08.c
index 9885313666..440d8aeb03 100644
--- a/sw/airborne/srf08.c
+++ b/sw/airborne/srf08.c
@@ -43,51 +43,51 @@ static bool_t dummy_bool;
 void srf08_init(void)
 {
   unsigned int range=0;
-  i2c_init();
+  i2c0_init();
   I2C_START_TX(address);
-  i2c_transmit(0);                    
-  i2c_transmit(0x51);
+  i2c0_transmit(0);                    
+  i2c0_transmit(0x51);
        
   do{
-    i2c_start();
+    i2c0_start();
     range=i2c_sla(address);               
-    i2c_stop();
+    i2c0_stop();
   } while(range != I2C_NO_ERROR);  /** !!!!!!!!!!!!  WARNING : blocking wait */
 
 
   /** Setting the gain to the minimun value (to avoid echos ?) */
-  i2c_buf[0]=SRF08_SET_GAIN;
-  i2c_buf[1]=SRF08_MIN_GAIN;
-  i2c_send(address, 2, &dummy_bool);
+  i2c0_buf[0]=SRF08_SET_GAIN;
+  i2c0_buf[1]=SRF08_MIN_GAIN;
+  i2c0_send(address, 2, &dummy_bool);
 
   return;
 }
 /*###########################################################################*/
 
 void srf08_initiate_ranging(void) {
-  i2c_buf[0]=SRF08_COMMAND;
-  i2c_buf[1]=SRF08_CENTIMETERS;
-  i2c_send(address, 2, &dummy_bool);
+  i2c0_buf[0]=SRF08_COMMAND;
+  i2c0_buf[1]=SRF08_CENTIMETERS;
+  i2c0_send(address, 2, &dummy_bool);
 }
 
 bool_t srf08_received, srf08_got;
 
 /** Ask the value to the device */
 void srf08_receive(void) {
-  i2c_buf[0]=SRF08_ECHO_1;
+  i2c0_buf[0]=SRF08_ECHO_1;
   srf08_received = FALSE;
-  i2c_send(address, 1, &srf08_received);
+  i2c0_send(address, 1, &srf08_received);
 }
 
 /** Read values on the bus */
 void srf08_read(void) {
   srf08_got = FALSE;
-  i2c_get(address, 2, &srf08_got);
+  i2c0_get(address, 2, &srf08_got);
 }
 
 /** Copy the I2C buffer */
 void srf08_copy(void) {
-  srf08_range = i2c_buf[0] << 8 | i2c_buf[1];
+  srf08_range = i2c0_buf[0] << 8 | i2c0_buf[1];
 }
 
 void srf08_ping()
@@ -95,12 +95,12 @@ void srf08_ping()
   uint8_t byte;
 
   srf08_initiate_ranging();
-  while (!i2c_idle);
+  while (!i2c0_idle);
        
   do {
-    i2c_start();
+    i2c0_start();
     byte=i2c_sla(address);               
-    i2c_stop();
+    i2c0_stop();
   } while(byte != I2C_NO_ERROR);
        
 
@@ -117,18 +117,18 @@ unsigned int srf08_read_register(unsigned char srf08_register)
 
 
   I2C_START_TX(address);
-  i2c_transmit(srf08_register);
+  i2c0_transmit(srf08_register);
   I2C_START_RX(address);
        
   /* get high byte msb first */ 
   if(srf08_register>=2) {
-    i2c.rx_byte[1]=i2c_receive(I2C_CONTINUE);
+    i2c.rx_byte[1]=i2c0_receive(I2C_CONTINUE);
   }                         
        
   /* get low byte msb first  */ 
-  i2c.rx_byte[0]=i2c_receive(I2C_QUIT);                          
+  i2c.rx_byte[0]=i2c0_receive(I2C_QUIT);                          
 
-  i2c_stop();
+  i2c0_stop();
 
   return(i2c.rx_word);
 }
diff --git a/sw/airborne/stm32/init_hw.h b/sw/airborne/stm32/init_hw.h
index 515e081c9a..c5bc02ccce 100644
--- a/sw/airborne/stm32/init_hw.h
+++ b/sw/airborne/stm32/init_hw.h
@@ -34,6 +34,8 @@
 #include CONFIG
 #include <stm32/gpio.h>
 #include <stm32/rcc.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
 
 
 #ifdef PERIPHERALS_AUTO_INIT