diff --git a/sw/airborne/autopilot/estimator.c b/sw/airborne/autopilot/estimator.c
index 10c8351b44..01b1976e5c 100644
--- a/sw/airborne/autopilot/estimator.c
+++ b/sw/airborne/autopilot/estimator.c
@@ -121,13 +121,13 @@ void estimator_update_state_3DMG( void ) {
   estimator_psi = from_fbw.euler[1];
   estimator_theta = from_fbw.euler[2];
 }
-#elif defined SECTION_IMU_ANALOG
-#include "ahrs.h"
-void estimator_update_state_ANALOG( void ) {
-  estimator_phi = ahrs_euler[0];
-  estimator_theta = ahrs_euler[1];
-  estimator_psi = ahrs_euler[2];
-}
+//#elif defined SECTION_IMU_ANALOG
+//#include "ahrs.h"
+//void estimator_update_state_ANALOG( void ) {
+//  estimator_phi = ahrs_euler[0];
+//  estimator_theta = ahrs_euler[1];
+//  estimator_psi = ahrs_euler[2];
+//}
 #else //NO_IMU
 void estimator_update_state_infrared( void ) {
   float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ? 
diff --git a/sw/airborne/autopilot/main.c b/sw/airborne/autopilot/main.c
index a9e21bc39a..4f40a11329 100644
--- a/sw/airborne/autopilot/main.c
+++ b/sw/airborne/autopilot/main.c
@@ -433,7 +433,7 @@ void navigation_task( void ) {
   
   if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
 			|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
-    if (lateral_mode >= LATERAL_MODE_COURSE)
+    if (lateral_mode >=LATERAL_MODE_COURSE)
       course_pid_run(); /* aka compute nav_desired_roll */
     desired_roll = nav_desired_roll;
     if (vertical_mode == VERTICAL_MODE_AUTO_ALT)
@@ -525,12 +525,12 @@ inline void periodic_task( void ) {
         
 #if defined SECTION_IMU_3DMG
 
-#elif defined SECTION_IMU_ANALOG
+    //#elif defined SECTION_IMU_ANALOG
     //20Hz/3 it could be possible since it can run standalone at 60Hz/3
-    ahrs_update();
-	estimator_update_state_ANALOG();
+    //    ahrs_update();
+    //	estimator_update_state_ANALOG();
 #else //NO IMU
-	ir_update();
+    ir_update();
     estimator_update_state_infrared();
 #endif
     roll_pitch_pid_run(); /* Set  desired_aileron & desired_elevator */
@@ -578,3 +578,19 @@ void use_gps_pos( void ) {
     DOWNLINK_SEND_TAKEOFF(&cputime);
   }
 }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/autopilot/mainloop.c b/sw/airborne/autopilot/mainloop.c
index f79cc088df..c386e5cf4a 100644
--- a/sw/airborne/autopilot/mainloop.c
+++ b/sw/airborne/autopilot/mainloop.c
@@ -39,6 +39,7 @@
 #include "infrared.h"
 #include "estimator.h"
 #include "downlink.h"
+#include "uart.h"
 
 #ifdef SECTION_IMU_ANALOG
 #include "ahrs.h"
@@ -123,6 +124,8 @@ int main( void ) {
 	   *  -it's not safe always
 	   */
 	  ahrs_save_pqr_from_fbw();
+	  int16_t dummy;
+      DOWNLINK_SEND_IMU_3DMG(&from_fbw.euler_dot[0], &from_fbw.euler_dot[1], &from_fbw.euler_dot[2], &dummy, &dummy, &dummy);
 	  
 	  uart0_transmit('E');
       uart0_transmit(' ');
@@ -148,3 +151,5 @@ int main( void ) {
   return 0;
 }
 
+
+
diff --git a/sw/airborne/autopilot/uart.c b/sw/airborne/autopilot/uart.c
index 67604af7ab..efe16eb4a5 100644
--- a/sw/airborne/autopilot/uart.c
+++ b/sw/airborne/autopilot/uart.c
@@ -124,7 +124,7 @@ void uart0_init( void ) {
   /* Set frame format: 8data, 1stop bit */ 
   UCSR0C = _BV(UCSZ1) | _BV(UCSZ0);
   /* Enable uart receive interrupt */
-  sbi(UCSR0B, RXCIE ); 
+  //  sbi(UCSR0B, RXCIE ); 
 }
 
 void uart1_init( void ) {
diff --git a/sw/airborne/fly_by_wire/adc_fbw.c b/sw/airborne/fly_by_wire/adc_fbw.c
index c07b21b0d2..c9953478fe 100644
--- a/sw/airborne/fly_by_wire/adc_fbw.c
+++ b/sw/airborne/fly_by_wire/adc_fbw.c
@@ -43,8 +43,13 @@
 #define ANALOG_PORT_DIR	DDRC
 
 
-// 
+#ifdef SECTION_IMU_ANALOG
+#define ANALOG_VREF _BV(REFS0)
+#else
 #define ANALOG_VREF _BV(REFS0) | _BV(REFS1)
+#endif
+
+
 
 uint16_t		adc_samples[ NB_ADC ];
 
diff --git a/sw/airborne/fly_by_wire/adc_fbw.h b/sw/airborne/fly_by_wire/adc_fbw.h
index 6dd77eb335..f28d362708 100644
--- a/sw/airborne/fly_by_wire/adc_fbw.h
+++ b/sw/airborne/fly_by_wire/adc_fbw.h
@@ -42,7 +42,7 @@ extern uint16_t		adc_samples[ NB_ADC ];
 
 void adc_init( void );
 
-#define AV_NB_SAMPLE 0x20
+#define AV_NB_SAMPLE 0x08
 
 struct adc_buf {
   uint16_t sum;
diff --git a/sw/airborne/fly_by_wire/imu.c b/sw/airborne/fly_by_wire/imu.c
index 831fd5415e..d4aa9c0c7c 100644
--- a/sw/airborne/fly_by_wire/imu.c
+++ b/sw/airborne/fly_by_wire/imu.c
@@ -69,9 +69,9 @@ uint16_t raw_roll_dot  = 512*AV_NB_SAMPLE;
 uint16_t raw_pitch_dot = 512*AV_NB_SAMPLE;
 uint16_t raw_yaw_dot   = 512*AV_NB_SAMPLE;
 
-uint16_t raw_roll_dot_neutral  = 512*AV_NB_SAMPLE;
-uint16_t raw_pitch_dot_neutral = 512*AV_NB_SAMPLE;
-uint16_t raw_yaw_dot_neutral   = 512*AV_NB_SAMPLE;
+uint16_t raw_roll_dot_neutral  = IMU_ADC_ROLL_DOT_ZERO*AV_NB_SAMPLE;
+uint16_t raw_pitch_dot_neutral = IMU_ADC_PITCH_DOT_ZERO*AV_NB_SAMPLE;
+uint16_t raw_yaw_dot_neutral   = IMU_ADC_YAW_DOT_ZERO*AV_NB_SAMPLE;
 
 void imu_init ( void ) {
   adc_buf_channel(IMU_ADC_ROLL_DOT, &buf_roll_dot);
@@ -80,9 +80,9 @@ void imu_init ( void ) {
 }
 
 void imu_update ( void ) {
-  raw_roll_dot = buf_roll_dot.sum;
-  raw_pitch_dot = buf_pitch_dot.sum;
-  raw_yaw_dot = buf_yaw_dot.sum;
+  raw_roll_dot = IMU_ADC_ROLL_DOT_SIGN * buf_roll_dot.sum;
+  raw_pitch_dot = IMU_ADC_PITCH_DOT_SIGN * buf_pitch_dot.sum;
+  raw_yaw_dot = IMU_ADC_YAW_DOT_SIGN * buf_yaw_dot.sum;
   roll_dot = (int16_t)(raw_roll_dot - raw_roll_dot_neutral) / AV_NB_SAMPLE;
   pitch_dot = (int16_t)(raw_pitch_dot - raw_pitch_dot_neutral) / AV_NB_SAMPLE;
   yaw_dot = (int16_t)(raw_yaw_dot - raw_yaw_dot_neutral) / AV_NB_SAMPLE;;
diff --git a/sw/airborne/fly_by_wire/led.h b/sw/airborne/fly_by_wire/led.h
index 979d44b8bf..661d1a7019 100644
--- a/sw/airborne/fly_by_wire/led.h
+++ b/sw/airborne/fly_by_wire/led.h
@@ -20,6 +20,15 @@
 #define GREEN_LED_OFF()     PORTB |=  _BV(1)
 #define GREEN_LED_TOGGLE()  PORTB ^=  _BV(1)
 
+#define CounterPin 4
+
+#define CounterLedInit()  DDRD |= _BV(CounterPin)
+
+#define CounterLedOn()      PORTD &= ~_BV(CounterPin)
+#define CounterLedOff()     PORTD |=  _BV(CounterPin)
+#define CounterLedToggle()  PORTD ^=  _BV(CounterPin)
+
+
 
 
 
diff --git a/sw/airborne/fly_by_wire/main.c b/sw/airborne/fly_by_wire/main.c
index 4ac02f59f0..3938587bc8 100644
--- a/sw/airborne/fly_by_wire/main.c
+++ b/sw/airborne/fly_by_wire/main.c
@@ -38,6 +38,7 @@
 #include "spi.h"
 #include "link_autopilot.h"
 #include "radio.h"
+#include "led.h"
 
 
 #include "uart.h"
@@ -53,7 +54,6 @@
 
 #include "adc_fbw.h"
 struct adc_buf vsupply_adc_buf;
-struct adc_buf vservos_adc_buf;
 
 uint8_t mode;
 static uint8_t time_since_last_mega128;
@@ -104,8 +104,20 @@ inline void radio_control_task(void) {
   if (last_radio_contains_avg_channels) {
     mode = MODE_OF_PPRZ(last_radio[RADIO_MODE]);
   }
+#if defined SECTION_IMU_ANALOG
+  if (last_radio[RADIO_SWITCH1] > MAX_PPRZ/2) {
+    imu_capture_neutral();
+    CounterLedOn();
+  } else {
+    CounterLedOff();
+  } 
+#endif
   if (mode == MODE_MANUAL) {
 #if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
+    roll_dot_pgain = -100.  + (float)last_radio[RADIO_GAIN1] * 0.010;
+    roll_dot_dgain = 2.5 - (float)last_radio[RADIO_GAIN2] * 0.00025;
+    pitch_dot_pgain = roll_dot_pgain;
+    pitch_dot_dgain = roll_dot_dgain;
     control_set_desired(last_radio);
 #else
     servo_set(last_radio);
@@ -129,6 +141,11 @@ inline void spi_task(void) {
   spi_reset();
 }
 
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY 3
+// for compatibility
+#endif
+
 int main( void )
 {
   uart_init_tx();
@@ -138,9 +155,9 @@ int main( void )
   uart_print_string("FBW Booting $Id$\n");
 #endif
   adc_init();
-  adc_buf_channel(3, &vsupply_adc_buf);
-  adc_buf_channel(6, &vservos_adc_buf);
+  adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf);
 #if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
+  CounterLedInit();
   imu_init();
 #endif
   timer_init();
@@ -208,6 +225,14 @@ int main( void )
 #endif
 #if defined SECTION_IMU_3DMG || defined SECTION_IMU_ANALOG
       control_run();
+      if (radio_ok) {
+	if (last_radio[RADIO_THROTTLE] > 0.1*MAX_PPRZ) {
+	  servo_set(control_commands);
+	}
+	else {
+	  servo_set(failsafe);
+	}
+  }
 #endif
       if (_1Hz >= 60) {
 	_1Hz = 0;
diff --git a/sw/airborne/fly_by_wire/servo.c b/sw/airborne/fly_by_wire/servo.c
index bab2f48575..fb4991ba02 100644
--- a/sw/airborne/fly_by_wire/servo.c
+++ b/sw/airborne/fly_by_wire/servo.c
@@ -50,7 +50,7 @@
 #define _4017_RESET_PORT        PORTD
 #define _4017_RESET_DDR         DDRD
 #define _4017_RESET_PIN         7
-#endif /* CTL_BRD_V1_2 */
+#endif /* CTL_BRD_V1_2_1 */
 
 #define _4017_CLOCK_PORT        PORTB
 #define _4017_CLOCK_DDR         DDRB