diff --git a/sw/airborne/motor_bench/main_motor_bench.c b/sw/airborne/motor_bench/main_motor_bench.c
index ca4da5d764..4efcef39c6 100644
--- a/sw/airborne/motor_bench/main_motor_bench.c
+++ b/sw/airborne/motor_bench/main_motor_bench.c
@@ -7,7 +7,8 @@
 #include "mb_tacho.h"
 #include "mb_servo.h"
 #include "i2c.h"
-#include "mb_twi_controller_asctech.h"
+//#include "mb_twi_controller_asctech.h"
+#include "mb_twi_controller_mkk.h"
 #include "mb_current.h"
 #include "mb_scale.h"
 
@@ -21,6 +22,7 @@
 #include "adc.h"
 
 #include "mb_modes.h"
+#include "mb_static.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -51,7 +53,7 @@ static inline void main_init( void ) {
 #endif
 
   mb_servo_init();
-  mb_servo_set_range( 1275000, 1825000 );
+  mb_servo_set_range( 1090000, 1910000 );
 
   adc_init();
   mb_current_init();
@@ -64,8 +66,14 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic_task( void ) {
-  mb_mode_periodic();
+  float rpm = mb_tacho_get_averaged();
+  mb_current_periodic();
+  float amps = mb_current_amp;
+  float thrust = mb_scale_thrust;
+  float torque = 0.;
 
+  mb_mode_periodic(rpm, thrust, amps);
+  
   float throttle = mb_modes_throttle;
 
 #if defined USE_TWI_CONTROLLER
@@ -73,17 +81,16 @@ static inline void main_periodic_task( void ) {
 #endif
   mb_servo_set(throttle);
 
-  float rpm = mb_tacho_get_averaged();
-  mb_current_periodic();
-  float amps = mb_current_amp;
-  float thrust = mb_scale_thrust;
-  float torque = 0.;
 
   RunOnceEvery(125, {
       DOWNLINK_SEND_ALIVE(16, MD5SUM);
       PeriodicSendDlValue();
     });
   DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &throttle, &rpm, &amps , &thrust, &torque, &cpu_time_sec, &mb_modes_mode);
+
+
+
+
 }
 
 static inline  void main_event_task( void ) {
diff --git a/sw/airborne/motor_bench/mb_buss_twi_controller.h b/sw/airborne/motor_bench/mb_buss_twi_controller.h
deleted file mode 100644
index c14cfcb69e..0000000000
--- a/sw/airborne/motor_bench/mb_buss_twi_controller.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef MB_BUSS_TWI_CONTROLLER_H
-#define MB_BUSS_TWI_CONTROLLER_H
-
-#include "std.h"
-
-extern void mb_buss_twi_controller_init(void);
-
-extern void mb_buss_twi_controller_set( float throttle );
-
-#endif /* MB_BUSS_TWI_CONTROLLER_H */
-
-
diff --git a/sw/airborne/motor_bench/mb_modes.c b/sw/airborne/motor_bench/mb_modes.c
index 7569025966..4935a12fb2 100644
--- a/sw/airborne/motor_bench/mb_modes.c
+++ b/sw/airborne/motor_bench/mb_modes.c
@@ -1,5 +1,7 @@
 #include "mb_modes.h"
 
+#include "mb_static.h"
+
 #include "adc.h"
 #include "sys_time.h"
 
@@ -15,11 +17,15 @@ float mb_modes_step_low_throttle;
 float mb_modes_step_high_throttle;
 float mb_modes_step_duration;
 
+float mb_modes_sine_freq;
+float mb_modes_sine_mean;
+float mb_modes_sine_ampl;
+
 static void mb_modes_manual( void );
 static void mb_modes_ramp( void );
 static void mb_modes_step( void );
 static void mb_modes_prbs( void );
-
+static void mb_modes_sine( void );
 
 static struct adc_buf mb_modes_adc_buf; /* manual mode */
 
@@ -30,15 +36,21 @@ void mb_mode_init(void) {
 
   mb_modes_ramp_duration = 200;
 
-  mb_modes_step_low_throttle = 0.6;
-  mb_modes_step_high_throttle = 0.7;
-  mb_modes_step_duration = 1.;
+  mb_modes_step_low_throttle = 0.48;
+  mb_modes_step_high_throttle = 0.65;
+  mb_modes_step_duration = 0.5;
+
+  mb_modes_sine_freq = 80.;
+  mb_modes_sine_mean = 0.5;
+  mb_modes_sine_ampl = 0.1;
+
+  mb_static_init();
 
 }
 
 void mb_mode_event(void) {} 
 
-void mb_mode_periodic(void) {
+void mb_mode_periodic(float rpm, float thrust, float current) {
   switch (mb_modes_mode) {
   case MB_MODES_IDLE :
     mb_modes_throttle = 0.;
@@ -47,11 +59,20 @@ void mb_mode_periodic(void) {
     mb_modes_manual();
     break;
   case MB_MODES_RAMP :
-    mb_modes_ramp();
+    mb_static_periodic(rpm, thrust, current);
+    mb_modes_throttle = (float)mb_static_throttle / (float)MB_STATIC_MAX_THROTTLE;
+    //    mb_modes_ramp();
     break;
   case MB_MODES_STEP :
     mb_modes_step();
     break;
+  case MB_MODES_SINE :
+    mb_modes_sine();
+    break;
+  case MB_MODES_FIXED_RPM :
+    mb_mode_fixed_rpm_periodic(rpm, thrust, current);
+    mb_modes_throttle = mb_mode_fixed_rpm_throttle;
+    break;
   default:
     mb_modes_throttle = 0.;
   }
@@ -90,4 +111,8 @@ static void mb_modes_step( void ) {
   }
 }
 
-
+static void mb_modes_sine( void ) {
+  float now = GET_CUR_TIME_FLOAT();
+  float alpha = 2. * M_PI * mb_modes_sine_freq * now;
+  mb_modes_throttle = mb_modes_sine_mean + mb_modes_sine_ampl * sin(alpha);
+}
diff --git a/sw/airborne/motor_bench/mb_modes.h b/sw/airborne/motor_bench/mb_modes.h
index 9f73263817..adb3ba47e9 100644
--- a/sw/airborne/motor_bench/mb_modes.h
+++ b/sw/airborne/motor_bench/mb_modes.h
@@ -3,12 +3,15 @@
 
 #include "std.h"
 
+#include "mb_mode_fixed_rpm.h"
 
-#define MB_MODES_IDLE   0
-#define MB_MODES_MANUAL 1
-#define MB_MODES_RAMP   2
-#define MB_MODES_STEP   3
-#define MB_MODES_PRBS   4
+#define MB_MODES_IDLE      0
+#define MB_MODES_MANUAL    1
+#define MB_MODES_RAMP      2
+#define MB_MODES_STEP      3
+#define MB_MODES_PRBS      4
+#define MB_MODES_SINE      5
+#define MB_MODES_FIXED_RPM 6
 
 extern uint8_t mb_modes_mode;
 extern float mb_modes_throttle;
@@ -24,11 +27,15 @@ extern float mb_modes_step_duration;
 
 extern void mb_mode_init(void);
 extern void mb_mode_event(void);
-extern void mb_mode_periodic(void);
+extern void mb_mode_periodic(float rpm, float thrust, float current);
 
 #define mb_modes_SetMode(_val) {			\
     mb_modes_mode = _val;				\
     mb_modes_last_change_time = GET_CUR_TIME_FLOAT();	\
+    if  (mb_modes_mode == MB_MODES_RAMP)		\
+      mb_static_init();					\
+    if  (mb_modes_mode == MB_MODES_FIXED_RPM)		\
+      mb_mode_fixed_rpm_init();				\
   }
 
 #endif /* MB_MODES_H */
diff --git a/sw/airborne/motor_bench/mb_scale.c b/sw/airborne/motor_bench/mb_scale.c
index 06b967dbda..1b0110ad6f 100644
--- a/sw/airborne/motor_bench/mb_scale.c
+++ b/sw/airborne/motor_bench/mb_scale.c
@@ -1,15 +1,19 @@
 #include "mb_scale.h"
 
 volatile uint32_t mb_scale_pulse_len;
-float mb_scale_thrust;
-float mb_scale_torque;
+volatile float mb_scale_thrust;
+volatile float mb_scale_torque;
 
-uint32_t mb_scale_neutral = 2892000; //2944640;
-float    mb_scale_gain = 1.69e-3; //2.72e-3;
+volatile uint32_t mb_scale_neutral = 2892000; //2944640;
+float    mb_scale_gain =  0.0018584; //1;
+volatile uint8_t  mb_scale_calib;
 
 void mb_scale_init ( void ) {
   /* select pin for capture */
   ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT;
   /* enable capture 0.3 on falling edge + trigger interrupt */
   T0CCR |= TCCR_CR3_F | TCCR_CR3_I;
+  mb_scale_thrust = 0.;
+  mb_scale_torque = 0.;
+  mb_scale_calib = 0;
 }
diff --git a/sw/airborne/motor_bench/mb_scale.h b/sw/airborne/motor_bench/mb_scale.h
index 0211889200..58663c42f4 100644
--- a/sw/airborne/motor_bench/mb_scale.h
+++ b/sw/airborne/motor_bench/mb_scale.h
@@ -10,24 +10,42 @@
 #define ICP_PINSEL_BIT 26
 
 extern volatile uint32_t mb_scale_pulse_len;
-extern float mb_scale_thrust;
-extern float mb_scale_torque;
+extern volatile float mb_scale_thrust;
+extern volatile float mb_scale_torque;
 
-extern uint32_t mb_scale_neutral;
+extern volatile uint32_t mb_scale_neutral;
 extern float    mb_scale_gain;
+extern volatile uint8_t  mb_scale_calib;
+
+#define MB_SCALE_NB_CALIB 50
+
 
 void mb_scale_init ( void );
 
 
-#define MB_SCALE_ICP_ISR() {					\
-    static uint32_t last;					\
-    uint32_t now = T0CR3;					\
+#define MB_SCALE_ICP_ISR() {						\
+    static uint32_t last;						\
+    uint32_t now = T0CR3;						\
     mb_scale_pulse_len = now - last;					\
-    int32_t diff = (int32_t)mb_scale_pulse_len - (int32_t)mb_scale_neutral; \
-    mb_scale_thrust = mb_scale_gain * diff;				\
-    last = now;							\
+    last = now;								\
+    if (mb_scale_calib > 0) {						\
+      mb_scale_thrust += mb_scale_pulse_len;				\
+      if (mb_scale_calib >=  MB_SCALE_NB_CALIB) {			\
+	mb_scale_neutral = mb_scale_thrust /  MB_SCALE_NB_CALIB;	\
+	mb_scale_calib = 0;						\
+      }									\
+      else								\
+	mb_scale_calib++;						\
+    }									\
+    else {								\
+      int32_t diff = (int32_t)mb_scale_pulse_len - (int32_t)mb_scale_neutral; \
+      mb_scale_thrust = mb_scale_gain * diff;				\
+    }									\
   }
 
-
+#define mb_scale_Calib(_val) {			\
+    mb_scale_calib = 1;				\
+    mb_scale_thrust = 0.;			\
+  }
 
 #endif /* MB_SCALE_H */
diff --git a/sw/airborne/motor_bench/mb_servo.c b/sw/airborne/motor_bench/mb_servo.c
index 3dd0a67393..a2dd213bd4 100644
--- a/sw/airborne/motor_bench/mb_servo.c
+++ b/sw/airborne/motor_bench/mb_servo.c
@@ -13,7 +13,7 @@ void mb_servo_init( void ) {
   /* enable and select the type of PWM channel */
   PWMPCR |= PWMPCR_ENA5;
   /* set Match0 value (refresh rate) */
-  PWMMR0 = MY_NB_CLOCK_TIMER_PWM(25000);  
+  PWMMR0 = MY_NB_CLOCK_TIMER_PWM(5000);  
   /* commit PWMMRx changes */
   PWMLER = PWMLER_LATCH0;
   /* enable PWM timer in PWM mode */
diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.c b/sw/airborne/motor_bench/mb_twi_controller_asctech.c
index 2ce8e8e57a..1bf465b7b1 100644
--- a/sw/airborne/motor_bench/mb_twi_controller_asctech.c
+++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.c
@@ -79,3 +79,8 @@ void mb_twi_controller_set( float throttle ) {
   else
     mb_twi_nb_overun++;
 }
+
+void mb_twi_controller_set_raw( uint8_t throttle ) {
+
+
+}
diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.h b/sw/airborne/motor_bench/mb_twi_controller_asctech.h
index 4d5431affc..aa030c014d 100644
--- a/sw/airborne/motor_bench/mb_twi_controller_asctech.h
+++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.h
@@ -5,6 +5,7 @@
 
 extern void mb_twi_controller_init(void);
 extern void mb_twi_controller_set( float throttle );
+extern void mb_twi_controller_set_raw( uint8_t throttle );
 
 #define MB_TWI_CONTROLLER_COMMAND_NONE     0
 #define MB_TWI_CONTROLLER_COMMAND_TEST     1
diff --git a/sw/airborne/motor_bench/mb_buss_twi_controller.c b/sw/airborne/motor_bench/mb_twi_controller_mkk.c
similarity index 71%
rename from sw/airborne/motor_bench/mb_buss_twi_controller.c
rename to sw/airborne/motor_bench/mb_twi_controller_mkk.c
index b9f6aac7ea..6e6d099c6e 100644
--- a/sw/airborne/motor_bench/mb_buss_twi_controller.c
+++ b/sw/airborne/motor_bench/mb_twi_controller_mkk.c
@@ -1,4 +1,4 @@
-#include "mb_buss_twi_controller.h"
+#include "mb_twi_controller_mkk.h"
 
 #include <string.h>
 
@@ -10,7 +10,7 @@ uint8_t mb_buss_twi_nb_overun;
 uint8_t mb_buss_twi_i2c_done;
 
 
-#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 255
+#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 200
 /*
   Slave address 
   front = 0x52
@@ -18,14 +18,14 @@ uint8_t mb_buss_twi_i2c_done;
   right = 0x56
   left  = 0x58
 */
-#define MB_BUSS_TWI_CONTROLLER_ADDR 0x52
+#define MB_BUSS_TWI_CONTROLLER_ADDR 0x56
 
-void mb_buss_twi_controller_init(void) {
+void mb_twi_controller_init(void) {
   mb_buss_twi_nb_overun = 0;
   mb_buss_twi_i2c_done = TRUE;
 }
 
-void mb_buss_twi_controller_set( float throttle ) {
+void mb_twi_controller_set( float throttle ) {
   if (mb_buss_twi_i2c_done) {
     mb_buss_twi_command = throttle * MB_BUSS_TWI_CONTROLLER_MAX_CMD;
     i2c_buf[0] = mb_buss_twi_command;
diff --git a/sw/airborne/motor_bench/mb_twi_controller_mkk.h b/sw/airborne/motor_bench/mb_twi_controller_mkk.h
new file mode 100644
index 0000000000..a7e91b933e
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_twi_controller_mkk.h
@@ -0,0 +1,12 @@
+#ifndef MB_TWI_CONTROLLER_MKK_H
+#define MB_TWI_CONTROLLER_MKK_H
+
+#include "std.h"
+
+extern void mb_twi_controller_init(void);
+
+extern void mb_twi_controller_set( float throttle );
+
+#endif /* MB_TWI_CONTROLLER_MKK_H */
+
+