diff --git a/sw/airborne/motor_bench/main_motor_bench.c b/sw/airborne/motor_bench/main_motor_bench.c
index 238df80a48..ae538b9586 100644
--- a/sw/airborne/motor_bench/main_motor_bench.c
+++ b/sw/airborne/motor_bench/main_motor_bench.c
@@ -76,18 +76,13 @@ static inline void main_periodic_task( void ) {
   float rpm = mb_tacho_get_averaged();
   mb_current_periodic();
   float amps = mb_current_amp;
-  mb_scale_periodic();
   float thrust = mb_scale_thrust;
   float torque = 0.;
 
-  static uint8_t my_cnt = 0;
-  my_cnt++;
-  if ( my_cnt >= 125 ) {
-    //    LED_TOGGLE(1);
-    my_cnt = 0;
-    DOWNLINK_SEND_ALIVE(16, MD5SUM);
-    PeriodicSendDlValue();
-  }
+  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);
 }
 
diff --git a/sw/airborne/motor_bench/mb_buss_twi_controller.c b/sw/airborne/motor_bench/mb_buss_twi_controller.c
new file mode 100644
index 0000000000..b9f6aac7ea
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_buss_twi_controller.c
@@ -0,0 +1,36 @@
+#include "mb_buss_twi_controller.h"
+
+#include <string.h>
+
+#include "i2c.h"
+
+uint8_t mb_buss_twi_command;
+
+uint8_t mb_buss_twi_nb_overun;
+uint8_t mb_buss_twi_i2c_done;
+
+
+#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 255
+/*
+  Slave address 
+  front = 0x52
+  back  = 0x54
+  right = 0x56
+  left  = 0x58
+*/
+#define MB_BUSS_TWI_CONTROLLER_ADDR 0x52
+
+void mb_buss_twi_controller_init(void) {
+  mb_buss_twi_nb_overun = 0;
+  mb_buss_twi_i2c_done = TRUE;
+}
+
+void mb_buss_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;
+    i2c_transmit(MB_BUSS_TWI_CONTROLLER_ADDR, 1, &mb_buss_twi_i2c_done);
+  }
+  else
+    mb_buss_twi_nb_overun++;
+}
diff --git a/sw/airborne/motor_bench/mb_buss_twi_controller.h b/sw/airborne/motor_bench/mb_buss_twi_controller.h
new file mode 100644
index 0000000000..c14cfcb69e
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_buss_twi_controller.h
@@ -0,0 +1,12 @@
+#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_current.c b/sw/airborne/motor_bench/mb_current.c
new file mode 100644
index 0000000000..5e3046cde4
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_current.c
@@ -0,0 +1,17 @@
+#include "mb_current.h"
+
+#include "adc.h"
+
+static struct adc_buf mb_current_buf;
+
+float mb_current_amp;
+
+void mb_current_init(void) {
+  adc_buf_channel(4, &mb_current_buf, 16);
+
+}
+
+
+void mb_current_periodic(void) {
+  mb_current_amp = (float)mb_current_buf.sum * 0.00113607  - 2.8202;
+}
diff --git a/sw/airborne/motor_bench/mb_current.h b/sw/airborne/motor_bench/mb_current.h
new file mode 100644
index 0000000000..cd911455ea
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_current.h
@@ -0,0 +1,11 @@
+#ifndef MB_CURRENT_H
+#define MB_CURRENT_H
+
+#include "std.h"
+
+extern void mb_current_init(void);
+extern void mb_current_periodic(void);
+
+extern float mb_current_amp;
+
+#endif /* MB_CURRENT_H */
diff --git a/sw/airborne/motor_bench/mb_modes.c b/sw/airborne/motor_bench/mb_modes.c
new file mode 100644
index 0000000000..7569025966
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_modes.c
@@ -0,0 +1,93 @@
+#include "mb_modes.h"
+
+#include "adc.h"
+#include "sys_time.h"
+
+
+uint8_t mb_modes_mode;
+float mb_modes_throttle;
+
+float mb_modes_last_change_time;
+
+float mb_modes_ramp_duration;
+
+float mb_modes_step_low_throttle;
+float mb_modes_step_high_throttle;
+float mb_modes_step_duration;
+
+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 struct adc_buf mb_modes_adc_buf; /* manual mode */
+
+void mb_mode_init(void) {
+  adc_buf_channel(1, &mb_modes_adc_buf, 16);
+  mb_modes_mode = MB_MODES_IDLE;
+  mb_modes_throttle = 0.;
+
+  mb_modes_ramp_duration = 200;
+
+  mb_modes_step_low_throttle = 0.6;
+  mb_modes_step_high_throttle = 0.7;
+  mb_modes_step_duration = 1.;
+
+}
+
+void mb_mode_event(void) {} 
+
+void mb_mode_periodic(void) {
+  switch (mb_modes_mode) {
+  case MB_MODES_IDLE :
+    mb_modes_throttle = 0.;
+    break;
+  case MB_MODES_MANUAL :
+    mb_modes_manual();
+    break;
+  case MB_MODES_RAMP :
+    mb_modes_ramp();
+    break;
+  case MB_MODES_STEP :
+    mb_modes_step();
+    break;
+  default:
+    mb_modes_throttle = 0.;
+  }
+}
+
+
+static void mb_modes_manual( void ) {
+    uint16_t poti =  mb_modes_adc_buf.sum;
+    mb_modes_throttle = (float)poti/(16.*1024.);
+}
+
+static void mb_modes_ramp( void ) {
+  float now = GET_CUR_TIME_FLOAT();
+  float elapsed = now - mb_modes_last_change_time;
+  if ( elapsed < mb_modes_ramp_duration)
+    mb_modes_throttle = elapsed/mb_modes_ramp_duration;
+  else if ( elapsed < 2 * mb_modes_ramp_duration)
+    mb_modes_throttle = 2 - elapsed/mb_modes_ramp_duration;
+  else {
+    mb_modes_last_change_time = now;
+    mb_modes_throttle = 0.;
+  }
+}
+
+
+static void mb_modes_step( void ) {
+  float now = GET_CUR_TIME_FLOAT();
+  float elapsed = now - mb_modes_last_change_time;
+  if ( elapsed < mb_modes_step_duration)
+    mb_modes_throttle = mb_modes_step_low_throttle;
+  else if ( elapsed < 2 * mb_modes_step_duration)
+    mb_modes_throttle = mb_modes_step_high_throttle;
+  else {
+    mb_modes_last_change_time = now;
+    mb_modes_throttle = mb_modes_step_low_throttle;
+  }
+}
+
+
diff --git a/sw/airborne/motor_bench/mb_modes.h b/sw/airborne/motor_bench/mb_modes.h
new file mode 100644
index 0000000000..9f73263817
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_modes.h
@@ -0,0 +1,34 @@
+#ifndef MB_MODES_H
+#define MB_MODES_H
+
+#include "std.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
+
+extern uint8_t mb_modes_mode;
+extern float mb_modes_throttle;
+
+extern float mb_modes_last_change_time;
+
+extern float mb_modes_ramp_duration;
+
+extern float mb_modes_step_low_throttle;
+extern float mb_modes_step_high_throttle;
+extern float mb_modes_step_duration;
+
+
+extern void mb_mode_init(void);
+extern void mb_mode_event(void);
+extern void mb_mode_periodic(void);
+
+#define mb_modes_SetMode(_val) {			\
+    mb_modes_mode = _val;				\
+    mb_modes_last_change_time = GET_CUR_TIME_FLOAT();	\
+  }
+
+#endif /* MB_MODES_H */
diff --git a/sw/airborne/motor_bench/mb_scale.c b/sw/airborne/motor_bench/mb_scale.c
new file mode 100644
index 0000000000..a44357b409
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_scale.c
@@ -0,0 +1,15 @@
+#include "mb_scale.h"
+
+volatile uint32_t mb_scale_pulse_len;
+float mb_scale_thrust;
+float mb_scale_torque;
+
+uint32_t mb_scale_neutral = 2906400;
+float    mb_scale_gain = 0.0021563;
+
+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;
+}
diff --git a/sw/airborne/motor_bench/mb_scale.h b/sw/airborne/motor_bench/mb_scale.h
new file mode 100644
index 0000000000..7e9d22a5c2
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_scale.h
@@ -0,0 +1,33 @@
+#ifndef MB_SCALE_H
+#define MB_SCALE_H
+
+#include "std.h"
+#include "LPC21xx.h"
+
+/* P0.29 CAP0.3 */
+#define ICP_PINSEL     PINSEL1
+#define ICP_PINSEL_VAL 0x02
+#define ICP_PINSEL_BIT 26
+
+extern volatile uint32_t mb_scale_pulse_len;
+extern float mb_scale_thrust;
+extern float mb_scale_torque;
+
+extern uint32_t mb_scale_neutral;
+extern float    mb_scale_gain;
+
+void mb_scale_init ( void );
+
+
+#define MB_SCALE_ICP_ISR() {					\
+    static uint32_t last;					\
+    uint32_t now = T0CR3;					\
+    mb_scale_pulse_len = now - last;				\
+    mb_scale_thrust = mb_scale_gain *				\
+      ( mb_scale_pulse_len - mb_scale_neutral);			\
+    last = now;							\
+  }
+
+
+
+#endif /* MB_SCALE_H */
diff --git a/sw/airborne/motor_bench/mb_servo.c b/sw/airborne/motor_bench/mb_servo.c
new file mode 100644
index 0000000000..3dd0a67393
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_servo.c
@@ -0,0 +1,66 @@
+#include "mb_servo.h"
+
+#include "sys_time.h"
+#define MY_NB_CLOCK_TIMER_PWM(time_us) SYS_TICS_OF_USEC(time_us)
+
+uint32_t mb_servo_max_pulse_ns, mb_servo_min_pulse_ns;
+
+void mb_servo_set_ns(uint32_t duration_ns);
+
+void mb_servo_init( void ) {
+  /* set P0.21 as PWM5 output */
+  PINSEL1 |= (0X01 << 10);  
+  /* enable and select the type of PWM channel */
+  PWMPCR |= PWMPCR_ENA5;
+  /* set Match0 value (refresh rate) */
+  PWMMR0 = MY_NB_CLOCK_TIMER_PWM(25000);  
+  /* commit PWMMRx changes */
+  PWMLER = PWMLER_LATCH0;
+  /* enable PWM timer in PWM mode */
+  PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
+  mb_servo_min_pulse_ns = MIN_SERVO_NS;
+  mb_servo_max_pulse_ns = MAX_SERVO_NS;
+}
+
+void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ) {
+  mb_servo_min_pulse_ns = min_pulse_ns;
+  mb_servo_max_pulse_ns = max_pulse_ns;
+}  
+
+void mb_servo_set_us(uint32_t duration_us) {
+  /* set Match5 value (pulse duration )*/
+  PWMMR5 = MY_NB_CLOCK_TIMER_PWM(duration_us);
+ /* commit PWMMRx changes */
+  PWMLER = PWMLER_LATCH5;
+}
+
+void mb_servo_set_ns(uint32_t duration_ns) {
+  /* set Match5 value (pulse duration )*/
+  PWMMR5 = SYS_TICS_OF_NSEC(duration_ns);
+ /* commit PWMMRx changes */
+  PWMLER = PWMLER_LATCH5;
+}
+
+/* normalized throttle between 0. and 1. */
+void mb_servo_set(float throttle) {
+  uint32_t range = mb_servo_max_pulse_ns - mb_servo_min_pulse_ns;
+  uint32_t pulse_ns = mb_servo_min_pulse_ns + throttle * (range);
+  mb_servo_set_ns(pulse_ns);
+}
+
+#define FOO_DELAY() {				\
+    uint32_t foo = 0;				\
+    while (foo<10000000) foo++;			\
+  }
+
+
+/* arm the brushless controller */
+
+void mb_servo_arm (void) {
+  mb_servo_set(0.);
+  FOO_DELAY();
+  mb_servo_set(1.);
+  FOO_DELAY();
+  mb_servo_set(0.);
+}
+
diff --git a/sw/airborne/motor_bench/mb_servo.h b/sw/airborne/motor_bench/mb_servo.h
new file mode 100644
index 0000000000..26a8f2ef2b
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_servo.h
@@ -0,0 +1,19 @@
+
+#ifndef MB_SERVO_H
+#define MB_SERVO_H
+
+#include "std.h"
+
+#define MIN_SERVO_US 1000
+#define MAX_SERVO_US 2000
+#define MIN_SERVO_NS 1000000
+#define MAX_SERVO_NS 2000000
+#define RANGE_SERVO_US (MAX_SERVO_US - MIN_SERVO_US) 
+
+void mb_servo_init( void );
+void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns );
+void mb_servo_set_us(uint32_t duration_us);
+//void mb_servo_set_ns(uint32_t duration_ns);
+void mb_servo_set(float throttle);
+void mb_servo_arm (void);
+#endif /* MB_SERVO_H */
diff --git a/sw/airborne/motor_bench/mb_tacho.c b/sw/airborne/motor_bench/mb_tacho.c
new file mode 100644
index 0000000000..a2bba80892
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_tacho.c
@@ -0,0 +1,60 @@
+#include "mb_tacho.h"
+
+#include "LPC21xx.h"
+
+#include "interrupt_hw.h"
+
+volatile uint32_t mb_tacho_duration;
+volatile uint8_t got_one_pulse;
+volatile float mb_tacho_averaged;
+volatile uint16_t  mb_tacho_nb_pulse;
+
+/* INPUT CAPTURE CAP0.0 on P0.22*/
+#define MB_TACHO_PINSEL     PINSEL1
+#define MB_TACHO_PINSEL_VAL 0x02
+#define MB_TACHO_PINSEL_BIT 12
+
+#define MB_TACHO_NB_SLOT 65
+//#define MB_TACHO_NB_SLOT 36
+
+void mb_tacho_init ( void ) {
+  /* select pin for capture */
+  MB_TACHO_PINSEL |= MB_TACHO_PINSEL_VAL << MB_TACHO_PINSEL_BIT;
+  /* enable capture 0.2 on falling edge + trigger interrupt */
+  T0CCR |= TCCR_CR0_F | TCCR_CR0_I;
+}
+
+uint32_t mb_tacho_get_duration( void ) {
+  int_disable();
+  uint32_t my_duration = 0;
+  if (got_one_pulse) {
+    my_duration = mb_tacho_duration;
+  }
+  got_one_pulse = FALSE;
+  int_enable();
+  return my_duration;
+}
+
+float mb_tacho_get_averaged( void ) {
+
+  int_disable();
+  float ret;
+  float tacho;
+  const float tach_to_rpm = 15000000.*60./(float)MB_TACHO_NB_SLOT;
+  if (mb_tacho_nb_pulse) 
+    tacho = mb_tacho_averaged / (float)mb_tacho_nb_pulse ;
+  else
+   tacho = 0.;
+  
+  if (tacho ==0)
+    ret = 0;
+  else
+    ret = tach_to_rpm/tacho;
+  
+  mb_tacho_averaged = 0.;
+  mb_tacho_nb_pulse = 0;
+  int_enable();
+
+  return ret;
+
+}
diff --git a/sw/airborne/motor_bench/mb_tacho.h b/sw/airborne/motor_bench/mb_tacho.h
new file mode 100644
index 0000000000..3e5af6a1f3
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_tacho.h
@@ -0,0 +1,26 @@
+#ifndef MB_TACHO_H
+#define MB_TACHO_H
+
+#include "std.h"
+
+extern void mb_tacho_init ( void );
+extern uint32_t mb_tacho_get_duration( void );
+extern float mb_tacho_get_averaged( void );
+
+extern volatile uint32_t mb_tacho_duration;
+extern volatile uint8_t got_one_pulse;
+extern volatile float mb_tacho_averaged;
+extern volatile uint16_t  mb_tacho_nb_pulse;
+
+#define MB_TACHO_ISR() {						\
+    static uint32_t tmb_last;						\
+    uint32_t t_now = T0CR0;						\
+    uint32_t diff = t_now - tmb_last;					\
+    mb_tacho_duration = diff;						\
+    mb_tacho_averaged += diff;						\
+    mb_tacho_nb_pulse++;						\
+    tmb_last = t_now;							\
+    got_one_pulse = TRUE;                                               \
+  }
+
+#endif /* MB_TACHO_H */
diff --git a/sw/airborne/motor_bench/mb_twi_controller.c b/sw/airborne/motor_bench/mb_twi_controller.c
new file mode 100644
index 0000000000..58b32c095d
--- /dev/null
+++ b/sw/airborne/motor_bench/mb_twi_controller.c
@@ -0,0 +1,28 @@
+#include "mb_twi_controller.h"
+
+#include <string.h>
+
+#include "i2c.h"
+
+uint16_t mb_twi_command;
+
+uint8_t mb_twi_nb_overun;
+uint8_t mb_twi_i2c_done;
+
+
+void mb_twi_controller_init(void) {
+  mb_twi_nb_overun = 0;
+  mb_twi_i2c_done = TRUE;
+}
+
+void mb_twi_controller_set( float throttle ) {
+  if (mb_twi_i2c_done) {
+    mb_twi_command = throttle * MB_TWI_CONTROLLER_MAX_CMD;
+    i2c_buf[0] = (uint8_t)(mb_twi_command&0xFF);
+    i2c_buf[1] = (uint8_t)(mb_twi_command>>8);
+    i2c_transmit(MB_TWI_CONTROLLER_ADDR, 2, &mb_twi_i2c_done);
+  }
+  else
+    mb_twi_nb_overun++;
+
+}
diff --git a/sw/airborne/motor_bench/mb_twi_controller.h b/sw/airborne/motor_bench/mb_twi_controller.h
index 1c21a4dbde..7a55e373ff 100644
--- a/sw/airborne/motor_bench/mb_twi_controller.h
+++ b/sw/airborne/motor_bench/mb_twi_controller.h
@@ -7,6 +7,16 @@ extern void mb_twi_controller_init(void);
 
 extern void mb_twi_controller_set( float throttle );
 
+#define MB_TWI_CONTROLLER_MAX_CMD 65535
+/*
+  Slave address 
+  front = 0x52
+  back  = 0x54
+  right = 0x56
+  left  = 0x58
+*/
+#define MB_TWI_CONTROLLER_ADDR 0x52
+
 #endif /* MB_TWI_CONTROLLER_H */