diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml
index a9edbe90a3..bbfb74c32d 100644
--- a/conf/airframes/booz.xml
+++ b/conf/airframes/booz.xml
@@ -35,91 +35,7 @@
 
   <makefile>
 
-ARCHI=arm7
-
-FLASH_MODE = IAP
-
-
-#
-# controller CPU
-#
-
-ctl.ARCHDIR = $(ARCHI)
-ctl.ARCH = arm7tdmi
-ctl.TARGET = ctl
-ctl.TARGETDIR = ctl
-
-ctl.CFLAGS += -DCONTROLLER -DCONFIG=\"conf_booz.h\"
-ctl.srcs = booz_controller_main.c
-
-ctl.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)' -DTIME_LED=1
-ctl.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-ctl.CFLAGS += -DLED
-
-ctl.srcs += $(SRC_ARCH)/armVIC.c
-
-ctl.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-ctl.srcs += $(SRC_ARCH)/uart_hw.c
-
-ctl.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 
-ctl.srcs += downlink.c pprz_transport.c 
-
-ctl.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-ctl.srcs += booz_datalink.c
-
-ctl.CFLAGS += -DCONTROLLER -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DSPI_SELECT_SLAVE0_PIN=20 -DSPI_SELECT_SLAVE0_PORT=0 -DSSPCPSR_VAL=0x0C
-ctl.srcs += spi.c $(SRC_ARCH)/spi_hw.c link_imu.c
-
-ctl.srcs += booz_estimator.c booz_control.c booz_autopilot.c
-
-#ctl.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING
-#ctl.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
-
-ctl.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
-ctl.srcs += $(SRC_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
-ctl.CFLAGS += -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
-ctl.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DRC_LED=4
-ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
-
-ctl.srcs += commands.c
-
-ctl.srcs += booz_telemetry.c
-
-
-#
-# FILTER CPU
-#
-
-flt.ARCHDIR = $(ARCHI)
-flt.ARCH = arm7tdmi
-flt.TARGET = flt
-flt.TARGETDIR = flt
-
-flt.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\"  -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)'
-flt.srcs = booz_filter_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-flt.CFLAGS += -DLED
-
-flt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-flt.srcs += $(SRC_ARCH)/uart_hw.c
-
-flt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 
-flt.srcs += downlink.c pprz_transport.c booz_filter_telemetry.c
-
-flt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
-flt.srcs += $(SRC_ARCH)/adc_hw.c
-
-flt.srcs += $(SRC_ARCH)/max1167.c
-
-flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
-
-flt.srcs += multitilt.c
-
-flt.CFLAGS += -DIMU
-flt.srcs += link_imu.c
+include $(PAPARAZZI_SRC)/conf/autopilot/conf_booz.makefile
 
 
   </makefile>
diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml
index 244d7ad8af..b589ff6471 100644
--- a/conf/telemetry/booz.xml
+++ b/conf/telemetry/booz.xml
@@ -26,6 +26,7 @@
     <mode name="default">
       <message name="IMU_GYRO"              period=".017"/>
       <message name="IMU_GYRO_LP"           period=".017"/>
+      <message name="IMU_GYRO_RAW"          period=".25"/>
     </mode>
     <mode name="raw_sensors">
       <message name="IMU_GYRO_RAW"          period=".017"/>
diff --git a/sw/airborne/arm7/booz_link_mcu_hw.c b/sw/airborne/arm7/booz_link_mcu_hw.c
new file mode 100644
index 0000000000..2b58dd49b3
--- /dev/null
+++ b/sw/airborne/arm7/booz_link_mcu_hw.c
@@ -0,0 +1,134 @@
+#include "booz_link_mcu.h"
+
+#ifdef BOOZ_FILTER_MCU
+
+
+#include "armVIC.h"
+
+volatile uint8_t spi0_data_available;
+volatile uint8_t spi0_idx_buf;
+uint8_t* spi0_buffer_output = (uint8_t*)&inter_mcu_state;
+uint8_t* spi0_buffer_input = (uint8_t*)&booz_link_mcu_state_unused;
+
+
+
+void SPI0_ISR(void) __attribute__((naked));
+
+#define Spi0InitBuf() {						      \
+    spi0_idx_buf = 0;						      \
+    S0SPDR = spi0_buffer_output[0];				      \
+  }
+
+#define Spi0OneByte() {							\
+    /* FIXME : do something usefull with the status register reading */ \
+    uint8_t foo __attribute__((unused)) = S0SPSR;			\
+    spi0_buffer_input[spi0_idx_buf] = S0SPDR;				\
+    spi0_idx_buf++;							\
+    if (spi0_idx_buf < sizeof(inter_mcu_state)) {			\
+      S0SPDR = spi0_buffer_output[spi0_idx_buf];			\
+    }									\
+    else {								\
+      spi0_data_available = TRUE;					\
+      BoozLinkMcuSetUnavailable();						\
+    }									\
+  } 
+
+
+
+/*  */
+#define S0SPCR_SPIE  7  /* SPI enable */
+
+void booz_link_mcu_hw_init ( void ) {
+  /* init SPI0 */
+  /* setup pins for sck, miso, mosi, SSEL */
+  PINSEL0 |= 1<<8 | 1<<10| 1<<12 | 1<< 14;
+  /* setup P1_16 to P1_25 as GPIO */
+  PINSEL2 &= ~(1<<3);
+  /* P1_24 (DRDY) is output */
+  SetBit(IO1DIR, SPI0_DRDY);
+  /* DRDY idles high */
+  BoozLinkMcuSetUnavailable();
+  /* configure SPI : 8 bits CPOL=0 CPHA=0 MSB_first slave */
+  S0SPCR = _BV(3);
+  /* setup SPI clock rate */
+  S0SPCCR = 0x20;
+
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT(VIC_SPI0);   // SPI0 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_SPI0);     // SPI0 interrupt enabled
+  VICVectCntl1 = VIC_ENABLE | VIC_SPI0;
+  VICVectAddr1 = (uint32_t)SPI0_ISR;    // address of the ISR
+
+  /* clear pending interrupt */
+  SetBit(S0SPINT, SPI0IF);
+  /* enable SPI interrupt */
+  SetBit(S0SPCR, S0SPCR_SPIE);
+
+}
+
+
+void SPI0_ISR(void) {
+  ISR_ENTRY();
+  Spi0OneByte();
+  /* clear the interrupt */
+  S0SPINT = _BV(SPI0IF); 
+  /* clear this interrupt from the VIC */
+  VICVectAddr = 0x00000000;
+  ISR_EXIT();
+}
+
+
+
+#endif /* BOOZ_FILTER_MCU */
+
+#ifdef BOOZ_CONTROLLER_MCU  
+/* IMU connected to SSP ( aka SPI1 ) */
+
+#include "LPC21xx.h"
+#include "interrupt_hw.h" 
+
+/* DRDY connected pin to P0.16 EINT0 */ 
+#define LINK_IMU_DRDY_PINSEL PINSEL1
+#define LINK_IMU_DRDY_PINSEL_VAL 0x01
+#define LINK_IMU_DRDY_PINSEL_BIT 0
+#define LINK_IMU_DRDY_EINT 0
+
+static void EXTINT0_ISR(void) __attribute__((naked));
+
+void booz_link_mcu_hw_init ( void ) {
+
+
+  /* configure DRDY pin */
+  LINK_IMU_DRDY_PINSEL |= LINK_IMU_DRDY_PINSEL_VAL << LINK_IMU_DRDY_PINSEL_BIT;
+  SetBit(EXTMODE, LINK_IMU_DRDY_EINT);   /* EINT is edge trigered */
+  ClearBit(EXTPOLAR,LINK_IMU_DRDY_EINT); /* EINT is trigered on falling edge */
+  SetBit(EXTINT,LINK_IMU_DRDY_EINT);     /* clear pending EINT */
+  
+  /* initialize interrupt vector */
+  VICIntSelect &= ~VIC_BIT( VIC_EINT0 );  /* select EINT0 as IRQ source */
+  VICIntEnable = VIC_BIT( VIC_EINT0 );    /* enable it */
+  VICVectCntl9 = VIC_ENABLE | VIC_EINT0;
+  VICVectAddr9 = (uint32_t)EXTINT0_ISR;   // address of the ISR 
+
+  spi_buffer_input = (uint8_t*)&inter_mcu_state;
+  spi_buffer_output = (uint8_t*)&booz_link_mcu_state_unused;
+  spi_buffer_length = sizeof(inter_mcu_state);
+
+
+}
+
+
+void EXTINT0_ISR(void) {
+  ISR_ENTRY();
+
+  SpiSelectSlave0();
+  SpiStart();
+
+  /* clear EINT2 */
+  SetBit(EXTINT,LINK_IMU_DRDY_EINT); /* clear EINT0 */
+  
+  VICVectAddr = 0x00000000;    /* clear this interrupt from the VIC */
+  ISR_EXIT();
+}
+
+#endif /* BOOZ_CONTROLLER_MCU */
diff --git a/sw/airborne/arm7/booz_link_mcu_hw.h b/sw/airborne/arm7/booz_link_mcu_hw.h
new file mode 100644
index 0000000000..fdc7358c5a
--- /dev/null
+++ b/sw/airborne/arm7/booz_link_mcu_hw.h
@@ -0,0 +1,33 @@
+#ifndef BOOZ_LINK_MCU_HW_H
+#define BOOZ_LINK_MCU_HW_H
+
+#include "LPC21xx.h"
+
+/*
+  wiring on IMU_V3
+ P0_4   SCK0
+ P0_5   MISO0
+ P0_6   MOSI0
+ P0_7   SSEL0
+ P1_24  DRDY
+*/
+#define SPI0_DRDY 24
+
+extern volatile uint8_t spi0_data_available;
+extern volatile uint8_t spi0_idx_buf;
+extern uint8_t* spi0_buffer_output;
+//extern uint8_t* spi0_buffer_input;
+
+
+
+#define Spi0InitBuf() {						      \
+    spi0_idx_buf = 0;						      \
+    S0SPDR = spi0_buffer_output[0];				      \
+  }
+
+
+#define BoozLinkMcuSetUnavailable() { IO1SET = _BV(SPI0_DRDY); }
+#define BoozLinkMcuSetAvailable()   { IO1CLR = _BV(SPI0_DRDY); }
+
+
+#endif /* BOOZ_LINK_MCU_HW_H */
diff --git a/sw/airborne/arm7/imu_v3_hw.c b/sw/airborne/arm7/imu_v3_hw.c
index 649f2d1248..bc12f31a7d 100644
--- a/sw/airborne/arm7/imu_v3_hw.c
+++ b/sw/airborne/arm7/imu_v3_hw.c
@@ -4,9 +4,6 @@
 
 static void SPI1_ISR(void) __attribute__((naked));
 
-uint8_t spi_cur_slave;
-
-
 /* SSPCR0 settings */
 #define SSP_DDS  0x07 << 0  /* data size         : 8 bits        */
 #define SSP_FRF  0x00 << 4  /* frame format      : SPI           */
@@ -21,7 +18,6 @@ uint8_t spi_cur_slave;
 #define SSP_SOD  0x00 << 3  /* slave output disable : don't care when master */
 
 void imu_v3_hw_init(void) {
-  spi_cur_slave = SPI_SLAVE_NONE;  
 
   max1167_init();
 
diff --git a/sw/airborne/arm7/imu_v3_hw.h b/sw/airborne/arm7/imu_v3_hw.h
index e65228d44b..27953b8715 100644
--- a/sw/airborne/arm7/imu_v3_hw.h
+++ b/sw/airborne/arm7/imu_v3_hw.h
@@ -1,32 +1,5 @@
 #ifndef IMU_V3_HW_H
 #define IMU_V3_HW_H
 
-#include "max1167.h"
-
-extern void imu_v3_hw_init(void);
-
-#define SPI_SLAVE_NONE 0
-#define SPI_SLAVE_MAX  1
-extern uint8_t spi_cur_slave;
-
-#define ImuEventCheckAndHandle(user_handler) {	\
-    if (max1167_data_available) {		\
-      max1167_data_available = FALSE;		\
-      spi_cur_slave = SPI_SLAVE_NONE;		\
-      ImuUpdateGyros();				\
-      ImuUpdateAccels();			\
-      user_handler();				\
-    }						\
-  }
-
-#define ImuPeriodic() {				\
-    if (spi_cur_slave != SPI_SLAVE_NONE) {	\
-      DOWNLINK_SEND_AHRS_OVERRUN();		\
-    }						\
-    else {					\
-      spi_cur_slave = SPI_SLAVE_MAX;		\
-      max1167_read();				\
-    }						\
-  }
 
 #endif /* IMU_V3_HW_H */
diff --git a/sw/airborne/arm7/link_mcu_hw.h b/sw/airborne/arm7/link_mcu_hw.h
index e2703513b0..6a4feaefb4 100644
--- a/sw/airborne/arm7/link_mcu_hw.h
+++ b/sw/airborne/arm7/link_mcu_hw.h
@@ -28,16 +28,16 @@
 #ifndef LINK_MCU_HW_H
 #define LINK_MCU_HW_H
 
+#ifdef BOOZ_FILTER_MCU
+
+#include "LPC21xx.h"
+#define BoozLinkMcuSetUnavailable() { IO1SET = _BV(SPI0_DRDY); }
+#define BoozLinkMcuSetAvailable()   { IO1CLR = _BV(SPI0_DRDY); }
+
+#endif /* BOOZ_FILTER_MCU */
+
+
 
-#define CRC_INIT 0x0
-#define CrcLow(x) ((x)&0xff)
-#define CrcHigh(x) ((x)>>8)
 
-static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) {
-  uint8_t a = ((uint8_t)CrcHigh(crc)) + data; 
-  uint8_t b = ((uint8_t)CrcLow(crc)) + a;
-  crc = b | a << 8; 
-  return crc;
-}
 
 #endif /* LINK_MCU_HW_H */
diff --git a/sw/airborne/booz_controller_main.c b/sw/airborne/booz_controller_main.c
index 66902e30db..aa101bcaaa 100644
--- a/sw/airborne/booz_controller_main.c
+++ b/sw/airborne/booz_controller_main.c
@@ -12,7 +12,8 @@
 #include "radio_control.h"
 
 #include "spi.h"
-#include "link_imu.h"
+#include "booz_link_mcu.h"
+
 #include "booz_estimator.h"
 #include "booz_control.h"
 #include "booz_autopilot.h"
@@ -30,12 +31,6 @@ int16_t trim_q = 0;
 int16_t trim_r = 0;
 uint8_t vbat = 0;
 
-//FIXME
-#ifdef SITL
-uint32_t link_imu_nb_err;
-uint8_t  link_imu_status;
-#endif
-
 #ifndef SITL
 int main( void ) {
   booz_controller_main_init();
@@ -61,10 +56,8 @@ STATIC_INLINE void booz_controller_main_init( void ) {
   ppm_init();
   radio_control_init();
 
-#ifndef SITL
   spi_init();
-  link_imu_init();
-#endif
+  booz_link_mcu_init();
   
   booz_estimator_init();
   booz_control_init();
@@ -82,11 +75,9 @@ STATIC_INLINE void booz_controller_main_init( void ) {
 
 STATIC_INLINE void booz_controller_main_periodic_task( void ) {
   
-  // FIXME
-#ifndef SITL
-  link_imu_periodic_task();
-#endif
-  
+  /* check for timeout */
+  booz_link_mcu_periodic_task();
+  /* run control loops */
   booz_autopilot_periodic_task();
 
   SetActuatorsFromCommands(commands);
@@ -119,10 +110,10 @@ STATIC_INLINE void booz_controller_main_event_task( void ) {
   // FIXME
 #ifndef SITL
   DlEventCheckAndHandle();
-
-  LinkImuEventCheckAndHandle();
 #endif
 
+  BoozLinkMcuEventCheckAndHandle();
+
   RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event);
  
 }
diff --git a/sw/airborne/booz_estimator.c b/sw/airborne/booz_estimator.c
index 09ce793f10..11cf9dcaf2 100644
--- a/sw/airborne/booz_estimator.c
+++ b/sw/airborne/booz_estimator.c
@@ -1,5 +1,8 @@
 #include "booz_estimator.h"
 
+#include "booz_inter_mcu.h"
+
+
 float booz_estimator_uf_p;
 float booz_estimator_uf_q;
 float booz_estimator_uf_r;
@@ -27,3 +30,17 @@ void booz_estimator_init( void ) {
   booz_estimator_psi = 0.;
 
 }
+
+void booz_estimator_read_inter_mcu_state( void ) {
+
+    booz_estimator_uf_p  = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
+    booz_estimator_uf_q  = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
+    booz_estimator_uf_r  = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
+    booz_estimator_p     = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
+    booz_estimator_q     = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
+    booz_estimator_r     = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
+    booz_estimator_phi   = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
+    booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
+    booz_estimator_psi   = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
+
+}
diff --git a/sw/airborne/booz_estimator.h b/sw/airborne/booz_estimator.h
index 0ea85c61b5..6d541677e3 100644
--- a/sw/airborne/booz_estimator.h
+++ b/sw/airborne/booz_estimator.h
@@ -14,5 +14,6 @@ extern float booz_estimator_theta;
 extern float booz_estimator_psi;
 
 extern void booz_estimator_init( void );
+extern void booz_estimator_read_inter_mcu_state( void );
 
 #endif /* BOOZ_ESTIMATOR_H */
diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c
index e42cd736ec..e3f795e561 100644
--- a/sw/airborne/booz_filter_main.c
+++ b/sw/airborne/booz_filter_main.c
@@ -14,7 +14,7 @@
 #include "downlink.h"
 #include "booz_filter_telemetry.h"
 
-#include "link_imu.h"
+#include "booz_link_mcu.h"
 
 static inline void on_imu_event( void );
 
@@ -43,22 +43,22 @@ STATIC_INLINE void booz_filter_main_init( void ) {
   imu_v3_init();
 
   multitilt_init();
-  //FIXME
-#ifndef SITL
-  link_imu_init();
-#endif
+
+  booz_link_mcu_init();
+
   int_enable();
 }
 
 // static uint32_t t0, t1, diff;
 
 STATIC_INLINE void booz_filter_main_event_task( void ) {
-  
+  /* check if measurements are available */
   ImuEventCheckAndHandle(on_imu_event);
 
 }
 
 STATIC_INLINE void booz_filter_main_periodic_task( void ) {
+  /* triger measurements */
   ImuPeriodic();
   static uint8_t _50hz = 0;
   _50hz++;
@@ -92,7 +92,5 @@ static inline void on_imu_event( void ) {
     break;
   }
   
-#ifndef SITL
-  link_imu_send();
-#endif
+  booz_link_mcu_send();
 }
diff --git a/sw/airborne/booz_link_mcu.c b/sw/airborne/booz_link_mcu.c
new file mode 100644
index 0000000000..7b7902c0e5
--- /dev/null
+++ b/sw/airborne/booz_link_mcu.c
@@ -0,0 +1,95 @@
+#include "booz_link_mcu.h"
+
+#include "std.h"
+
+struct booz_inter_mcu_state booz_link_mcu_state_unused;
+
+uint16_t booz_link_mcu_crc;
+
+#define BOOZ_LINK_MCU_CRC_INIT 0x0
+#define BOOZ_LINK_MCU_PAYLOAD_LENGTH (sizeof(struct booz_inter_mcu_state) - 2)
+#define BoozLinkMcuCrcLow(x) (((x)&0xff))
+#define BoozLinkMcuCrcHigh(x) (((x)>>8))
+#define BoozLinkMcuComputeCRC() {					\
+    uint8_t i;								\
+    booz_link_mcu_crc = BOOZ_LINK_MCU_CRC_INIT;				\
+    for(i = 0; i < BOOZ_LINK_MCU_PAYLOAD_LENGTH; i++) {			\
+      uint8_t _byte = ((uint8_t*)&inter_mcu_state)[i];			\
+      uint8_t a = ((uint8_t)BoozLinkMcuCrcHigh(booz_link_mcu_crc)) + _byte; \
+      uint8_t b = ((uint8_t)BoozLinkMcuCrcLow(booz_link_mcu_crc)) + a;	\
+      booz_link_mcu_crc = b | a << 8;					\
+    }									\
+  }
+
+
+#ifdef BOOZ_FILTER_MCU  /* FILTER LPC code */
+
+#include "imu_v3.h"
+#include "multitilt.h"
+
+/* FIXME!!!! two function with same name in single MCU configuration */
+/* by chance  booz_link_mcu_hw_init does nothing in sim              */
+#ifndef SITL
+void booz_link_mcu_init ( void ) {
+  booz_link_mcu_hw_init();
+}
+#endif
+
+void booz_link_mcu_send ( void ) {
+  BoozLinkMcuSetUnavailable();
+  inter_mcu_fill_state();
+  BoozLinkMcuComputeCRC();
+  inter_mcu_state.crc = booz_link_mcu_crc;
+  Spi0InitBuf();
+  BoozLinkMcuSetAvailable();
+}
+
+
+
+#endif /* BOOZ_FILTER_MCU */
+
+
+
+
+
+#ifdef BOOZ_CONTROLLER_MCU  /* lpc controller board */
+
+#include "spi.h"
+
+#include "booz_estimator.h"
+
+uint32_t booz_link_mcu_nb_err;
+uint8_t  booz_link_mcu_status;
+uint32_t booz_link_mcu_timeout;
+#define BOOZ_LINK_MCU_TIMEOUT 100
+
+void booz_link_mcu_init ( void ) {
+
+  booz_link_mcu_hw_init();
+
+  booz_link_mcu_nb_err = 0;
+  booz_link_mcu_status = IMU_NO_LINK;
+  booz_link_mcu_timeout = BOOZ_LINK_MCU_TIMEOUT;
+
+}
+
+void booz_link_mcu_event_task( void ) {
+  BoozLinkMcuComputeCRC();
+  if (booz_link_mcu_crc == inter_mcu_state.crc) {
+    booz_link_mcu_timeout = 0;
+    booz_link_mcu_status = inter_mcu_state.status;
+    booz_estimator_read_inter_mcu_state();
+  }
+  else {
+    booz_link_mcu_nb_err++;
+  }
+}
+
+extern void booz_link_mcu_periodic_task( void ) {
+  if (booz_link_mcu_timeout < BOOZ_LINK_MCU_TIMEOUT)
+    booz_link_mcu_timeout++;
+  else
+    booz_link_mcu_status = IMU_NO_LINK;
+}
+
+#endif /* >BOOZ_CONTROLLER_MCU */
diff --git a/sw/airborne/booz_link_mcu.h b/sw/airborne/booz_link_mcu.h
new file mode 100644
index 0000000000..c735cbfe39
--- /dev/null
+++ b/sw/airborne/booz_link_mcu.h
@@ -0,0 +1,53 @@
+#ifndef BOOZ_LINK_LINK_MCU_H
+#define BOOZ_LINK_LINK_MCU_H
+
+#include <inttypes.h>
+
+#include "6dof.h"
+
+#include "booz_link_mcu_hw.h"
+
+#include "booz_inter_mcu.h"
+#include "spi.h"
+
+extern void booz_link_mcu_init ( void );
+extern void booz_link_mcu_hw_init ( void );
+extern struct booz_inter_mcu_state booz_link_mcu_state_unused; /* single dir only */
+
+#define CRC_INIT 0x0
+#define CrcLow(x) ((x)&0xff)
+#define CrcHigh(x) ((x)>>8)
+
+static inline uint16_t BoozLinkMcuCrcUpdate(uint16_t crc, uint8_t data) {
+  uint8_t a = ((uint8_t)CrcHigh(crc)) + data; 
+  uint8_t b = ((uint8_t)CrcLow(crc)) + a;
+  crc = b | a << 8; 
+  return crc;
+}
+
+
+#ifdef BOOZ_FILTER_MCU
+
+extern void booz_link_mcu_send( void );
+
+#endif /* BOOZ_FILTER_MCU */
+
+
+#ifdef BOOZ_CONTROLLER_MCU
+
+extern uint32_t booz_link_mcu_nb_err;
+extern uint8_t  booz_link_mcu_status;
+extern void booz_link_mcu_event_task( void );
+extern void booz_link_mcu_periodic_task( void );
+
+#define BoozLinkMcuEventCheckAndHandle() { \
+    if (spi_message_received) {		   \
+      spi_message_received = FALSE;	   \
+      booz_link_mcu_event_task();	   \
+    }					   \
+  }					   \
+
+#endif /* BOOZ_CONTROLLER_MCU */
+
+
+#endif /* BOOZ_LINK_LINK_MCU_H */
diff --git a/sw/airborne/booz_telemetry.h b/sw/airborne/booz_telemetry.h
index 0962f7b512..4de9f1b402 100644
--- a/sw/airborne/booz_telemetry.h
+++ b/sw/airborne/booz_telemetry.h
@@ -8,7 +8,7 @@
 
 #include "radio_control.h"
 #include "actuators.h"
-#include "link_imu.h"
+#include "booz_link_mcu.h"
 #include "booz_estimator.h"
 #include "booz_autopilot.h"
 #include "booz_control.h"
@@ -19,16 +19,40 @@
 
 #include "downlink.h"
 
-#define PERIODIC_SEND_BOOZ_STATUS() DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &link_imu_status, &rc_status, &booz_autopilot_mode, &booz_autopilot_mode, &cpu_time_sec, &buss_twi_blmc_nb_err)
-#define PERIODIC_SEND_BOOZ_FD()     DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, &booz_estimator_q, &booz_estimator_r,\
-							  &booz_estimator_phi, &booz_estimator_theta, &booz_estimator_psi); 
-#define PERIODIC_SEND_BOOZ_DEBUG() DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, &booz_control_q_sp, &booz_control_r_sp, &booz_control_power_sp); 
+#define PERIODIC_SEND_BOOZ_STATUS()					\
+  DOWNLINK_SEND_BOOZ_STATUS(&booz_link_mcu_nb_err,			\
+			    &booz_link_mcu_status,			\
+			    &rc_status, \
+			    &booz_autopilot_mode, \
+			    &booz_autopilot_mode, \
+			    &cpu_time_sec, \
+			    &buss_twi_blmc_nb_err)
 
-#define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
+#define PERIODIC_SEND_BOOZ_FD()						\
+  DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p,				\
+			&booz_estimator_q,				\
+			&booz_estimator_r,				\
+			&booz_estimator_phi,				\
+			&booz_estimator_theta,				\
+			&booz_estimator_psi); 
 
-#define PERIODIC_SEND_BOOZ_RATE_LOOP() DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp, &booz_estimator_q, &booz_control_q_sp, &booz_estimator_r, &booz_control_r_sp ); 
+#define PERIODIC_SEND_BOOZ_DEBUG()					\
+  DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp,				\
+			   &booz_control_q_sp,				\
+			   &booz_control_r_sp,				\
+			   &booz_control_power_sp); 
 
-#define PERIODIC_SEND_BOOZ_ATT_LOOP() DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, &booz_estimator_theta, &booz_control_theta_sp); 
+#define PERIODIC_SEND_ACTUATORS()			\
+  DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
+
+#define PERIODIC_SEND_BOOZ_RATE_LOOP()					\
+  DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp,	\
+			       &booz_estimator_q, &booz_control_q_sp,	\
+			       &booz_estimator_r, &booz_control_r_sp ); 
+
+#define PERIODIC_SEND_BOOZ_ATT_LOOP()					\
+  DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, \
+			      &booz_estimator_theta, &booz_control_theta_sp); 
 
 #define PERIODIC_SEND_BOOZ_UF_RATES() \
   DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \
diff --git a/sw/airborne/control_grz.c b/sw/airborne/control_grz.c
deleted file mode 100644
index c4f94ad031..0000000000
--- a/sw/airborne/control_grz.c
+++ /dev/null
@@ -1,320 +0,0 @@
-/*
- * Paparazzi $Id$
- *  
- * Copyright (C) 2006 Pascal Brisset, 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. 
- *
- */
-
-#include "std.h"
-#include "airframe.h"
-#include "control_grz.h"
-#include "link_imu.h"
-#include "paparazzi.h"
-#include "radio_control.h"
-#include "commands.h"
-#include "estimator.h"
-#include "fbw_downlink.h"
-
-#define NORMALIZE(n, lower, uper) {		\
-    while ( n > uper)  { n -= (uper - lower);}	\
-    while ( n < lower) { n += (uper - lower);}	\
-}
-
-
-float trim_roll = -450;
-float trim_yaw = -250;
-float trim_pitch = 0;
-
-float max_roll_dot_sp  = (1.5/MAX_PPRZ);
-float max_pitch_dot_sp = (1.5/MAX_PPRZ);
-float max_yaw_dot_sp   = (1.5/MAX_PPRZ);
-
-float max_roll_sp  = (0.4/MAX_PPRZ);
-float max_pitch_sp = (0.4/MAX_PPRZ);
-/* yaw is incremented acording to stick position */
-#define UPDATE_SP_DT 0.016384
-float max_yaw_sp   = (0.8/MAX_PPRZ*UPDATE_SP_DT);
-
-float max_v_sp   = (6./MAX_PPRZ);
-
-static float throttle_setpoint = 0.;
-
-float max_z_dot_sp =  (5./MAX_PPRZ);
-float ctl_grz_z_dot_pgain = -2000;
-float ctl_grz_z_dot_igain = 3.;
-float ctl_grz_z_dot_dgain = 8.;
-float ctl_grz_z_dot = 0;
-float ctl_grz_z_dot_setpoint = 0.;
-float ctl_grz_z_dot_sum_err = 0.;
-float ctl_grz_throttle_level = 6700.;
-
-
-float max_z_sp =  (4./MAX_PPRZ);
-float ctl_grz_z_pgain = -1.2;
-float ctl_grz_z_igain = 0.;
-float ctl_grz_z_dgain = 5.;
-float ctl_grz_z = 0;
-float ctl_grz_z_setpoint = 0.;
-float ctl_grz_z_sum_err = 0.;
-
-
-bool_t flying = FALSE;
-
-#define MIN_Z_DOT -1.
-#define MAX_Z_DOT 1.
-
-struct pid roll_pid;
-struct pid pitch_pid;
-struct pid yaw_pid;
-
-struct pid roll_dot_pid;
-struct pid pitch_dot_pid;
-struct pid yaw_dot_pid;
-
-struct pid vn_pid;
-struct pid ve_pid;
-
-void ctl_grz_init( void ) {
-
-  roll_dot_pid.p_gain = 4700.;
-  roll_dot_pid.i_gain = 1.;
-  roll_dot_pid.d_gain = 1.875;
-
-  pitch_dot_pid.p_gain = -4700.;
-  pitch_dot_pid.i_gain = 1.;
-  pitch_dot_pid.d_gain = 1.875;
-
-  yaw_dot_pid.p_gain = -4700.;
-  yaw_dot_pid.i_gain = 1.;
-  yaw_dot_pid.d_gain = 2.;
-
-  roll_pid.p_gain = -5;
-  roll_pid.i_gain = 0.;
-  roll_pid.d_gain = 1.;
-  roll_pid.saturation = 3.;
-
-  pitch_pid.p_gain = -5;
-  pitch_pid.i_gain = 0.;
-  pitch_pid.d_gain = 1.;
-  pitch_pid.saturation = 3.;
-
-  yaw_pid.p_gain = -1.;
-  yaw_pid.i_gain = 0.;
-  yaw_pid.d_gain = 0.;
-  yaw_pid.saturation = 2.;
-  yaw_pid.set_point = 0.; // RadOfDeg(-20);
-
-  vn_pid.p_gain = -0.1;
-  vn_pid.i_gain = 0.;
-  vn_pid.d_gain = 0.;
-  vn_pid.saturation = .2;
-
-  ve_pid.p_gain = -0.2;
-  ve_pid.i_gain = 1.;
-  ve_pid.d_gain = 1.;
-  ve_pid.saturation = .2;
-}
-
-/** FIXME: should be done by SetCommandsFromRC(commands); */
-void ctl_grz_set_setpoints_rate( void ) {
-  roll_dot_pid.set_point = - commands[COMMAND_ROLL_DOT] * max_roll_dot_sp;
-  pitch_dot_pid.set_point = commands[COMMAND_PITCH_DOT] * max_pitch_dot_sp;
-  yaw_dot_pid.set_point = - commands[COMMAND_YAW_DOT] * max_yaw_dot_sp;
-  throttle_setpoint = commands[COMMAND_Z];
-}
-
-
-#define YAW_GAP ((int16_t)(0.05*MAX_PPRZ))
-void ctl_grz_set_setpoints_auto1( void ) {
-  roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp;
-  pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp;
-  #if 0
-  //  yaw_dot_pid.set_point = - rc_values[RADIO_YAW] * max_yaw_dot_sp;
-  /* yaw stick increments yaw setpoint */
-  if (rc_values[RADIO_YAW] > YAW_GAP || rc_values[RADIO_YAW] < -YAW_GAP) {
-    yaw_pid.set_point += - rc_values[RADIO_YAW] * max_yaw_dot_sp;
-    NORMALIZE(yaw_pid.set_point, -M_PI, M_PI);  
-  }
-  #endif
-  //  ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
-  throttle_setpoint = rc_values[RADIO_THROTTLE];
-  //  ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
-}
-
-void ctl_grz_set_setpoints_auto2( void ) {
-  roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp;
-  pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp;
-
-/*   float vright = - rc_values[RADIO_ROLL] * max_v_sp; */
-/*   float vfront = - rc_values[RADIO_PITCH] * max_v_sp; */
-  
-/*   float alpha = M_PI/2. - yaw_pid.measure; */
-/*   float cos_alpha = cos(alpha); */
-/*   float sin_alpha = sin(alpha); */
-/*   ve_pid.set_point = sin_alpha*vright + cos_alpha*vfront; */
-/*   vn_pid.set_point = - cos_alpha*vright + sin_alpha*vfront; */
- 
-  ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp; 
-  //  ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
-  // throttle_setpoint = rc_values[RADIO_THROTTLE];
-}
-
-void ctl_grz_set_measures( void ) {
-  roll_dot_pid.measure = link_imu_state.rates[AXIS_X];
-  pitch_dot_pid.measure = link_imu_state.rates[AXIS_Y];
-  yaw_dot_pid.measure = link_imu_state.rates[AXIS_Z];
-
-  roll_pid.measure = link_imu_state.eulers[AXIS_X];
-  pitch_pid.measure = link_imu_state.eulers[AXIS_Y];
-  yaw_pid.measure = link_imu_state.eulers[AXIS_Z];
-
-  ve_pid.measure = estimator_hspeed_mod*sin(estimator_hspeed_dir);
-  vn_pid.measure = estimator_hspeed_mod*cos(estimator_hspeed_dir);
-}
-
-
-void ctl_grz_rate_run ( void ) {
-  float err = roll_dot_pid.measure - roll_dot_pid.set_point;
-  float d_err = err - roll_dot_pid.last_err;
-  roll_dot_pid.last_err = err;
-  if (flying) {
-    roll_dot_pid.sum_err += err/100;
-    Bound(roll_dot_pid.sum_err, -0.2, 0.2);
-  } else
-    roll_dot_pid.sum_err = 0.;
-
-  commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_pid.d_gain + roll_dot_pid.sum_err * roll_dot_pid.i_gain) * roll_dot_pid.p_gain));
-
-  err = pitch_dot_pid.measure - pitch_dot_pid.set_point;
-  d_err = err - pitch_dot_pid.last_err;
-  pitch_dot_pid.last_err = err;
-  if (flying) {
-    pitch_dot_pid.sum_err += err/100;
-    Bound(pitch_dot_pid.sum_err, -0.2, 0.2);
-  } else
-    pitch_dot_pid.sum_err = 0;
-
-  commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_pid.d_gain + pitch_dot_pid.sum_err * pitch_dot_pid.i_gain) * pitch_dot_pid.p_gain));
-  
-  err = yaw_dot_pid.measure - yaw_dot_pid.set_point;
-  d_err = err - yaw_dot_pid.last_err;
-  yaw_dot_pid.last_err = err;
-  if (flying) {
-    yaw_dot_pid.sum_err += err/100;
-    Bound(yaw_dot_pid.sum_err, -0.2, 0.2);
-  } else
-    yaw_dot_pid.sum_err = 0;
-  commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)((err + d_err * yaw_dot_pid.d_gain  + yaw_dot_pid.sum_err * yaw_dot_pid.i_gain) * yaw_dot_pid.p_gain));
-
-  commands[COMMAND_THROTTLE] = throttle_setpoint;
-}
-
-
-
-void ctl_grz_attitude_run ( void ) {
-  float err = roll_pid.measure - roll_pid.set_point;
-  float d_err = err - roll_pid.last_err;
-  roll_pid.last_err = err;
-  roll_dot_pid.set_point = (err + d_err * roll_pid.d_gain) * roll_pid.p_gain; 
-  //  Bound(roll_dot_pid.set_point, roll_pid.saturation, -roll_pid.saturation);
-
-  err = pitch_pid.measure - pitch_pid.set_point;
-  d_err = err - pitch_pid.last_err;
-  pitch_pid.last_err = err;
-  pitch_dot_pid.set_point = (err + d_err * pitch_pid.d_gain) * pitch_pid.p_gain; 
-  //  Bound(pitch_dot_pid.set_point, pitch_pid.saturation, -pitch_pid.saturation);
-  
-  err = yaw_pid.measure - yaw_pid.set_point;
-  NORMALIZE(err, -M_PI, M_PI);
-  d_err = err - yaw_pid.last_err;
-  yaw_pid.last_err = err;
-  yaw_dot_pid.set_point = (err + d_err * yaw_pid.d_gain) * yaw_pid.p_gain; 
-  //  Bound(yaw_dot_pid.set_point, yaw_pid.saturation, -yaw_pid.saturation);
-  
-}
-
-
-void ctl_grz_alt_run( void ) {
-  float err =  ctl_grz_z - ctl_grz_z_setpoint;
-  static float z_last_err;
-  float d_err = err - z_last_err;
-  z_last_err = err;
-  //  ctl_grz_z_sum_err += err;
-
-  if (ctl_grz_z_setpoint < 0.1)
-    ctl_grz_z_dot_setpoint = -10;
-  else
-    ctl_grz_z_dot_setpoint = Chop(((err + d_err * ctl_grz_z_dgain + ctl_grz_z_sum_err * ctl_grz_z_igain) * ctl_grz_z_pgain), MIN_Z_DOT, MAX_Z_DOT);
-}
-
-void ctl_grz_z_dot_run( void ) {
-  float err =  ctl_grz_z_dot - ctl_grz_z_dot_setpoint;
-  static float z_dot_last_err;
-  float d_err = err - z_dot_last_err;
-  z_dot_last_err = err;
-  
-  if (flying) {
-    ctl_grz_z_dot_sum_err += err/600;
-    Bound(ctl_grz_z_dot_sum_err, -1., 1.);
-  } else
-    ctl_grz_z_dot_sum_err = 0;
-
-  throttle_setpoint = TRIM_PPRZ(ctl_grz_throttle_level + (int16_t)((err + d_err * ctl_grz_z_dot_dgain + ctl_grz_z_dot_sum_err * ctl_grz_z_dot_igain) * ctl_grz_z_dot_pgain));
-}
-
-
-float north_angle_set_point;
-float east_angle_set_point;
-
-
-void ctl_grz_horiz_speed_run ( void ) {
-  float err = ve_pid.measure - ve_pid.set_point;
-  float d_err = err - ve_pid.last_err;
-  ve_pid.last_err = err;
-  ve_pid.sum_err += err/100;
-  east_angle_set_point = (err + d_err * ve_pid.d_gain + ve_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain; 
-  Bound(east_angle_set_point, -ve_pid.saturation, ve_pid.saturation);
-  Bound(ve_pid.sum_err, -3, 3);
-
-  err = vn_pid.measure - vn_pid.set_point;
-  d_err = err - vn_pid.last_err;
-  vn_pid.last_err = err;
-  vn_pid.sum_err += err/100;
-  north_angle_set_point = - ((err + d_err * ve_pid.d_gain + vn_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain);
-  Bound(north_angle_set_point, -vn_pid.saturation, vn_pid.saturation);
-  Bound(vn_pid.sum_err, -3, 3);
-
-  float alpha = (M_PI/2. - yaw_pid.measure);
-  float cos_alpha = cos(alpha);
-  float sin_alpha = sin(alpha);
-
-  roll_pid.set_point = sin_alpha*east_angle_set_point + cos_alpha*north_angle_set_point;
-  pitch_pid.set_point = - cos_alpha*east_angle_set_point + sin_alpha*north_angle_set_point;
-}
-
-void ctl_grz_reset( void ) {
-  ctl_grz_z_dot_sum_err = 0;
-  ve_pid.sum_err = 0;
-  vn_pid.sum_err = 0;
-  roll_dot_pid.sum_err = 0;
-  pitch_dot_pid.sum_err = 0;
-  yaw_dot_pid.sum_err = 0;
-}
-
diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h
index 56e160f598..0f5e4fd5dc 100644
--- a/sw/airborne/imu_v3.h
+++ b/sw/airborne/imu_v3.h
@@ -4,6 +4,7 @@
 #include "std.h"
 #include "6dof.h"
 
+#include "max1167.h"
 #include "imu_v3_hw.h"
 
 /* calibrated sensor readings */
@@ -22,6 +23,7 @@ extern uint16_t imu_gyro_raw[AXIS_NB];
 extern int16_t  imu_mag_raw[AXIS_NB];
 
 extern void imu_v3_init(void);
+extern void imu_v3_hw_init(void);
 extern void imu_v3_detect_vehicle_still(void);
 extern bool_t imu_vehicle_still;
 extern float imu_vs_gyro_initial_bias[AXIS_NB];
@@ -132,6 +134,23 @@ extern struct adc_buf buf_bat;
 }
 
 
+#define ImuEventCheckAndHandle(user_handler) {	\
+    if (max1167_status == STA_MAX1167_DATA_AVAILABLE) {	\
+      max1167_status = STA_MAX1167_IDLE;		\
+      ImuUpdateGyros();					\
+      ImuUpdateAccels();				\
+      user_handler();					\
+    }							\
+  }
+
+#define ImuPeriodic() {				\
+    if (max1167_status != STA_MAX1167_IDLE) {	\
+      DOWNLINK_SEND_AHRS_OVERRUN();		\
+    }						\
+    else {					\
+      max1167_read();				\
+    }						\
+  }
 
 
 #endif /* IMU_V3_H */
diff --git a/sw/airborne/link_imu.c b/sw/airborne/link_imu.c
deleted file mode 100644
index 22fb01b0af..0000000000
--- a/sw/airborne/link_imu.c
+++ /dev/null
@@ -1,232 +0,0 @@
-#include "link_imu.h"
-
-#include "std.h"
-
-struct imu_state link_imu_state;
-struct imu_state link_imu_state_foo;
-
-uint16_t link_imu_crc;
-
-#define LINK_IMU_CRC_INIT 0x0
-#define LINK_IMU_PAYLOAD_LENGTH (sizeof(struct imu_state) - 2)
-#define LinkImuCrcLow(x) ((x)&0xff)
-#define LinkImuCrcHigh(x) ((x)>>8)
-#define LinkImuComputeCRC() {						\
-    uint8_t i;								\
-    link_imu_crc = LINK_IMU_CRC_INIT;					\
-    for(i = 0; i < LINK_IMU_PAYLOAD_LENGTH; i++) {			\
-      uint8_t _byte = ((uint8_t*)&link_imu_state)[i];			\
-      uint8_t a = ((uint8_t)LinkImuCrcHigh(link_imu_crc)) + _byte;	\
-      uint8_t b = ((uint8_t)LinkImuCrcLow(link_imu_crc)) + a;		\
-      link_imu_crc = b | a << 8;					\
-    }									\
-  }
-
-
-#ifdef IMU  /* IMU LPC code */
-
-#include "imu_v3.h"
-#include "multitilt.h"
-
-#include "LPC21xx.h"
-#include "armVIC.h"
-
-volatile uint8_t spi0_data_available;
-volatile uint8_t spi0_idx_buf;
-uint8_t* spi0_buffer_output = (uint8_t*)&link_imu_state;
-uint8_t* spi0_buffer_input = (uint8_t*)&link_imu_state_foo;
-
-#define LinkImuSetUnavailable() { IO1SET = _BV(SPI0_DRDY); }
-#define LinkImuSetAvailable()   { IO1CLR = _BV(SPI0_DRDY); }
-
-void SPI0_ISR(void) __attribute__((naked));
-
-#define Spi0InitBuf() {						      \
-    spi0_idx_buf = 0;						      \
-    S0SPDR = spi0_buffer_output[0];				      \
-  }
-
-#define Spi0OneByte() {							\
-    /* FIXME : do something usefull with the status register reading */ \
-    uint8_t foo __attribute__((unused)) = S0SPSR;			\
-    spi0_buffer_input[spi0_idx_buf] = S0SPDR;				\
-    spi0_idx_buf++;							\
-    if (spi0_idx_buf < sizeof(link_imu_state)) {			\
-      S0SPDR = spi0_buffer_output[spi0_idx_buf];			\
-    }									\
-    else {								\
-      spi0_data_available = TRUE;					\
-      LinkImuSetUnavailable();						\
-    }									\
-  } 
-
-/*
-  wiring on IMU_V3
- P0_4   SCK0
- P0_5   MISO0
- P0_6   MOSI0
- P0_7   SSEL0
- P1_24  DRDY
-*/
-
-#define SPI0_DRDY 24
-
-/*  */
-#define S0SPCR_SPIE  7  /* SPI enable */
-
-void link_imu_init ( void ) {
-  /* init SPI0 */
-  /* setup pins for sck, miso, mosi, SSEL */
-  PINSEL0 |= 1<<8 | 1<<10| 1<<12 | 1<< 14;
-  /* setup P1_16 to P1_25 as GPIO */
-  PINSEL2 &= ~(1<<3);
-  /* P1_24 (DRDY) is output */
-  SetBit(IO1DIR, SPI0_DRDY);
-  /* DRDY idles high */
-  LinkImuSetUnavailable();
-  /* configure SPI : 8 bits CPOL=0 CPHA=0 MSB_first slave */
-  S0SPCR = _BV(3);
-  /* setup SPI clock rate */
-  S0SPCCR = 0x20;
-
-  /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT(VIC_SPI0);   // SPI0 selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_SPI0);     // SPI0 interrupt enabled
-  VICVectCntl1 = VIC_ENABLE | VIC_SPI0;
-  VICVectAddr1 = (uint32_t)SPI0_ISR;    // address of the ISR
-
-  /* clear pending interrupt */
-  SetBit(S0SPINT, SPI0IF);
-  /* enable SPI interrupt */
-  SetBit(S0SPCR, S0SPCR_SPIE);
-
-}
-
-void link_imu_send ( void ) {
-  LinkImuSetUnavailable();
-  
-  link_imu_state.r_rates[AXIS_P] = imu_vs_gyro_unbiased[AXIS_P] * RATE_PI_S/M_PI;
-  link_imu_state.r_rates[AXIS_Q] = imu_vs_gyro_unbiased[AXIS_Q] * RATE_PI_S/M_PI;
-  link_imu_state.r_rates[AXIS_R] = imu_vs_gyro_unbiased[AXIS_R] * RATE_PI_S/M_PI;
-  link_imu_state.f_rates[AXIS_P] = mtt_p * RATE_PI_S/M_PI;
-  link_imu_state.f_rates[AXIS_Q] = mtt_q * RATE_PI_S/M_PI;
-  link_imu_state.f_rates[AXIS_R] = mtt_r * RATE_PI_S/M_PI;
-  link_imu_state.f_eulers[AXIS_X] = mtt_phi * ANGLE_PI/M_PI;
-  link_imu_state.f_eulers[AXIS_Y] = mtt_theta* ANGLE_PI/M_PI;
-  link_imu_state.f_eulers[AXIS_Z] = mtt_psi  * ANGLE_PI/M_PI;
-  link_imu_state.status = IMU_RUNNING;
-
-  LinkImuComputeCRC();
-  link_imu_state.crc = link_imu_crc;
-
-  Spi0InitBuf();
-  LinkImuSetAvailable();
-}
-
-
-void SPI0_ISR(void) {
-  ISR_ENTRY();
-  Spi0OneByte();
-  /* clear the interrupt */
-  S0SPINT = _BV(SPI0IF); 
-  /* clear this interrupt from the VIC */
-  VICVectAddr = 0x00000000;
-  ISR_EXIT();
-}
-
-
-#endif /* IMU */
-
-
-
-
-
-#ifdef CONTROLLER  /* lpc controller board */
-/* IMU connected to SSP ( aka SPI1 ) */
-
-#include "LPC21xx.h"
-#include "interrupt_hw.h" 
-#include "spi.h"
-
-#include "booz_estimator.h"
-
-uint32_t link_imu_nb_err;
-uint8_t  link_imu_status;
-uint32_t link_imu_timeout;
-#define LINK_IMU_TIMEOUT 100
-
-/* DRDY connected pin to P0.16 EINT0 */ 
-#define LINK_IMU_DRDY_PINSEL PINSEL1
-#define LINK_IMU_DRDY_PINSEL_VAL 0x01
-#define LINK_IMU_DRDY_PINSEL_BIT 0
-#define LINK_IMU_DRDY_EINT 0
-
-static void EXTINT0_ISR(void) __attribute__((naked));
-
-
-void link_imu_init ( void ) {
-
-  /* configure DRDY pin */
-  LINK_IMU_DRDY_PINSEL |= LINK_IMU_DRDY_PINSEL_VAL << LINK_IMU_DRDY_PINSEL_BIT;
-  SetBit(EXTMODE, LINK_IMU_DRDY_EINT);   /* EINT is edge trigered */
-  ClearBit(EXTPOLAR,LINK_IMU_DRDY_EINT); /* EINT is trigered on falling edge */
-  SetBit(EXTINT,LINK_IMU_DRDY_EINT);     /* clear pending EINT */
-  
-  /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT( VIC_EINT0 );  /* select EINT0 as IRQ source */
-  VICIntEnable = VIC_BIT( VIC_EINT0 );    /* enable it */
-  VICVectCntl9 = VIC_ENABLE | VIC_EINT0;
-  VICVectAddr9 = (uint32_t)EXTINT0_ISR;   // address of the ISR 
-
-  spi_buffer_input = (uint8_t*)&link_imu_state;
-  spi_buffer_output = (uint8_t*)&link_imu_state_foo;
-  spi_buffer_length = sizeof(link_imu_state);
-
-  link_imu_nb_err = 0;
-  link_imu_status = IMU_NO_LINK;
-  link_imu_timeout = LINK_IMU_TIMEOUT;
-
-}
-
-void link_imu_event_task( void ) {
-  LinkImuComputeCRC();
-  if (link_imu_crc == link_imu_state.crc) {
-    link_imu_timeout = 0;
-    link_imu_status = link_imu_state.status;
-    booz_estimator_uf_p = link_imu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
-    booz_estimator_uf_q = link_imu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
-    booz_estimator_uf_r = link_imu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
-    booz_estimator_p = link_imu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
-    booz_estimator_q = link_imu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
-    booz_estimator_r = link_imu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
-    booz_estimator_phi   = link_imu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
-    booz_estimator_theta = link_imu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
-    booz_estimator_psi   = link_imu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
-  }
-  else {
-    link_imu_nb_err++;
-  }
-}
-
-extern void link_imu_periodic_task( void ) {
-  if (link_imu_timeout < LINK_IMU_TIMEOUT)
-    link_imu_timeout++;
-  else
-    link_imu_status = IMU_NO_LINK;
-}
-
-
-void EXTINT0_ISR(void) {
-  ISR_ENTRY();
-
-  SpiSelectSlave0();
-  SpiStart();
-
-  /* clear EINT2 */
-  SetBit(EXTINT,LINK_IMU_DRDY_EINT); /* clear EINT0 */
-  
-  VICVectAddr = 0x00000000;    /* clear this interrupt from the VIC */
-  ISR_EXIT();
-}
-
-#endif /* CONTROLLER */
diff --git a/sw/airborne/link_imu.h b/sw/airborne/link_imu.h
deleted file mode 100644
index c618e0d4e4..0000000000
--- a/sw/airborne/link_imu.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#ifndef LINK_IMU_H
-#define LINK_IMU_H
-
-#include <inttypes.h>
-
-#include "6dof.h"
-
-/* angles are transmitted as int16 : PI -> 16384 */
-/* rates are transmitted as int16 : 1 PI s-1 -> 16384 */
-#define ANGLE_PI 0X3FFF
-#define RATE_PI_S 0X3FFF
-
-#define IMU_UNINIT  0
-#define IMU_RUNNING 1
-#define IMU_CRASHED 2
-#define IMU_NO_LINK 3
-
-struct imu_state {
-  int16_t r_rates[AXIS_NB];
-  int16_t f_rates[AXIS_NB];
-  int16_t f_eulers[AXIS_NB];
-  int16_t pad0;
-  uint8_t status;
-  uint8_t pad1;
-  uint16_t crc;
-};
-
-
-extern struct imu_state link_imu_state;
-extern void link_imu_init ( void );
-
-#ifdef IMU
-
-extern void link_imu_send( void );
-
-#endif /* IMU */
-
-
-#ifdef CONTROLLER
-
-extern uint32_t link_imu_nb_err;
-extern uint8_t  link_imu_status;
-extern void link_imu_event_task( void );
-extern void link_imu_periodic_task( void );
-
-#define LinkImuEventCheckAndHandle() { \
-    if (spi_message_received) {	       \
-      spi_message_received = FALSE;    \
-      link_imu_event_task();	       \
-    }				       \
-  }				       \
-
-#endif /* CONTROLLER */
-
-
-#endif /* LINK_IMU_H */
diff --git a/sw/airborne/main_fbw.c b/sw/airborne/main_fbw.c
index 19625079b7..6924aa4a08 100644
--- a/sw/airborne/main_fbw.c
+++ b/sw/airborne/main_fbw.c
@@ -54,8 +54,6 @@
 #include "link_mcu.h"
 #endif
 
-#include "link_imu.h"
-
 #ifdef ADC
 struct adc_buf vsupply_adc_buf;
 #endif
@@ -99,10 +97,6 @@ void init_fbw( void ) {
   spi_init();
   link_mcu_restart();
 #endif
-#ifdef LINK_IMU
-  spi_init();
-  link_imu_init();
-#endif
 
   fbw_mode = FBW_MODE_FAILSAFE;
 
@@ -169,14 +163,6 @@ void event_task_fbw( void) {
 #endif /* MCU_SPI_LINK */
 #endif /* INTER_MCU */
 
-#ifdef LINK_IMU
-  if (spi_message_received) {
-    /* Got a message on SPI. */
-    spi_message_received = FALSE;
-    link_imu_event_task();
-    EstimatorSetAtt(link_imu_state.eulers[AXIS_X], link_imu_state.eulers[AXIS_Z],  link_imu_state.eulers[AXIS_Y]);
-  }
-#endif /* LINK_IMU */
 }
 
 
diff --git a/sw/airborne/multitilt.h b/sw/airborne/multitilt.h
index a47415b5b7..908d1b8ed4 100644
--- a/sw/airborne/multitilt.h
+++ b/sw/airborne/multitilt.h
@@ -4,8 +4,8 @@
 #include "std.h"
 
 #define MT_STATUS_UNINIT       0
-#define MT_STATUS_RUNNING      2
-#define MT_STATUS_CRASHED      3
+#define MT_STATUS_RUNNING      1
+#define MT_STATUS_CRASHED      2
 
 extern uint8_t mtt_status;
 
diff --git a/sw/airborne/sim/booz_link_mcu_hw.c b/sw/airborne/sim/booz_link_mcu_hw.c
new file mode 100644
index 0000000000..77fdda4ad6
--- /dev/null
+++ b/sw/airborne/sim/booz_link_mcu_hw.c
@@ -0,0 +1,3 @@
+#include "booz_link_mcu.h"
+
+void booz_link_mcu_hw_init ( void ) {}
diff --git a/sw/airborne/sim/booz_link_mcu_hw.h b/sw/airborne/sim/booz_link_mcu_hw.h
new file mode 100644
index 0000000000..a6b55a3112
--- /dev/null
+++ b/sw/airborne/sim/booz_link_mcu_hw.h
@@ -0,0 +1,12 @@
+#ifndef BOOZ_LINK_MCU_HW_H
+#define BOOZ_LINK_MCU_HW_H
+
+/* FIXME: not sure what that does here !!!! */
+#define Spi0InitBuf() {}
+
+#define BoozLinkMcuSetUnavailable() { }
+#define BoozLinkMcuSetAvailable()   { spi_message_received = TRUE; }
+
+
+
+#endif /* BOOZ_LINK_MCU_HW_H */
diff --git a/sw/airborne/sim/imu_v3_hw.h b/sw/airborne/sim/imu_v3_hw.h
index 58fc05c465..7b1e1490a1 100644
--- a/sw/airborne/sim/imu_v3_hw.h
+++ b/sw/airborne/sim/imu_v3_hw.h
@@ -1,24 +1,4 @@
 #ifndef IMU_V3_HW_H
 #define IMU_V3_HW_H
 
-#include "std.h"
-
-//#include "booz_sensors_model.h"
-
-extern void imu_v3_hw_init(void);
-
-extern bool_t imu_data_available;
-
-#define ImuEventCheckAndHandle(user_handler) {	\
-    if (imu_data_available) {		\
-      imu_data_available = FALSE;		\
-      /* ImuUpdateGyros(); */			\
-      /* ImuUpdateAccels();*/			\
-      user_handler();				\
-    }						\
-  }
-
-
-#define ImuPeriodic() {	}
-
 #endif /* IMU_V3_HW_H */
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index a1be5ac2e1..13460c4ba0 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -109,7 +109,7 @@ BOOZ_SIM_SRCS = main_booz_sim.c           \
                 booz_joystick.c           \
 
 CFLAGS += -DSITL
-CFLAGS += -DCONTROLLER
+CFLAGS += -DBOOZ_CONTROLLER_MCU
 CFLAGS += -DCONFIG=\"conf_booz.h\"
 
 BOOZ_AB_SRCS += $(AB)/booz_controller_main.c
@@ -136,10 +136,16 @@ CFLAGS += -DDOWNLINK_TRANSPORT=IvyTransport
 BOOZ_AB_SRCS += $(AB_ARCH)/ivy_transport.c
 LDFLAGS += -lglibivy
 
+BOOZ_AB_SRCS += $(AB)/booz_inter_mcu.c
+CFLAGS += -DUSE_SPI
+BOOZ_AB_SRCS += $(AB)/booz_link_mcu.c $(AB_ARCH)/booz_link_mcu_hw.c
+BOOZ_AB_SRCS += $(AB)/spi.c $(AB_ARCH)/spi_hw.c
+
 #../airborne/link_imu.c 
 #CFLAGS += -DDATALINK=PPRZ
 #BOOZ_AB_SRCS += ../airborne/datalink.c
 
+CFLAGS += -DBOOZ_FILTER_MCU
 BOOZ_AB_SRCS += $(AB)/booz_estimator.c
 BOOZ_AB_SRCS += $(AB)/booz_control.c
 BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
@@ -148,10 +154,16 @@ BOOZ_AB_SRCS += $(AB)/commands.c
 BOOZ_AB_SRCS += $(AB)/booz_filter_main.c
 
 CFLAGS += -DADC_CHANNEL_AX=1 -DADC_CHANNEL_AY=2 -DADC_CHANNEL_AZ=3 -DADC_CHANNEL_BAT=4
-BOOZ_AB_SRCS  += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c $(AB_ARCH)/adc_hw.c
+BOOZ_AB_SRCS  += $(AB_ARCH)/adc_hw.c
+
 
 BOOZ_AB_SRCS += $(AB)/booz_filter_telemetry.c
 
+BOOZ_AB_SRCS  += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c
+
+BOOZ_AB_SRCS  += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c
+
+
 BOOZ_AB_SRCS += $(AB)/multitilt.c
 
 
diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c
index 33679f35a1..dd97c666db 100644
--- a/sw/simulator/booz_sensors_model.c
+++ b/sw/simulator/booz_sensors_model.c
@@ -1,9 +1,52 @@
 #include "booz_sensors_model.h"
 
+#include <math.h>
+
 #include "booz_flight_model.h"
 
-void booz_sensors_model_run(void) {
+
+struct BoozSensorsModel bsm;
+
+void booz_sensors_model_init(void) {
+  bsm.accel = v_get(AXIS_NB);
+  bsm.accel->ve[AXIS_X] = 0.;
+  bsm.accel->ve[AXIS_Y] = 0.;
+  bsm.accel->ve[AXIS_Z] = 0.;
 
 
+  bsm.gyro = v_get(AXIS_NB);
+  bsm.gyro->ve[AXIS_P] = 0.;
+  bsm.gyro->ve[AXIS_Q] = 0.;
+  bsm.gyro->ve[AXIS_R] = 0.;
+
+  bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
+  m_zero(bsm.gyro_sensitivity);
+  bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = 1./-0.0002202;
+  bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = 1./-0.0002150;
+  bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = 1./ 0.0002104;
+
+  bsm.gyro_neutral = v_get(AXIS_NB);
+  bsm.gyro_neutral->ve[AXIS_P] = 40885;
+  bsm.gyro_neutral->ve[AXIS_Q] = 40910;
+  bsm.gyro_neutral->ve[AXIS_R] = 39552;
+
+}
+
+
+void booz_sensors_model_run(void) {
+  
+  /* extract rotational speed from flight model state */
+  static VEC *rate_body = VNULL;
+  rate_body = v_resize(rate_body, AXIS_NB);
+  rate_body->ve[AXIS_P] = bfm.state->ve[BFMS_P];
+  rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q];
+  rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R];
+
+  /* compute sensor readings */
+  bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro); 
+  bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro); 
+  bsm.gyro->ve[AXIS_P] = round( bsm.gyro->ve[AXIS_P]);
+  bsm.gyro->ve[AXIS_Q] = round( bsm.gyro->ve[AXIS_Q]);
+  bsm.gyro->ve[AXIS_R] = round( bsm.gyro->ve[AXIS_R]);
 
 }
diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h
index 6ad40f5224..1557067ecd 100644
--- a/sw/simulator/booz_sensors_model.h
+++ b/sw/simulator/booz_sensors_model.h
@@ -2,7 +2,18 @@
 #define BOOZ_SENSORS_MODEL_H
 
 #include "6dof.h"
+#include <matrix.h>
 
+extern void booz_sensors_model_init(void);
 extern void booz_sensors_model_run(void);
 
+struct BoozSensorsModel {
+  VEC* accel;
+  VEC* gyro;
+  MAT* gyro_sensitivity;
+  VEC* gyro_neutral;
+};
+
+extern struct BoozSensorsModel bsm;
+
 #endif /* BOOZ_SENSORS_MODEL_H */
diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c
index d833741c67..c8306ab800 100644
--- a/sw/simulator/main_booz_sim.c
+++ b/sw/simulator/main_booz_sim.c
@@ -27,11 +27,14 @@ double sim_time;
 #define DT_DISPLAY 0.04
 double disp_time;
 
-double foo_commands[] = {0., 0., 0., 0.};
+double booz_sim_actuators_values[] = {0., 0., 0., 0.};
 
 static gboolean booz_sim_periodic(gpointer data);
 static inline void booz_sim_display(void);
 
+static void booz_sim_set_ppm_from_joystick( void );
+static void booz_sim_read_actuators( void );
+
 #ifdef SIM_UART
 static void sim_uart_init(void);
 #endif
@@ -44,49 +47,42 @@ static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *arg
 #include "booz_estimator.h"
 #include "radio_control.h"
 #include "actuators.h"
+#include "imu_v3.h"
+#include "booz_inter_mcu.h"
+
 volatile bool_t ppm_valid;
-#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
 
 static gboolean booz_sim_periodic(gpointer data) {
+  /* read actuators positions */
+  booz_sim_read_actuators();
 
-  booz_flight_model_run(DT, foo_commands);
+  booz_flight_model_run(DT, booz_sim_actuators_values);
   booz_sensors_model_run();
   sim_time += DT;
 
-  booz_sim_display();
-
-  booz_estimator_p = bfm.state->ve[BFMS_P];
-  booz_estimator_q = bfm.state->ve[BFMS_Q];
-  booz_estimator_r = bfm.state->ve[BFMS_R];
-
-  booz_estimator_phi = bfm.state->ve[BFMS_PHI];
-  booz_estimator_theta = bfm.state->ve[BFMS_THETA];
-  booz_estimator_psi = bfm.state->ve[BFMS_PSI];
+  /* call the filter periodic task to read sensors                      */
+  /* the sim implementation of max1167_read ( called by ImuPeriodic() ) */
+  /* will post a ImuEvent                                               */
+  booz_filter_main_periodic_task();
+  /* process the ImuEvent */
+  /* it will run the filter and the inter-process communication which   */
+  /* will post a BoozLinkMcuEvent in the Controller process             */
+  booz_filter_main_event_task();
+  /* process the BoozLinkMcuEvent                                       */
+  /* this will update the controller stimator                           */
+  booz_controller_main_event_task();
 
   /* post a radio control event */
-  ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
-  ppm_pulses[RADIO_PITCH]    = 1498 + booz_joystick_value[JS_PITCH]    * (2050-950);
-  ppm_pulses[RADIO_ROLL]     = 1500 + booz_joystick_value[JS_ROLL]     * (2050-950);
-  ppm_pulses[RADIO_YAW]      = 1500 + booz_joystick_value[JS_YAW]      * (2050-950);
-  ppm_pulses[RADIO_MODE]     = 1500 + booz_joystick_value[JS_MODE]     * (2050-950);
+  booz_sim_set_ppm_from_joystick();
   ppm_valid = TRUE;
+  /* and let the controller process it */
   booz_controller_main_event_task();
-  /* run periodic tak */
+
+  /* call the controller periodic task to run control loops            */
   booz_controller_main_periodic_task();
-  booz_filter_main_periodic_task();
-  /* get actuators values */
-#if 0
-  foo_commands[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 1200)/(double)(1850-1200);
-  foo_commands[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 1200)/(double)(1850-1200);
-  foo_commands[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 1200)/(double)(1850-1200);
-  foo_commands[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 1200)/(double)(1850-1200);
-#endif
 
-  foo_commands[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK]   - 0)/(double)(255);
-  foo_commands[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 0)/(double)(255);
-  foo_commands[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 0)/(double)(255);
-  foo_commands[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT]   - 0)/(double)(255);
 
+  booz_sim_display();
 
   return TRUE;
 }
@@ -98,6 +94,8 @@ int main ( int argc, char** argv) {
   
   booz_flight_model_init();
 
+  booz_sensors_model_init();
+
   booz_flightgear_init(fg_host, fg_port);
 
 #ifdef SIM_UART
@@ -129,7 +127,7 @@ int main ( int argc, char** argv) {
 // Helpers
 ////////////////////
 
-
+#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
 static inline void booz_sim_display(void) {
   if (sim_time >= disp_time) {
     disp_time+= DT_DISPLAY;
@@ -144,8 +142,30 @@ static inline void booz_sim_display(void) {
 
 
 
+static void booz_sim_set_ppm_from_joystick( void ) {
+  ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
+  ppm_pulses[RADIO_PITCH]    = 1498 + booz_joystick_value[JS_PITCH]    * (2050-950);
+  ppm_pulses[RADIO_ROLL]     = 1500 + booz_joystick_value[JS_ROLL]     * (2050-950);
+  ppm_pulses[RADIO_YAW]      = 1500 + booz_joystick_value[JS_YAW]      * (2050-950);
+  ppm_pulses[RADIO_MODE]     = 1500 + booz_joystick_value[JS_MODE]     * (2050-950);
+}
 
 
+static void booz_sim_read_actuators( void ) {
+#if 0
+  booz_sim_actuators_values[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 1200)/(double)(1850-1200);
+  booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 1200)/(double)(1850-1200);
+  booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 1200)/(double)(1850-1200);
+  booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 1200)/(double)(1850-1200);
+#endif
+
+  booz_sim_actuators_values[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK]   - 0)/(double)(255);
+  booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 0)/(double)(255);
+  booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 0)/(double)(255);
+  booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT]   - 0)/(double)(255);
+
+}
+
 
 #if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport