diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml
index 0c2c6dc346..2cc72ccca7 100644
--- a/conf/airframes/booz.xml
+++ b/conf/airframes/booz.xml
@@ -6,6 +6,11 @@ ARCHI=arm7
 
 FLASH_MODE = IAP
 
+
+#
+# controller CPU
+#
+
 ctl.ARCHDIR = $(ARCHI)
 ctl.ARCH = arm7tdmi
 ctl.TARGET = ctl
@@ -35,6 +40,10 @@ ctl.srcs += link_imu.c
 ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
+#
+# AHRS CPU
+#
+
 imu.ARCHDIR = $(ARCHI)
 imu.ARCH = arm7tdmi
 imu.TARGET = imu
@@ -59,7 +68,8 @@ imu.srcs += $(SRC_ARCH)/micromag.c
 
 imu.srcs += imu_v3.c
 
-imu.srcs += ahrs_new.c
+//imu.srcs += ahrs_new.c
+imu.srcs += multitilt.c
 
 imu.srcs += link_imu.c
 
diff --git a/sw/airborne/AMI601.c b/sw/airborne/AMI601.c
new file mode 100644
index 0000000000..b4a1910ae0
--- /dev/null
+++ b/sw/airborne/AMI601.c
@@ -0,0 +1,144 @@
+#include "AMI601.h"
+
+#include "i2c.h"
+
+#include "led.h"
+
+#include <string.h>
+
+#define FOO_DELAY() {  \
+    uint16_t foo = 1;  \
+    while (foo) foo++; \
+  }
+
+
+#define AMI601_SLAVE_ADDR 0x60
+
+float ami601_ax;
+float ami601_ay;
+float ami601_az;
+
+float ami601_mx;
+float ami601_my;
+float ami601_mz;
+
+uint8_t ami601_foo1;
+uint8_t ami601_foo2;
+uint8_t ami601_foo3;
+
+uint16_t ami601_val[AMI601_NB_CHAN];
+static volatile bool_t ami601_i2c_done;
+
+/* TRG ( trigger) P0.28 */
+#define AMI601_TRG_PIN   28
+#define AMI601_TRG_IODIR IO0DIR
+#define AMI601_TRG_IOSET IO0SET
+#define AMI601_TRG_IOCLR IO0CLR
+
+/* RST ( reset ) P0.29 */
+#define AMI601_RST_PIN   29
+#define AMI601_RST_IODIR IO0DIR
+#define AMI601_RST_IOSET IO0SET
+#define AMI601_RST_IOCLR IO0CLR
+
+/* BUSY ( busy ) P0.30 EINT3 */
+#define AMI601_BUSY_PIN   
+#define AMI601_BUSY_IODIR 
+#define AMI601_BUSY_IOSET 
+#define AMI601_BUSY_IOCLR 
+
+void ami601_init( void ) {
+  /* configure TRG pin as output and assert it*/
+  SetBit(AMI601_TRG_IODIR , AMI601_TRG_PIN);
+  SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN);
+
+  /* configure RST pin as output and set it low*/
+  SetBit(AMI601_RST_IODIR , AMI601_RST_PIN);
+  SetBit(AMI601_RST_IOCLR , AMI601_RST_PIN);
+
+
+  uint8_t i;
+  for (i=0; i< AMI601_NB_CHAN; i++) {
+    ami601_val[i] = 0;
+  }
+  ami601_i2c_done = TRUE;
+
+
+  FOO_DELAY();
+  /* assert reset */
+  SetBit(AMI601_RST_IOSET , AMI601_RST_PIN);  
+
+
+}
+
+void ami601_periodic( void ) {
+  /* pulse TRG */
+  //  SetBit(AMI601_TRG_IOCLR , AMI601_TRG_PIN);
+  //  FOO_DELAY();
+  //  SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN);
+
+  if (ami601_i2c_done) {
+    LED_ON(2);
+    const uint8_t read_cmd[] = { 0x55, 0xAA, 0X14};
+    memcpy((void*)i2c_buf, read_cmd, sizeof(read_cmd));
+    i2c_transmit(AMI601_SLAVE_ADDR, sizeof(read_cmd), &ami601_i2c_done);
+    ami601_i2c_done = FALSE;
+
+    while (!ami601_i2c_done);
+
+    FOO_DELAY();
+
+    i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done);
+    while (!ami601_i2c_done);
+    LED_OFF(2);
+    
+    ami601_foo1 = i2c_buf[0];
+    ami601_foo1 = i2c_buf[1];
+    ami601_foo1 = i2c_buf[2];
+
+    uint8_t i;
+    for (i=0; i< AMI601_NB_CHAN; i++) {
+      ami601_val[i] = 	i2c_buf[3 + 2 * i];
+      ami601_val[i] +=  i2c_buf[3 + 2 * i + 1]  * 256;
+    }
+
+    
+
+  }
+
+}
+
+
+
+void ami601_send_sleep( void ) {
+
+
+}
+
+#define AX_NEUTRAL 2323
+#define AY_NEUTRAL 2323
+#define AZ_NEUTRAL 2323
+
+#define AX_GAIN 0.0283936
+#define AY_GAIN -0.0283936
+#define AZ_GAIN 0.0283936
+
+#define MX_NEUTRAL 2048
+#define MY_NEUTRAL 2048
+#define MZ_NEUTRAL 2048
+
+#define MX_GAIN -1.
+#define MY_GAIN  1.
+#define MZ_GAIN  1.
+
+void ami601_scale_measures(void) {
+
+  ami601_ax = (ami601_val[3] - AX_NEUTRAL) * AX_GAIN;
+  ami601_ay = (ami601_val[5] - AY_NEUTRAL) * AY_GAIN;
+  ami601_az = (ami601_val[1] - AZ_NEUTRAL) * AZ_GAIN;
+
+  ami601_mx = (ami601_val[0] - MX_NEUTRAL) * MX_GAIN;
+  ami601_my = (ami601_val[4] - MY_NEUTRAL) * MY_GAIN;
+  ami601_mz = (ami601_val[2] - MZ_NEUTRAL) * MZ_GAIN;
+
+}
diff --git a/sw/airborne/AMI601.h b/sw/airborne/AMI601.h
new file mode 100644
index 0000000000..65a1fedeb5
--- /dev/null
+++ b/sw/airborne/AMI601.h
@@ -0,0 +1,25 @@
+#ifndef AMI601_H
+#define AMI601_H
+
+#include "std.h"
+
+extern void ami601_init( void );
+
+extern void ami601_periodic( void );
+
+#define AMI601_NB_CHAN 6
+extern uint16_t ami601_val[AMI601_NB_CHAN];
+extern uint8_t ami601_foo1;
+extern uint8_t ami601_foo2;
+extern uint8_t ami601_foo3;
+
+float ami601_ax;
+float ami601_ay;
+float ami601_az;
+
+float ami601_mx;
+float ami601_my;
+float ami601_mz;
+
+
+#endif /* AMI601_H */
diff --git a/sw/airborne/frames.h b/sw/airborne/frames.h
index 3dc4c8176b..f4d649733c 100644
--- a/sw/airborne/frames.h
+++ b/sw/airborne/frames.h
@@ -6,5 +6,8 @@
 #define AXIS_Z  2
 #define AXIS_NB 3
 
+#define AXIS_P 0
+#define AXIS_Q 1
+#define AXIS_R 2
 
 #endif /* FRAMES_H */
diff --git a/sw/airborne/link_imu.c b/sw/airborne/link_imu.c
index 2013a6fab2..79a704f4d5 100644
--- a/sw/airborne/link_imu.c
+++ b/sw/airborne/link_imu.c
@@ -9,7 +9,7 @@ struct imu_state link_imu_state_foo;
 #ifdef IMU  /* IMU LPC code */
 
 #include "imu_v3.h"
-#include "ahrs_new.h"
+//#include "ahrs_new.h"
 
 #include "LPC21xx.h"
 #include "armVIC.h"
@@ -90,6 +90,7 @@ void link_imu_init ( void ) {
 }
 
 void link_imu_send ( void ) {
+  /*
   if (!isnan(ahrs_phi)   &&
       !isnan(ahrs_theta) &&
       !isnan(ahrs_psi)) {
@@ -114,6 +115,7 @@ void link_imu_send ( void ) {
     link_imu_state.sin_theta = 0.;
     link_imu_state.status = IMU_CRASHED;
   }
+  */
   Spi0InitBuf();
 }
 
diff --git a/sw/airborne/main_imu.c b/sw/airborne/main_imu.c
index 899e27cfa5..c837ac51ab 100644
--- a/sw/airborne/main_imu.c
+++ b/sw/airborne/main_imu.c
@@ -11,7 +11,8 @@
 #include "max1167.h"
 #include "micromag.h"
 #include "imu_v3.h"
-#include "ahrs_new.h"
+//#include "ahrs_new.h"
+#include "multitilt.h"
 #include "link_imu.h"
 
 #include "messages.h"
@@ -96,7 +97,7 @@ static inline void main_event_task( void ) {
 #ifdef SEND_ACCEL
     DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]);
 #endif
-    ahrs_task();
+    //    ahrs_task();
 
     t1=T0TC;
     //    uint32_t dt = t1 - t0;
@@ -119,7 +120,7 @@ static inline void main_periodic_task( void ) {
 
 }
 
-
+#if 0
 static inline void ahrs_task( void ) {
   /* discard first 100 measures */
   if (ahrs_step == AHRS_STEP_UNINIT) {
@@ -153,7 +154,7 @@ static inline void ahrs_task( void ) {
     if (ahrs_step == AHRS_STEP_NB) ahrs_step = AHRS_STEP_ROLL;
   }
 }
-
+#endif
 
 /* SSPCR0 settings */
 #define SSP_DDS  0x07 << 0  /* data size         : 8 bits        */