diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml
index 77ea475b39..8b2c36cd0e 100644
--- a/conf/airframes/booz.xml
+++ b/conf/airframes/booz.xml
@@ -63,7 +63,7 @@ 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.srcs += downlink.c pprz_transport.c 
 
 ctl.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
 
@@ -76,6 +76,10 @@ ctl.srcs += booz_estimator.c booz_control.c
 ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
+ctl.srcs += commands.c
+
+ctl.srcs += booz_telemetry.c
+
 #
 # AHRS CPU
 #
diff --git a/conf/messages.xml b/conf/messages.xml
index f0f77efacb..404d96410d 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -432,6 +432,9 @@
   <message name="BOOZ_STATUS" ID="215">
     <field name="link_imu_nb_err" type="uint32"/>
     <field name="imu_status" type="uint8"/>
+    <field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
+<!--    <field name="mode" type="uint8" values="MANUAL|AUTO1|AUTO2|FAILSAFE"/> -->
+<!--    <field name="vsupply" type="uint8" unit="decivolt"/> -->
   </message>
 
   <message name="ENOSE_STATUS" ID="218">
diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml
index b580f8a4bc..48114074cc 100644
--- a/conf/telemetry/booz.xml
+++ b/conf/telemetry/booz.xml
@@ -1,16 +1,14 @@
 <?xml version="1.0"?>
 <!DOCTYPE telemetry SYSTEM "telemetry.dtd">
 <telemetry>
-  <process name="Ap">
-    <mode name="default">
-    </mode>
-    <mode name="debug">
-    </mode>
-  </process>
-  <process name="Fbw">
+
+  <process name="Main">
     <mode name="default">
+      <message name="BOOZ_STATUS"       period="1."/>
     </mode>
     <mode name="debug">
     </mode>
   </process>
+
+
 </telemetry>
diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c
index b045c507eb..9019d13606 100644
--- a/sw/airborne/booz_control.c
+++ b/sw/airborne/booz_control.c
@@ -2,6 +2,7 @@
 
 #include "booz_estimator.h"
 #include "commands.h"
+#include "radio_control.h"
 
 float booz_control_p_sp;
 float booz_control_q_sp;
diff --git a/sw/airborne/booz_downlink.h b/sw/airborne/booz_downlink.h
index 7e31878201..255f0724ce 100644
--- a/sw/airborne/booz_downlink.h
+++ b/sw/airborne/booz_downlink.h
@@ -2,10 +2,10 @@
 #define BOOZ_DOWNLINK_H
 
 #include "std.h"
+#include "periodic.h"
 
 static inline void booz_downlink_periodic_task(void) {
-
-
+  PeriodicSendMain()
 }
 
 
diff --git a/sw/airborne/imu_v3.c b/sw/airborne/imu_v3.c
index 1601b454d9..84625028d3 100644
--- a/sw/airborne/imu_v3.c
+++ b/sw/airborne/imu_v3.c
@@ -27,6 +27,7 @@ struct adc_buf buf_bat;
 
 #define IMU_DETECT_STILL_LEN 128
 bool_t imu_vehicle_still;
+float imu_vs_gyro_initial_bias[AXIS_NB];
 
 uint16_t imu_vs_accel_raw[AXIS_NB][IMU_DETECT_STILL_LEN];
 uint32_t imu_vs_accel_raw_sum[AXIS_NB];
@@ -64,6 +65,7 @@ void imu_init(void) {
     imu_vs_accel_raw_sum[i] = 0;
     imu_vs_gyro_raw_sum[i] = 0;
     imu_vs_mag_raw_sum[i] = 0;
+    imu_vs_gyro_initial_bias[i] = 0.;
     for (j=0; j<IMU_DETECT_STILL_LEN; j++) {
       imu_vs_accel_raw[i][j] = 0;
       imu_vs_gyro_raw[i][j] = 0;
diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h
index 98506a661f..d127204d94 100644
--- a/sw/airborne/imu_v3.h
+++ b/sw/airborne/imu_v3.h
@@ -20,6 +20,7 @@ extern int16_t  imu_mag_raw[AXIS_NB];
 extern void imu_init(void);
 extern void imu_detect_vehicle_still(void);
 extern bool_t imu_vehicle_still;
+extern float imu_vs_gyro_initial_bias[AXIS_NB];
 
 /* sliding average for still vehicle detection */
 extern uint32_t imu_vs_accel_raw_avg[AXIS_NB];
@@ -51,13 +52,13 @@ extern struct adc_buf buf_bat;
     imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (buf_az.sum - IMU_ACCEL_Z_NEUTRAL); \
 }
 
-#define IMU_GYRO_X_NEUTRAL 40885
-#define IMU_GYRO_Y_NEUTRAL 40910
-#define IMU_GYRO_Z_NEUTRAL 39552
+#define IMU_GYRO_P_NEUTRAL 40885
+#define IMU_GYRO_Q_NEUTRAL 40910
+#define IMU_GYRO_R_NEUTRAL 39552
 
-#define IMU_GYRO_X_GAIN -0.0002202
-#define IMU_GYRO_Y_GAIN -0.0002160
-#define IMU_GYRO_Z_GAIN  0.0002104
+#define IMU_GYRO_P_GAIN -0.0002202
+#define IMU_GYRO_Q_GAIN -0.0002150
+#define IMU_GYRO_R_GAIN  0.0002104
 
 #define ImuUpdateGyros() {						\
     imu_gyro_prev[AXIS_X] = imu_gyro[AXIS_X];				\
@@ -66,12 +67,12 @@ extern struct adc_buf buf_bat;
     imu_gyro_raw[AXIS_X] = max1167_values[0];				        \
     imu_gyro_raw[AXIS_Y] = max1167_values[1];				        \
     imu_gyro_raw[AXIS_Z] = max1167_values[2];				        \
-    imu_gyro[AXIS_X] = (float)((int32_t)max1167_values[0] - IMU_GYRO_X_NEUTRAL) \
-                        * IMU_GYRO_X_GAIN;				        \
-    imu_gyro[AXIS_Y] = (float)((int32_t)max1167_values[1] - IMU_GYRO_Y_NEUTRAL) \
-                        * IMU_GYRO_Y_GAIN;				        \
-    imu_gyro[AXIS_Z] = (float)((int32_t)max1167_values[2] - IMU_GYRO_Z_NEUTRAL) \
-                        * IMU_GYRO_Z_GAIN;				        \
+    imu_gyro[AXIS_X] = (float)((int32_t)max1167_values[0] - IMU_GYRO_P_NEUTRAL) \
+                        * IMU_GYRO_P_GAIN;				        \
+    imu_gyro[AXIS_Y] = (float)((int32_t)max1167_values[1] - IMU_GYRO_Q_NEUTRAL) \
+                        * IMU_GYRO_Q_GAIN;				        \
+    imu_gyro[AXIS_Z] = (float)((int32_t)max1167_values[2] - IMU_GYRO_R_NEUTRAL) \
+                        * IMU_GYRO_R_GAIN;				        \
   }
 
 #define ImuUpdateMag() {						\
@@ -98,15 +99,18 @@ extern struct adc_buf buf_bat;
     imu_accel[AXIS_X] =	IMU_ACCEL_X_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_X] - IMU_ACCEL_X_NEUTRAL); \
     imu_accel[AXIS_Y] =	IMU_ACCEL_Y_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_Y] - IMU_ACCEL_Y_NEUTRAL); \
     imu_accel[AXIS_Z] =	IMU_ACCEL_Z_GAIN *((int32_t)imu_vs_accel_raw_avg[AXIS_Z] - IMU_ACCEL_Z_NEUTRAL); \
-    imu_gyro[AXIS_X] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_X] - IMU_GYRO_X_NEUTRAL)       \
-                        * IMU_GYRO_X_GAIN;						        \
-    imu_gyro[AXIS_Y] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Y] - IMU_GYRO_Y_NEUTRAL)       \
-                        * IMU_GYRO_Y_GAIN;				                        \
-    imu_gyro[AXIS_Z] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Z] - IMU_GYRO_Z_NEUTRAL)       \
-                        * IMU_GYRO_Z_GAIN;				                        \
-    imu_gyro_prev[AXIS_X] = imu_gyro[AXIS_X];				\
-    imu_gyro_prev[AXIS_Y] = imu_gyro[AXIS_Y];				\
-    imu_gyro_prev[AXIS_Z] = imu_gyro[AXIS_Z];				\
+    imu_gyro[AXIS_X] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_P] - IMU_GYRO_P_NEUTRAL)       \
+                        * IMU_GYRO_P_GAIN;						        \
+    imu_gyro[AXIS_Y] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_Q] - IMU_GYRO_Q_NEUTRAL)       \
+                        * IMU_GYRO_Q_GAIN;				                        \
+    imu_gyro[AXIS_Z] = (float)((int32_t)imu_vs_gyro_raw_avg[AXIS_R] - IMU_GYRO_R_NEUTRAL)       \
+                        * IMU_GYRO_R_GAIN;				                        \
+    imu_gyro_prev[AXIS_P] = imu_gyro[AXIS_P];				\
+    imu_gyro_prev[AXIS_Q] = imu_gyro[AXIS_Q];				\
+    imu_gyro_prev[AXIS_R] = imu_gyro[AXIS_R];				\
+    imu_vs_gyro_initial_bias[AXIS_P] = imu_gyro[AXIS_P];		\
+    imu_vs_gyro_initial_bias[AXIS_Q] = imu_gyro[AXIS_Q];		\
+    imu_vs_gyro_initial_bias[AXIS_R] = imu_gyro[AXIS_R];		\
 }
 
 
diff --git a/sw/airborne/main_booz.c b/sw/airborne/main_booz.c
index 9fa02b1d3d..1f57e592c8 100644
--- a/sw/airborne/main_booz.c
+++ b/sw/airborne/main_booz.c
@@ -10,7 +10,7 @@
 #include "uart.h"
 #include "messages.h"
 #include "downlink.h"
-#include "booz_downlink.h"
+#include "booz_telemetry.h"
 
 #include "spi.h"
 #include "link_imu.h"
@@ -35,7 +35,13 @@ static inline void main_init( void ) {
   hw_init();
   led_init();
   sys_time_init();
+
+  //  actuators_init();
+  //  SetCommands(commands_failsafe);
+
+  ppm_init();
   radio_control_init();
+
   spi_init();
   link_imu_init();
   uart1_init_tx();
@@ -44,16 +50,21 @@ static inline void main_init( void ) {
 
 static inline void main_periodic_task( void ) {
   radio_control_periodic_task();
-  booz_downlink_periodic_task();
+  booz_telemetry_periodic_task();
 
 
   static uint8_t my_cnt = 0;
   my_cnt++;
   if (!(my_cnt%10)) {
     LED_TOGGLE(1);
-    DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &(link_imu_state.status));
-  }
+    //    DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &(link_imu_state.status));
 
+    radio_control_periodic_task();
+    //    if (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) {
+    //      fbw_mode = FBW_MODE_AUTO;
+    //    }
+    
+  }
 }
 
 static inline  void main_event_task( void ) {
@@ -76,6 +87,16 @@ static inline  void main_event_task( void ) {
     DOWNLINK_SEND_BOOZ_FD(&link_imu_state.rates[AXIS_P], &link_imu_state.rates[AXIS_Q], &link_imu_state.rates[AXIS_R], &link_imu_state.eulers[AXIS_X], &link_imu_state.eulers[AXIS_Y], &link_imu_state.eulers[AXIS_Z]); 
   }
 
+  if (ppm_valid) {
+    ppm_valid = FALSE;
+    radio_control_event_task();
+    //    if (rc_values_contains_avg_channels) {
+    //      fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
+    //    }
+    /* setpoints */
+
+  }
+
 }
 
 bool_t dl_msg_available;
diff --git a/sw/airborne/main_imu.c b/sw/airborne/main_imu.c
index 6b95317d03..512797b1a2 100644
--- a/sw/airborne/main_imu.c
+++ b/sw/airborne/main_imu.c
@@ -21,15 +21,15 @@
 
 //#define SEND_ACCEL 1
 //#define SEND_MAG   1
-//#define SEND_GYRO  1
-#define SEND_ACCEL_RAW 1
-#define SEND_MAG_RAW   1
+#define SEND_GYRO  1
+//#define SEND_ACCEL_RAW 1
+//#define SEND_MAG_RAW   1
 #define SEND_GYRO_RAW  1
-#define SEND_ACCEL_RAW_AVG 1
-#define SEND_MAG_RAW_AVG   1
-#define SEND_GYRO_RAW_AVG  1
-#define SEND_AHRS_STATE 1
-#define SEND_AHRS_COV   1
+//#define SEND_ACCEL_RAW_AVG 1
+//#define SEND_MAG_RAW_AVG   1
+//#define SEND_GYRO_RAW_AVG  1
+//#define SEND_AHRS_STATE 1
+//#define SEND_AHRS_COV   1
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -155,10 +155,10 @@ static inline void ahrs_task( void ) {
     DOWNLINK_SEND_IMU_MAG_RAW_AVG(&imu_vs_mag_raw_avg[AXIS_X], &imu_vs_mag_raw_avg[AXIS_Y], &imu_vs_mag_raw_avg[AXIS_Z], \
 				  &imu_vs_mag_raw_var[AXIS_X], &imu_vs_mag_raw_var[AXIS_Y], &imu_vs_mag_raw_var[AXIS_Z]);
 #endif
-    if (imu_vehicle_still) {
-      ahrs_step = AHRS_STEP_ROLL;
-      afe_init(imu_mag, imu_accel, imu_gyro);
-    }
+    //    if (imu_vehicle_still) {
+    //      ahrs_step = AHRS_STEP_ROLL;
+    //      afe_init(imu_mag, imu_accel, imu_gyro);
+    //}
   }
   else {
     afe_predict(imu_gyro);
diff --git a/sw/airborne/main_mtt.c b/sw/airborne/main_mtt.c
index c315321a46..3b810e39ef 100644
--- a/sw/airborne/main_mtt.c
+++ b/sw/airborne/main_mtt.c
@@ -15,6 +15,8 @@
 #include "messages.h"
 #include "downlink.h"
 
+#include "link_imu.h"
+
 //#define SEND_ACCEL 1
 //#define SEND_GYRO  1
 //#define SEND_ACCEL_RAW 1
@@ -58,6 +60,7 @@ static inline void main_init( void ) {
   main_init_spi1();
   max1167_init();
   multitilt_init();
+  link_imu_init();
   int_enable();
 }
 
@@ -83,7 +86,8 @@ static inline void main_event_task( void ) {
     DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], &imu_accel_raw[AXIS_Y], &imu_accel_raw[AXIS_Z]);
 #endif
 
-    ahrs_task();
+    ahrs_task(); 
+    link_imu_send();
   }
 }
 
@@ -118,15 +122,15 @@ static inline void ahrs_task( void ) {
     break;
 
   case MT_STATUS_RUNNING : {
-    t0 = T0TC;
+    //    t0 = T0TC;
     multitilt_predict(imu_gyro_prev);
     multitilt_update(imu_accel);
-    t1 = T0TC;
-    uint32_t dif = t1 - t0;
+    //    t1 = T0TC;
+    //    uint32_t dif = t1 - t0;
     static uint32_t foo_cnt = 0;
     foo_cnt++;
     if (!(foo_cnt % 10)) {
-      DOWNLINK_SEND_TIME(&dif);
+      //      DOWNLINK_SEND_TIME(&dif);
 #ifdef SEND_AHRS_STATE
     const float foo = 0.;
     DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &foo, &foo, &mtt_bp, &mtt_bq, &mtt_br);
diff --git a/sw/airborne/multitilt.c b/sw/airborne/multitilt.c
index 91ce274e94..28acd6c80e 100644
--- a/sw/airborne/multitilt.c
+++ b/sw/airborne/multitilt.c
@@ -34,6 +34,17 @@ static const float R_accel = 0.3;
 
 void multitilt_init(void) {
   mtt_status = MT_STATUS_UNINIT;
+  mtt_phi = 0.;
+  mtt_theta = 0.;
+  mtt_psi = 0.;
+
+  mtt_p = 0.;
+  mtt_q = 0.;
+  mtt_r = 0.;
+
+  mtt_bp = 0.;
+  mtt_bq = 0.;
+  mtt_br = 0.;
 }
 
 void multitilt_start(const float* accel, const float* gyro) {
diff --git a/sw/airborne/multitilt.h b/sw/airborne/multitilt.h
index 909ea7867c..a47415b5b7 100644
--- a/sw/airborne/multitilt.h
+++ b/sw/airborne/multitilt.h
@@ -19,9 +19,11 @@ extern float mtt_q;
 extern float mtt_bq;
 extern float mtt_P_theta[2][2];
 
+extern float mtt_psi;
 extern float mtt_r;
 extern float mtt_br;
 
+
 extern void multitilt_init(void);
 extern void multitilt_start( const float* accel, const float* gyro);
 extern void multitilt_predict( const float* gyro );