diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml
index e8a23c9ce3..9466b2a0f8 100644
--- a/conf/airframes/booz2_a1.xml
+++ b/conf/airframes/booz2_a1.xml
@@ -160,6 +160,7 @@ include $(PAPARAZZI_SRC)/conf/autopilot/booz2_test_progs.makefile
 sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a1.h\"
 include $(PAPARAZZI_SRC)/conf/autopilot/booz2_simulator.makefile
 
+ap.CFLAGS += -DUART1_BAUD=B57600
 include $(PAPARAZZI_SRC)/conf/autopilot/booz2_actuators_buss.makefile
 include $(PAPARAZZI_SRC)/conf/autopilot/booz2_imu_b2v1.makefile
 
diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml
index 0db5de665d..06a0e4a848 100644
--- a/conf/airframes/motor_bench.xml
+++ b/conf/airframes/motor_bench.xml
@@ -34,8 +34,9 @@ main.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
 main.srcs += $(MB)/mb_servo.c
 
+main.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=3
 main.CFLAGS += -DUSE_TWI_CONTROLLER
-main.CFLAGS += -DI2C_SCLL=100 -DI2C_SCLH=100
+#main.CFLAGS += -DI2C_SCLL=100 -DI2C_SCLH=100
 #main.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(MB)/mb_twi_controller.c
 main.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(MB)/mb_twi_controller_asctech.c
 #main.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(MB)/mb_twi_controller_mkk.c
@@ -54,7 +55,46 @@ main.srcs += $(MB)/mb_modes.c
 main.srcs += $(MB)/mb_static.c
 main.srcs += $(MB)/mb_mode_fixed_rpm.c
 
+foo.ARCHDIR = $(ARCHI)
+foo.ARCH = arm7tdmi
+foo.TARGET = mb
+foo.TARGETDIR = mb
 
+foo.CFLAGS += -DCONFIG=\"conf_motor_bench.h\" -I$(MB)
+foo.srcs = $(MB)/main_foo.c
+
+foo.CFLAGS += -DPERIODIC_TASK_FREQ='250.'
+foo.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./PERIODIC_TASK_FREQ)'
+foo.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+foo.CFLAGS += -DLED
+foo.srcs += $(SRC_ARCH)/armVIC.c
+
+foo.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=9
+foo.CFLAGS += -DUSE_TWI_CONTROLLER
+foo.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(MB)/mb_twi_controller_asctech.c
+
+
+
+
+bar.ARCHDIR = $(ARCHI)
+bar.ARCH = arm7tdmi
+bar.TARGET = mb
+bar.TARGETDIR = mb
+
+bar.CFLAGS += -DCONFIG=\"conf_motor_bench.h\" -I$(MB)
+bar.srcs = $(MB)/main_bar.c
+
+bar.CFLAGS += -DPERIODIC_TASK_FREQ='250.'
+bar.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./PERIODIC_TASK_FREQ)'
+bar.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+bar.CFLAGS += -DLED
+bar.srcs += $(SRC_ARCH)/armVIC.c
+
+bar.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=9
+bar.CFLAGS += -DUSE_TWI_CONTROLLER
+bar.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(MB)/mb_twi_controller_mkk.c
 
   </makefile>
 
diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index da1f39e620..ca34f72db9 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -44,7 +44,8 @@ ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
 ap.CFLAGS += -DLED
 ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 
-ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
+# -DUART1_BAUD=B57600
+ap.CFLAGS += -DUSE_UART1  -DUART1_VIC_SLOT=6
 ap.srcs += $(SRC_ARCH)/uart_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 
diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile
index 218c032cba..53a70ca044 100644
--- a/conf/autopilot/booz2_test_progs.makefile
+++ b/conf/autopilot/booz2_test_progs.makefile
@@ -142,7 +142,8 @@ test_usb.TARGETDIR = test_usb
 
 test_usb.CFLAGS += -DCONFIG=\"booz2_board_usb.h\" $(BOOZ_CFLAGS)
 test_usb.srcs += $(SRC_BOOZ_TEST)/booz2_test_usb.c
-test_usb.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIME_LED=1
+test_usb.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+# -DTIME_LED=1
 test_usb.CFLAGS += -DLED
 test_usb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 
@@ -312,7 +313,7 @@ test_rc.TARGET = test_rc
 test_rc.TARGETDIR = test_rc
 
 test_rc.CFLAGS += -DCONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
-test_rc.srcs += $(BOOZ_PRIV_TEST)/booz2_test_rc.c
+test_rc.srcs += $(SRC_BOOZ_TEST)/booz2_test_rc.c
 test_rc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIME_LED=4
 test_rc.CFLAGS += -DLED
 test_rc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
@@ -360,8 +361,8 @@ test_amc.ARCH = arm7tdmi
 test_amc.TARGET = test_amc
 test_amc.TARGETDIR = test_amc
 
-test_amc.CFLAGS += -DCONFIG=$(BOARD_CFG) -I$(BOOZ_PRIV) -I$(BOOZ_PRIV_ARCH) -I$(BOOZ_ARCH)
-test_amc.srcs += $(BOOZ_PRIV_TEST)/booz2_test_amc.c
+test_amc.CFLAGS += -DCONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) 
+test_amc.srcs += $(SRC_BOOZ_TEST)/booz2_test_amc.c
 test_amc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIME_LED=1
 test_amc.CFLAGS += -DLED
 test_amc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
@@ -372,15 +373,15 @@ test_amc.srcs += $(SRC_ARCH)/uart_hw.c
 test_amc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
 test_amc.srcs += downlink.c pprz_transport.c
 test_amc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-test_amc.srcs += $(BOOZ_PRIV)/booz2_datalink.c
+test_amc.srcs += $(SRC_BOOZ)/booz2_datalink.c
 
 test_amc.CFLAGS += -DACTUATORS=\"actuators_asctec_twi_blmc_hw.h\"
-test_amc.srcs += $(BOOZ_PRIV_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c
+test_amc.srcs += $(SRC_BOOZ_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c
 test_amc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
 test_amc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 
-test_amc.CFLAGS += -DFLOAT_T=float
-
+test_amc.CFLAGS += -DFLOAT_T=float 
+#-DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
 
 #
 # test 24 bits baro
diff --git a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
index 511c48166d..41d08c2f55 100644
--- a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
+++ b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
@@ -170,8 +170,6 @@ void booz_ahrs_propagate(void) {
   INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat);
   /* compute LTP to BODY eulers */
   INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat);
-  //  booz_ahrs.ltp_to_body_euler.phi   = booz_ahrs.ltp_to_imu_euler.phi - IMU_BODY_TO_IMU_PHI;
-  //booz_ahrs.ltp_to_body_euler.theta = booz_ahrs.ltp_to_imu_euler.theta - IMU_BODY_TO_IMU_THETA;
 
   /* Do we compute actual body rate ? */
   RATES_COPY(booz_ahrs.body_rate, booz_ahrs.imu_rate);
diff --git a/sw/airborne/booz/test/booz2_test_rc.c b/sw/airborne/booz/test/booz2_test_rc.c
new file mode 100644
index 0000000000..b98ed73e68
--- /dev/null
+++ b/sw/airborne/booz/test/booz2_test_rc.c
@@ -0,0 +1,55 @@
+#include <inttypes.h>
+
+#include "std.h"
+#include "init_hw.h"
+#include "sys_time.h"
+#include "led.h"
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "interrupt_hw.h"
+
+#include "radio_control.h"
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void );
+
+static void on_rc_event(void);
+
+int main( void ) {
+  main_init();
+  while(1) {
+    if (sys_time_periodic())
+      main_periodic_task();
+    main_event_task();
+  }
+  return 0;
+}
+
+static inline void main_init( void ) {
+  hw_init();
+  sys_time_init();
+  led_init();
+
+  uart1_init_tx();
+
+  ppm_init();
+  radio_control_init();
+
+  int_enable();
+}
+
+static inline void main_periodic_task( void ) {
+  RunOnceEvery(100, {LED_TOGGLE(3); DOWNLINK_SEND_BOOT(&cpu_time_sec);});
+  RunOnceEvery(10, { radio_control_periodic_task(); });
+}
+
+static inline void main_event_task( void ) {
+  RadioControlEventCheckAndHandle(on_rc_event);
+}
+
+static void on_rc_event(void) {
+
+}
diff --git a/sw/airborne/booz/test/booz2_test_usb.c b/sw/airborne/booz/test/booz2_test_usb.c
index 5a4bfecba2..8a7592ba2a 100644
--- a/sw/airborne/booz/test/booz2_test_usb.c
+++ b/sw/airborne/booz/test/booz2_test_usb.c
@@ -45,7 +45,7 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic_task( void ) {
-  RunOnceEvery(100, {
+  RunOnceEvery(10, {
       LED_TOGGLE(1);
       DOWNLINK_SEND_ALIVE(16, MD5SUM);
     });
diff --git a/sw/airborne/fms/fms_ap_link.c b/sw/airborne/fms/fms_ap_link.c
index f32e01b27a..d57c22e315 100644
--- a/sw/airborne/fms/fms_ap_link.c
+++ b/sw/airborne/fms/fms_ap_link.c
@@ -1,6 +1,8 @@
 #include "fms_ap_link.h"
 
 #include <stdlib.h>
+
+#include "fms_debug.h"
 #include "fms_serial_port.h"
 
 
@@ -33,7 +35,7 @@ void ap_link_free(struct FmsApLink* me) {
 
 void ap_link_parse(struct FmsApLink* me, int nb_bytes) {
 
-  printf("got %d bytes\n", nb_bytes);
+  TRACE(TRACE_DEBUG, "ap link parsing %d bytes \n", nb_bytes);
 
 }
 
diff --git a/sw/airborne/fms/fms_ap_link.h b/sw/airborne/fms/fms_ap_link.h
index 2f6025a891..31ece9240b 100644
--- a/sw/airborne/fms/fms_ap_link.h
+++ b/sw/airborne/fms/fms_ap_link.h
@@ -3,7 +3,8 @@
 
 #include "fms_serial_port.h"
 
-#define AP_LINK_BUF_SIZE 4096
+//#define AP_LINK_BUF_SIZE 4096
+#define AP_LINK_BUF_SIZE 256
 
 struct FmsApLink {
   struct FmsSerialPort* sp;
diff --git a/sw/airborne/fms/fms_gs_link.c b/sw/airborne/fms/fms_gs_link.c
index 4cd8a2bb4c..39e01c3389 100644
--- a/sw/airborne/fms/fms_gs_link.c
+++ b/sw/airborne/fms/fms_gs_link.c
@@ -1,8 +1,9 @@
-
 #include "fms_gs_link.h"
 
 #include <stdlib.h>
 
+#include "fms_debug.h"
+
 struct FmsGsLink* gs_link_new(const char* str_ip, const int port) {
 
   struct FmsGsLink* me = malloc(sizeof(struct FmsGsLink));
@@ -11,3 +12,13 @@ struct FmsGsLink* gs_link_new(const char* str_ip, const int port) {
 
 }
 
+
+
+int gs_link_write(struct FmsGsLink* me, char* buf, int len) {
+
+  TRACE(TRACE_DEBUG, "gs_link writing %d bytes to network\n", len);
+  return network_write(me->network, buf, len);
+
+}
+
+
diff --git a/sw/airborne/fms/fms_gs_link.h b/sw/airborne/fms/fms_gs_link.h
index b8ad59cee9..3c39a75425 100644
--- a/sw/airborne/fms/fms_gs_link.h
+++ b/sw/airborne/fms/fms_gs_link.h
@@ -9,6 +9,7 @@ struct FmsGsLink {
 
 extern struct FmsGsLink* gs_link_new(const char* str_ip, const int port);
 
+extern int gs_link_write(struct FmsGsLink* me, char* buf, int len);
 
 
 #endif /* FMS_GS_LINK_H */
diff --git a/sw/airborne/fms/fms_main.c b/sw/airborne/fms/fms_main.c
index 4eef40a93d..138b407c92 100644
--- a/sw/airborne/fms/fms_main.c
+++ b/sw/airborne/fms/fms_main.c
@@ -1,16 +1,22 @@
+// nc -l -u -p 2442 > /tmp/test123
 
 #include <glib.h>
 
 #include <stdio.h>
 
 #define AP_DEVICE "/dev/ttyACM0"
-#define GS_IP     "192.168.1.8"
+#define GS_IP     "10.31.4.19"
 #define GS_PORT   2442
 
+#include "fms_debug.h"
 #include "fms_ap_link.h"
 #include "fms_gs_link.h"
 
 
+static struct FmsApLink* ap_link;
+static struct FmsGsLink* gs_link;
+
+
 static gboolean on_ap_link_data_received(GIOChannel *source,
 					 GIOCondition condition,
 					 gpointer data);
@@ -23,18 +29,18 @@ static gboolean on_gs_link_data_received(GIOChannel *source,
 static gboolean on_ap_link_data_received(GIOChannel *source,
 					 GIOCondition condition,
 					 gpointer data) {
-  struct FmsApLink* ap_link = (struct FmsApLink*)data;
   gsize bytes_read;
   GError* _err = NULL;
   GIOStatus st = g_io_channel_read_chars(source, ap_link->buf, AP_LINK_BUF_SIZE, &bytes_read, &_err);
-  printf("in on_ap_link_data_received %d %d\n", st, _err);
-  if (_err != NULL) {
-    fprintf (stderr, "error reading serial: %s\n", _err->message);
-    g_error_free (_err);
-
+  if (!_err) {
+    if (st == G_IO_STATUS_NORMAL) {
+      ap_link_parse(ap_link, bytes_read);
+      gs_link_write(gs_link, ap_link->buf, bytes_read);
+    }
   }
-  if (st == G_IO_STATUS_NORMAL) {
-    ap_link_parse(ap_link, bytes_read);
+  else {
+    TRACE(TRACE_ERROR,"error reading serial: %s\n", _err->message);
+    g_error_free (_err);
   }
   return TRUE;
 }
@@ -48,8 +54,6 @@ static gboolean on_gs_link_data_received(GIOChannel *source,
 
 int main(int argc, char** argv) {
 
-  struct FmsApLink* ap_link;
-  struct FmsGsLink* gs_link;
   ap_link = ap_link_new(AP_DEVICE);
   if (!ap_link) {
     printf("error opening serial port %s\n", AP_DEVICE);
@@ -57,11 +61,15 @@ int main(int argc, char** argv) {
   }
   GIOChannel* ioc1 = g_io_channel_unix_new(ap_link->sp->fd);
   g_io_channel_set_encoding(ioc1, NULL, NULL);
-  g_io_add_watch (ioc1, G_IO_IN, on_ap_link_data_received, ap_link);
+  g_io_add_watch (ioc1, G_IO_IN, on_ap_link_data_received, NULL);
 
-  //  gs_link = gs_link_new(GS_IP, GS_PORT);
-  //  GIOChannel* ioc2 = g_io_channel_unix_new(gs_link->network->fd);
-  //  g_io_add_watch (ioc2, G_IO_IN, on_gs_link_data_received, ap_link);
+  gs_link = gs_link_new(GS_IP, GS_PORT);
+  if (!gs_link) {
+    printf("error opening network connection (%s:%d)\n", GS_IP, GS_PORT);
+    return -1;
+  }
+  GIOChannel* ioc2 = g_io_channel_unix_new(gs_link->network->socket);
+  g_io_add_watch (ioc2, G_IO_IN, on_gs_link_data_received, NULL);
 
   GMainLoop* ml = g_main_loop_new(NULL, FALSE);
   g_main_loop_run(ml);
diff --git a/sw/airborne/fms/fms_network.c b/sw/airborne/fms/fms_network.c
index 279767e303..f02af8552e 100644
--- a/sw/airborne/fms/fms_network.c
+++ b/sw/airborne/fms/fms_network.c
@@ -1,11 +1,11 @@
 #include "fms_network.h"
 
-
-
 #include <netdb.h>
 #include <netinet/in.h>
 #include <stdlib.h>
 
+#include "fms_debug.h"
+
 struct FmsNetwork* network_new(const char* str_ip, const int port) {
 
   struct FmsNetwork* me = malloc(sizeof(struct FmsNetwork));
@@ -13,8 +13,8 @@ struct FmsNetwork* network_new(const char* str_ip, const int port) {
   // struct FgNetChannel* chan = malloc(sizeof (struct FgNetChannel));
   int so_reuseaddr = 1;
   struct protoent * pte = getprotobyname("UDP");
-  me->fd = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
-  setsockopt(me->fd, SOL_SOCKET, SO_REUSEADDR, 
+  me->socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
+  setsockopt(me->socket, SOL_SOCKET, SO_REUSEADDR, 
              &so_reuseaddr, sizeof(so_reuseaddr));
 
   me->addr.sin_family = PF_INET;
@@ -24,3 +24,14 @@ struct FmsNetwork* network_new(const char* str_ip, const int port) {
   return me;
 
 }
+
+
+
+int network_write(struct FmsNetwork* me, char* buf, int len) {
+  ssize_t byte_written = sendto(me->socket, buf, len, 0, 
+				(struct sockaddr*)&me->addr, sizeof(me->addr));
+  if ( byte_written != len) {
+    TRACE(TRACE_ERROR, "error sending to network %d\n", byte_written);
+  }
+  return len;
+}
diff --git a/sw/airborne/fms/fms_network.h b/sw/airborne/fms/fms_network.h
index f22845a299..a160e3af7e 100644
--- a/sw/airborne/fms/fms_network.h
+++ b/sw/airborne/fms/fms_network.h
@@ -5,12 +5,13 @@
 #include <arpa/inet.h>
 
 struct FmsNetwork {
-  int fd;
+  int socket;
   struct sockaddr_in addr;
 };
 
 
 extern struct FmsNetwork* network_new(const char* str_ip, const int port);
+extern int network_write(struct FmsNetwork* me, char* buf, int len);
 
 #endif /* FMS_NETWORK_H */
 
diff --git a/sw/airborne/motor_bench/mb_twi_controller.c b/sw/airborne/motor_bench/mb_twi_controller.c
index 58b32c095d..f693dc1978 100644
--- a/sw/airborne/motor_bench/mb_twi_controller.c
+++ b/sw/airborne/motor_bench/mb_twi_controller.c
@@ -16,6 +16,9 @@ void mb_twi_controller_init(void) {
 }
 
 void mb_twi_controller_set( float throttle ) {
+  
+  LED_TOGGLE(1);
+
   if (mb_twi_i2c_done) {
     mb_twi_command = throttle * MB_TWI_CONTROLLER_MAX_CMD;
     i2c_buf[0] = (uint8_t)(mb_twi_command&0xFF);
diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.c b/sw/airborne/motor_bench/mb_twi_controller_asctech.c
index 1bf465b7b1..5f4277d0f6 100644
--- a/sw/airborne/motor_bench/mb_twi_controller_asctech.c
+++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.c
@@ -31,6 +31,7 @@ void mb_twi_controller_init(void) {
 }
 
 void mb_twi_controller_set( float throttle ) {
+
   if (mb_twi_i2c_done) {
     if (mb_twi_controller_asctech_command) {
       mb_twi_controller_asctech_command = FALSE;
@@ -41,6 +42,7 @@ void mb_twi_controller_set( float throttle ) {
 	i2c_buf[1] = mb_twi_controller_asctech_addr;
 	i2c_buf[2] = 0;
 	i2c_buf[3] = 231 + mb_twi_controller_asctech_addr;
+	//	mb_twi_i2c_done = FALSE;
 	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
       break;
       
@@ -49,6 +51,7 @@ void mb_twi_controller_set( float throttle ) {
 	i2c_buf[1] = mb_twi_controller_asctech_addr;
 	i2c_buf[2] = 0;
 	i2c_buf[3] = 234 + mb_twi_controller_asctech_addr;
+	//	mb_twi_i2c_done = FALSE;
 	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
 	break;
    
@@ -59,12 +62,14 @@ void mb_twi_controller_set( float throttle ) {
 	i2c_buf[3] = 230 + mb_twi_controller_asctech_addr + 
 	             mb_twi_controller_asctech_new_addr;
 	mb_twi_controller_asctech_addr = mb_twi_controller_asctech_new_addr;
+	//	mb_twi_i2c_done = FALSE;
 	i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
 	break;
 
       }
     }
     else {
+
       uint8_t pitch = 100;
       uint8_t roll  = 100;
       uint8_t yaw   = 100;
@@ -73,6 +78,7 @@ void mb_twi_controller_set( float throttle ) {
       i2c_buf[1] = roll;
       i2c_buf[2] = yaw;
       i2c_buf[3] = power;
+      //      mb_twi_i2c_done = FALSE;
       i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
     }
   }
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 9ee10eb74a..e386e5668f 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -1,3 +1,4 @@
+
 # Paparazzi simulator $Id$
 #  
 # Copyright (C) 2003-2006  Pascal Brisset, Antoine Drouin