diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml
index c372dd18bd..376126abc0 100644
--- a/conf/airframes/Poine/booz2_a1.xml
+++ b/conf/airframes/Poine/booz2_a1.xml
@@ -214,7 +214,9 @@
     <target name="test_baro"          board="booz_1.0"/>
     <target name="test_rc_spektrum"   board="booz_1.0"/>
     <target name="test_rc_ppm"        board="booz_1.0"/>
+    <target name="test_esc_mkk_simple" board="booz_1.0"/>
     <target name="test_actuators_mkk" board="booz_1.0"/>
+    <target name="test_ami601"        board="booz_1.0"/>
   </firmware>
 
 
diff --git a/conf/autopilot/booz_test_progs.makefile b/conf/autopilot/booz_test_progs.makefile
index b626d30e84..972a0ae6aa 100644
--- a/conf/autopilot/booz_test_progs.makefile
+++ b/conf/autopilot/booz_test_progs.makefile
@@ -166,6 +166,28 @@ test_rc_ppm.srcs   += $(SRC_BOOZ)/booz_radio_control.c \
                       $(SRC_BOOZ_ARCH)/radio_control/booz_radio_control_ppm_arch.c
 
 
+
+#
+# simple test of mikrokopter motor controllers
+#
+test_esc_mkk_simple.ARCHDIR = $(ARCHI)
+test_esc_mkk_simple.ARCH    = arm7tdmi
+test_esc_mkk_simple.TARGET  = test_esc_mkk_simple
+test_esc_mkk_simple.TARGETDIR = test_esc_mkk_simple
+test_esc_mkk_simple.CFLAGS = -I$(SRC_LISA) -I$(ARCHI) -DPERIPHERALS_AUTO_INIT
+test_esc_mkk_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_esc_mkk_simple.srcs = test/test_esc_mkk_simple.c	    \
+                           $(SRC_ARCH)/armVIC.c
+test_esc_mkk_simple.CFLAGS += -DUSE_LED
+test_esc_mkk_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+test_esc_mkk_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_esc_mkk_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_esc_mkk_simple.CFLAGS += -DACTUATORS_MKK_DEV=i2c0
+test_esc_mkk_simple.CFLAGS += -DUSE_I2C0
+test_esc_mkk_simple.CFLAGS += -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
+test_esc_mkk_simple.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+
+
 #
 # test actuators mkk
 #
@@ -194,5 +216,37 @@ test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
 test_actuators_mkk.srcs += $(SRC_BOOZ)/actuators/booz_actuators_mkk.c
 test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
 test_actuators_mkk.srcs += $(SRC_BOOZ)/actuators/booz_supervision.c
+test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEV=i2c0
 test_actuators_mkk.CFLAGS += -DUSE_I2C0
-test_actuators_mkk.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
\ No newline at end of file
+test_actuators_mkk.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+test_actuators_mkk.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
+
+
+#
+# test ami601
+#
+test_ami601.ARCHDIR = $(ARCHI)
+test_ami601.ARCH      = arm7tdmi
+test_ami601.TARGET = test_ami601
+test_ami601.TARGETDIR = test_ami601
+test_ami601.CFLAGS = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+test_ami601.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_ami601.srcs = test/peripherals/test_ami601.c \
+                   $(SRC_ARCH)/armVIC.c
+
+test_ami601.CFLAGS += -DUSE_LED
+
+test_ami601.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_ami601.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_ami601.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+test_ami601.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_ami601.srcs += $(SRC_ARCH)/uart_hw.c
+
+test_ami601.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) 
+test_ami601.srcs += downlink.c pprz_transport.c
+
+test_ami601.CFLAGS += -DUSE_AMI601
+test_ami601.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
+test_ami601.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11
+test_ami601.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
index 3eaba8ff7d..dd92ba3874 100644
--- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
@@ -49,7 +49,7 @@ ap.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
 
 ap.CFLAGS += -DUSE_AMI601
 ap.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
-ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
+ap.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11
 
 
 #
diff --git a/sw/airborne/arm7/i2c_hw.c b/sw/airborne/arm7/i2c_hw.c
index f8257b934f..630d22158c 100644
--- a/sw/airborne/arm7/i2c_hw.c
+++ b/sw/airborne/arm7/i2c_hw.c
@@ -309,8 +309,8 @@ void i2c1_hw_init ( void ) {
   I2C1SCLH = I2C1_SCLH_D;  
   
   // initialize the interrupt vector
-  VICIntSelect &= ~VIC_BIT(VIC_I2C1);              // I2C0 selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_I2C1);                // I2C0 interrupt enabled
+  VICIntSelect &= ~VIC_BIT(VIC_I2C1);              // I2C1 selected as IRQ
+  VICIntEnable = VIC_BIT(VIC_I2C1);                // I2C1 interrupt enabled
   _VIC_CNTL(I2C1_VIC_SLOT) = VIC_ENABLE | VIC_I2C1;
   _VIC_ADDR(I2C1_VIC_SLOT) = (uint32_t)i2c1_ISR;    // address of the ISR
 }
diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c
index 34b11c7ed1..b150821e95 100644
--- a/sw/airborne/arm7/sys_time_hw.c
+++ b/sw/airborne/arm7/sys_time_hw.c
@@ -47,7 +47,7 @@ uint32_t sys_time_chrono;       /* T0TC ticks */
 #endif
 
 #ifdef USE_AMI601
-#include "peripherals/booz_ami601.h"
+#include "booz/peripherals/booz_ami601.h"
 #else
 #define AMI601_IT 0x00
 #endif
diff --git a/sw/airborne/booz/actuators/booz_actuators_mkk.c b/sw/airborne/booz/actuators/booz_actuators_mkk.c
index 15cbc8d86e..2eb1eac92a 100644
--- a/sw/airborne/booz/actuators/booz_actuators_mkk.c
+++ b/sw/airborne/booz/actuators/booz_actuators_mkk.c
@@ -43,6 +43,7 @@ void actuators_init(void) {
     actuators_mkk.trans[i].type = I2CTransTx;
     actuators_mkk.trans[i].len_w = 1;
     actuators_mkk.trans[i].slave_addr = actuators_addr[i];
+    actuators_mkk.trans[i].stop_after_transmit = TRUE;
     actuators_mkk.trans[i].status = I2CTransSuccess;
   }
   
diff --git a/sw/airborne/booz/peripherals/booz_ami601.c b/sw/airborne/booz/peripherals/booz_ami601.c
index 9249b97c3f..1eb127031a 100644
--- a/sw/airborne/booz/peripherals/booz_ami601.c
+++ b/sw/airborne/booz/peripherals/booz_ami601.c
@@ -16,10 +16,11 @@ void ami601_init( void ) {
     ami601_values[i] = 0;
   }
   ami601_i2c_trans.status = I2CTransSuccess;
+  ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR;
+  ami601_i2c_trans.stop_after_transmit = TRUE;
   ami601_nb_err = 0;
   ami601_status = AMI601_IDLE;
 
-
 }
 
 void ami601_read( void ) {
@@ -30,7 +31,6 @@ void ami601_read( void ) {
     ami601_status = AMI601_SENDING_REQ;
     ami601_i2c_trans.type = I2CTransTx;
     ami601_i2c_trans.len_w = 3;
-    ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR;
     ami601_i2c_trans.buf[0] = 0x55;
     ami601_i2c_trans.buf[1] = 0xAA;
     ami601_i2c_trans.buf[2] = 0x14;
diff --git a/sw/airborne/booz/peripherals/booz_ami601.h b/sw/airborne/booz/peripherals/booz_ami601.h
index a83529bbbd..4fa438acd9 100644
--- a/sw/airborne/booz/peripherals/booz_ami601.h
+++ b/sw/airborne/booz/peripherals/booz_ami601.h
@@ -53,7 +53,7 @@ extern volatile uint32_t ami601_nb_err;
 	ami601_foo3 = ami601_i2c_trans.buf[2]; /* ERR ? */		\
 	uint8_t i;							\
 	for (i=0; i< AMI601_NB_CHAN; i++) {				\
-	  ami601_values[i] = ami601_i2c_trans.buf[3 + 2 * i];		\
+	  ami601_values[i]  = ami601_i2c_trans.buf[3 + 2 * i];		\
 	  ami601_values[i] += ami601_i2c_trans.buf[3 + 2 * i + 1] * 256; \
 	}								\
 	ami601_status = AMI601_DATA_AVAILABLE;				\
@@ -70,7 +70,6 @@ extern volatile uint32_t ami601_nb_err;
     ami601_status =  AMI601_READING_MEASURE;				\
     ami601_i2c_trans.type = I2CTransRx;					\
     ami601_i2c_trans.len_r = 15;					\
-    ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR;			\
     i2c_submit(&i2c1, &ami601_i2c_trans);				\
   }
 
diff --git a/sw/airborne/i2c.h b/sw/airborne/i2c.h
index 8c1bc91352..1af8e378d0 100644
--- a/sw/airborne/i2c.h
+++ b/sw/airborne/i2c.h
@@ -129,40 +129,40 @@ extern void   i2c_init(struct i2c_periph* p);
 extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t);
 
 #define I2CReceive(_p, _t, _s_addr, _len) { \
-  _t.type = I2CTransRx; \
-  _t.slave_addr = _s_addr; \
-  _t.len_r = _len; \
-  _t.len_w = 0; \
-  _t.stop_after_transmit = TRUE; \
-  i2c_submit(&(_p),&(_t)); \
-}
+    _t.type = I2CTransRx;		    \
+    _t.slave_addr = _s_addr;		    \
+    _t.len_r = _len;			    \
+    _t.len_w = 0;			    \
+    _t.stop_after_transmit = TRUE;	    \
+    i2c_submit(&(_p),&(_t));		    \
+  }
 
-#define I2CTransmit(_p, _t, _s_addr, _len) { \
-  _t.type = I2CTransTx; \
-  _t.slave_addr = _s_addr; \
-  _t.len_r = 0; \
-  _t.len_w = _len; \
-  _t.stop_after_transmit = TRUE; \
-  i2c_submit(&(_p),&(_t)); \
-}
+#define I2CTransmit(_p, _t, _s_addr, _len) {	\
+    _t.type = I2CTransTx;			\
+    _t.slave_addr = _s_addr;			\
+    _t.len_r = 0;				\
+    _t.len_w = _len;				\
+    _t.stop_after_transmit = TRUE;		\
+    i2c_submit(&(_p),&(_t));			\
+  }
 
-#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \
-  _t.type = I2CTransTx; \
-  _t.slave_addr = _s_addr; \
-  _t.len_r = 0; \
-  _t.len_w = _len; \
-  _t.stop_after_transmit = FALSE; \
-  i2c_submit(&(_p),&(_t)); \
-}
+#define I2CTransmitNoStop(_p, _t, _s_addr, _len) {	\
+    _t.type = I2CTransTx;				\
+    _t.slave_addr = _s_addr;				\
+    _t.len_r = 0;					\
+    _t.len_w = _len;					\
+    _t.stop_after_transmit = FALSE;			\
+    i2c_submit(&(_p),&(_t));				\
+  }
 
 #define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \
-  _t.type = I2CTransTxRx; \
-  _t.slave_addr = _s_addr; \
-  _t.len_r = _len_r; \
-  _t.len_w = _len_w; \
-  _t.stop_after_transmit = TRUE; \
-  i2c_submit(&(_p),&(_t)); \
-}
+    _t.type = I2CTransTxRx;				 \
+    _t.slave_addr = _s_addr;				 \
+    _t.len_r = _len_r;					 \
+    _t.len_w = _len_w;					 \
+    _t.stop_after_transmit = TRUE;			 \
+    i2c_submit(&(_p),&(_t));				 \
+  }
 
 
 #endif /* I2C_H */
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index bd1e2fcff3..9304beb915 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -360,15 +360,15 @@ struct FloatRates {
 
 #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+	\
 				   SQUARE(_q.qy) + SQUARE(_q.qz)))	\
-
+    
 #define FLOAT_QUAT_NORMALISE(q) {		                        \
     float norm = FLOAT_QUAT_NORM(q);					\
-	  if (norm > FLT_MIN) {						 \
-      q.qi = q.qi / norm;							\
-      q.qx = q.qx / norm;							\
-      q.qy = q.qy / norm;							\
-      q.qz = q.qz / norm;							\
-		}																	\
+    if (norm > FLT_MIN) {						\
+	    q.qi = q.qi / norm;						\
+      q.qx = q.qx / norm;						\
+      q.qy = q.qy / norm;						\
+      q.qz = q.qz / norm;						\
+	  }								\
   }
 
 #define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
@@ -380,9 +380,9 @@ struct FloatRates {
 
 /* _a2c = _a2b comp _b2c , aka  _a2c = _a2b * _b2c */
 #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) {		\
-		FLOAT_QUAT_COMP(_a2c, _a2b, _b2c);										\
-		FLOAT_QUAT_WRAP_SHORTEST(_a2c);												\
-		FLOAT_QUAT_NORMALISE(_a2c);														\
+    FLOAT_QUAT_COMP(_a2c, _a2b, _b2c);					\
+    FLOAT_QUAT_WRAP_SHORTEST(_a2c);					\
+    FLOAT_QUAT_NORMALISE(_a2c);						\
   }
 
 /* _a2c = _a2b comp _b2c , aka  _a2c = _a2b * _b2c */
@@ -396,10 +396,10 @@ struct FloatRates {
 #define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
 
 /* _a2b = _a2c comp_inv _b2c , aka  _a2b = _a2c * inv(_b2c) */
-#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) {				\
-		FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); 													\
-		FLOAT_QUAT_WRAP_SHORTEST(_a2b);																	\
-		FLOAT_QUAT_NORMALISE(_a2b);																			\
+#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) {		\
+    FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c);				\
+    FLOAT_QUAT_WRAP_SHORTEST(_a2b);					\
+    FLOAT_QUAT_NORMALISE(_a2b);						\
   }
 
 /* _a2b = _a2c comp_inv _b2c , aka  _a2b = _a2c * inv(_b2c) */
@@ -411,11 +411,11 @@ struct FloatRates {
   }
 
 /* _b2c = _a2b inv_comp _a2c , aka  _b2c = inv(_a2b) * _a2c */
-#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) {				\
-		FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); 													\
-		FLOAT_QUAT_WRAP_SHORTEST(_b2c);																	\
-		FLOAT_QUAT_NORMALISE(_b2c);																			\
-}
+#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) {		\
+    FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c);				\
+    FLOAT_QUAT_WRAP_SHORTEST(_b2c);					\
+    FLOAT_QUAT_NORMALISE(_b2c);						\
+  }
 
 /* _b2c = _a2b inv_comp _a2c , aka  _b2c = inv(_a2b) * _a2c */
 #define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) {				\
@@ -425,28 +425,28 @@ struct FloatRates {
     (_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
   }
 
-#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) {                         \
-  const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
-  const float c2 = cos(dt*v_norm/2.0);                                     \
-  const float s2 = sin(dt*v_norm/2.0);                                     \
-  if (v_norm < 1e-8) {                                                  \
-    (q_out).qi = 1;                                                     \
-    (q_out).qx = 0;                                                     \
-    (q_out).qy = 0;                                                     \
-    (q_out).qz = 0;                                                     \
-  } else {                                                              \
-    (q_out).qi = c2;                                                      \
-    (q_out).qx = (w).p/v_norm * s2;                                       \
-    (q_out).qy = (w).q/v_norm * s2;                                       \
-    (q_out).qz = (w).r/v_norm * s2;                                       \
-  }                                                                     \
-}
+#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) {			      \
+    const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
+    const float c2 = cos(dt*v_norm/2.0);				\
+    const float s2 = sin(dt*v_norm/2.0);				\
+    if (v_norm < 1e-8) {						\
+      (q_out).qi = 1;							\
+      (q_out).qx = 0;							\
+      (q_out).qy = 0;							\
+      (q_out).qz = 0;							\
+    } else {								\
+      (q_out).qi = c2;							\
+      (q_out).qx = (w).p/v_norm * s2;					\
+      (q_out).qy = (w).q/v_norm * s2;					\
+      (q_out).qz = (w).r/v_norm * s2;					\
+    }									\
+  }
 
 #define FLOAT_QUAT_ROTATE_FRAME(q_out, q_in, q_rot) {                   \
-  struct FloatQuat q_temp;                                              \
-  FLOAT_QUAT_INV_COMP(q_temp, q_rot, q_in);                             \
-  print_quat(q_temp);                                                   \
-  FLOAT_QUAT_COMP(q_out, q_temp, q_rot);                                \
+    struct FloatQuat q_temp;						\
+    FLOAT_QUAT_INV_COMP(q_temp, q_rot, q_in);				\
+    print_quat(q_temp);							\
+    FLOAT_QUAT_COMP(q_out, q_temp, q_rot);				\
   }
 
 #define FLOAT_QUAT_VMULT(v_out, q, v_in) {				\
@@ -475,7 +475,7 @@ struct FloatRates {
   }
 
 /* _qd = -0.5*omega(_r) * _q  */
-#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) {				 \
+#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) {				\
     (_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
     (_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
     (_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
@@ -483,9 +483,9 @@ struct FloatRates {
   }
 
 /* _qd = -0.5*omega(_r) * _q  */
-#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) {			 \
-    const float K_LAGRANGE = 1.;					 \
-    const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5;	 \
+#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) {			\
+    const float K_LAGRANGE = 1.;					\
+    const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5;	\
     (_qd).qi = -0.5*(      c*(_q).qi + (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
     (_qd).qx = -0.5*(-(_r).p*(_q).qi +      c*(_q).qx - (_r).r*(_q).qy + (_r).q*(_q).qz); \
     (_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx +      c*(_q).qy - (_r).p*(_q).qz); \
@@ -497,7 +497,7 @@ struct FloatRates {
     const float phi2   = (_e).phi/2.0;					\
     const float theta2 = (_e).theta/2.0;				\
     const float psi2   = (_e).psi/2.0;					\
-									\
+    									\
     const float s_phi2   = sinf( phi2 );				\
     const float c_phi2   = cosf( phi2 );				\
     const float s_theta2 = sinf( theta2 );				\
diff --git a/sw/airborne/test/Makefile b/sw/airborne/test/Makefile
index c6dbcbe6eb..13aeae8f7d 100644
--- a/sw/airborne/test/Makefile
+++ b/sw/airborne/test/Makefile
@@ -14,6 +14,9 @@ test_geodetic: test_geodetic.c ../math/pprz_geodetic_float.c ../math/pprz_geodet
 test_algebra: test_algebra.c ../math/pprz_trig_int.c
 	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
+test_martin:  test_martin.c ../math/pprz_trig_int.c
+	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
 test_bla: test_bla.c ../math/pprz_trig_int.c
 	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
diff --git a/sw/airborne/test/test_esc_mkk_simple.c b/sw/airborne/test/test_esc_mkk_simple.c
index 3011a1acdf..1d8917a9fe 100644
--- a/sw/airborne/test/test_esc_mkk_simple.c
+++ b/sw/airborne/test/test_esc_mkk_simple.c
@@ -59,7 +59,8 @@ static inline void main_periodic_task( void ) {
   trans.buf[0] = 0x04;
   trans.len_w = 1;
   trans.slave_addr = 0x58;
-  i2c_submit(&i2c1,&trans);
+  trans.stop_after_transmit = TRUE;
+  i2c_submit(&ACTUATORS_MKK_DEV,&trans);
 
   LED_PERIODIC();