diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml
index 36db4a5613..d4faa4b5c6 100644
--- a/conf/airframes/motor_bench.xml
+++ b/conf/airframes/motor_bench.xml
@@ -38,6 +38,9 @@ main.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1
 main.srcs += $(SRC_ARCH)/adc_hw.c
 main.srcs += $(SRC_ARCH)/mb_current.c
 
+main.CFLAGS += -DMB_SCALE
+main.srcs += $(SRC_ARCH)/mb_scale.c
+
 main.srcs += $(SRC_ARCH)/mb_modes.c
 
 
diff --git a/conf/messages.xml b/conf/messages.xml
index d8f3c892cf..78a7d73e24 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -402,6 +402,8 @@
       <field name="throttle" type="float" format="%.3f"/>
       <field name="rpm" type="float"/>
       <field name="current" type="float"/>
+      <field name="thrust" type="float"/>
+      <field name="torque" type="float"/>
       <field name="time_s" type="uint16"/>
       <field name="mode" type="uint8"/>
       
diff --git a/sw/airborne/arm7/mb_current.c b/sw/airborne/arm7/mb_current.c
index b97b784a51..5e3046cde4 100644
--- a/sw/airborne/arm7/mb_current.c
+++ b/sw/airborne/arm7/mb_current.c
@@ -13,9 +13,5 @@ void mb_current_init(void) {
 
 
 void mb_current_periodic(void) {
-  uint16_t cur_int = mb_current_buf.sum / mb_current_buf.av_nb_sample;
-  //  mb_current_amp = (float)mb_current_buf.sum;
-  //  mb_current_amp = (((float)cur_int / 1024 * 5) - 0.77)/ 0.185;
   mb_current_amp = (float)mb_current_buf.sum * 0.00113607  - 2.8202;
-
 }
diff --git a/sw/airborne/arm7/mb_modes.c b/sw/airborne/arm7/mb_modes.c
index 08515cf52d..29defc90c6 100644
--- a/sw/airborne/arm7/mb_modes.c
+++ b/sw/airborne/arm7/mb_modes.c
@@ -28,7 +28,7 @@ void mb_mode_init(void) {
   mb_modes_mode = MB_MODES_IDLE;
   mb_modes_throttle = 0.;
 
-  mb_modes_ramp_duration = 20;
+  mb_modes_ramp_duration = 40;
 
   mb_modes_step_low_throttle = 0.6;
   mb_modes_step_high_throttle = 0.7;
diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c
index 233aa01938..fa40c30f26 100644
--- a/sw/airborne/arm7/sys_time_hw.c
+++ b/sw/airborne/arm7/sys_time_hw.c
@@ -10,13 +10,14 @@ uint32_t cpu_time_ticks;
 
 #include "ppm.h"
 
-#ifdef ICP_SCALE
-#include "icp_scale.h"
-#endif /* ICP_SCALE */
+#ifdef MB_SCALE
+#include "mb_scale.h"
+#endif /* MB_SCALE */
 
 #ifdef MB_TACHO
 #include "mb_tacho.h"
-#define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I | TIR_CR0I)
+// FIXME : declared the scale interrupt here :(
+#define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I | TIR_CR0I | TIR_CR3I)
 #else
 #define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I)
 #endif /* MB_TACHO */
@@ -47,11 +48,11 @@ void TIMER0_ISR ( void ) {
       T0IR = TIR_MR1I; 
     }
 #endif
-#ifdef ICP_SCALE
-    if (T0IR&TIR_CR2I) {
-      ICP_ISR();
+#ifdef MB_SCALE
+    if (T0IR&TIR_CR3I) {
+       MB_SCALE_ICP_ISR();
       /* clear interrupt */
-      T0IR = TIR_CR2I;
+      T0IR = TIR_CR3I;
     }
 #endif
 #ifdef MB_TACHO
diff --git a/sw/airborne/main_motor_bench.c b/sw/airborne/main_motor_bench.c
index ef3856d3d2..0150475493 100644
--- a/sw/airborne/main_motor_bench.c
+++ b/sw/airborne/main_motor_bench.c
@@ -7,6 +7,7 @@
 #include "mb_tacho.h"
 #include "mb_servo.h"
 #include "mb_current.h"
+#include "mb_scale.h"
 
 
 #include "uart.h"
@@ -42,11 +43,12 @@ static inline void main_init( void ) {
   sys_time_init();
   mb_tacho_init();
   mb_servo_init();
+  mb_servo_set_range( 1275000, 1825000 );
   adc_init();
   mb_current_init();
+  mb_scale_init();
 
   uart0_init_tx();
-  mb_servo_arm();
   mb_mode_init();
 
   int_enable();
@@ -59,12 +61,15 @@ 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%10)) {
     LED_TOGGLE(1);
   }
-    DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &throttle, &rpm, &amps , &cpu_time_sec, &mb_modes_mode);
+  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/in_progress/motor_bench/main.c b/sw/in_progress/motor_bench/main.c
index c1100c5a36..ef0dd0ec9f 100644
--- a/sw/in_progress/motor_bench/main.c
+++ b/sw/in_progress/motor_bench/main.c
@@ -70,15 +70,17 @@ static void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, c
   double throttle =  atof(argv[1]);
   double rpm =  atof(argv[2]);
   double amp =  atof(argv[3]);
-  guint time_sec = atoi(argv[4]);
-  guint mode = atoi(argv[5]);
+  double thrust =  atof(argv[4]);
+  double torque =  atof(argv[5]);
+  guint time_sec = atoi(argv[6]);
+  guint mode = atoi(argv[7]);
   mb_state.mode = mode;
   mb_state.time = (double)time_sec + (double)time_ticks/15000000.;
   mb_state.throttle = throttle;
   mb_state.rpms = rpm;
   mb_state.amps = amp;
-  mb_state.thrust = 0.;
-  mb_state.torque = 0.;
+  mb_state.thrust = thrust;
+  mb_state.torque = torque;
   if (mb_state.log_channel) {
     GString* str = g_string_sized_new(256);
     g_string_printf(str, "%.4f %.3f %.0f %.1f %.1f %.1f\n", mb_state.time, mb_state.throttle, mb_state.rpms, mb_state.amps, mb_state.thrust, mb_state.torque);
@@ -100,9 +102,9 @@ static gboolean timeout_callback(gpointer data) {
   gtk_label_set_text(GTK_LABEL(mb_gui.lab_rpms), str->str);
   g_string_printf(str, "%.1f a", mb_state.amps);
   gtk_label_set_text(GTK_LABEL(mb_gui.lab_amps), str->str);
-  g_string_printf(str, "%.1f N", mb_state.thrust);
+  g_string_printf(str, "%.1f g", mb_state.thrust);
   gtk_label_set_text(GTK_LABEL(mb_gui.lab_thrust), str->str);
-  g_string_printf(str, "%.1f N", mb_state.torque);
+  g_string_printf(str, "%.1f g", mb_state.torque);
   gtk_label_set_text(GTK_LABEL(mb_gui.lab_torque), str->str);
   g_string_free(str, TRUE);
   return TRUE;
@@ -132,7 +134,7 @@ int main (int argc, char** argv) {
   gtk_widget_show_all(window);
 
   IvyInit ("MotorBench", "MotorBench READY", NULL, NULL, NULL, NULL);
-  IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+  IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
   IvyStart("127.255.255.255");
 
   g_timeout_add(40, timeout_callback, NULL);
diff --git a/sw/in_progress/motor_bench/mb_utils.sci b/sw/in_progress/motor_bench/mb_utils.sci
index 7017053fe5..b45bdfee34 100644
--- a/sw/in_progress/motor_bench/mb_utils.sci
+++ b/sw/in_progress/motor_bench/mb_utils.sci
@@ -30,3 +30,20 @@ end
 mclose(u);
 
 endfunction
+
+
+
+function [filtered] = low_pass_filter(f_sample, f_cut, raw_data)
+  
+  delta_t = 1/f_sample;
+  rc = 1 / ( 2 * %pi * f_cut);
+  alpha = delta_t / ( delta_t + rc );
+  
+  filtered=[raw_data(1)];
+  for i=2:length(raw_data)
+    fv = alpha * raw_data(i) + (1 - alpha) * filtered(i-1);
+    filtered = [filtered fv];
+  end
+  
+endfunction
+
diff --git a/sw/in_progress/motor_bench/test.sce b/sw/in_progress/motor_bench/test.sce
index 85b72ad35a..0a3ec401eb 100644
--- a/sw/in_progress/motor_bench/test.sce
+++ b/sw/in_progress/motor_bench/test.sce
@@ -10,11 +10,21 @@ filename = args(nb_args);
 [time, throttle, rpm, amp, thrust, torque] = read_mb_log(filename);
 
 
+f_sample = 200.;
+fc = 100.;
+f_rpm = low_pass_filter(f_sample, fc, rpm);
+
+
 xbasc();
-subplot(2,1,1)
+subplot(3,1,1)
 xtitle('Throttle');
 plot2d(time, throttle);
 
-subplot(2,1,2)
+subplot(3,1,2)
 xtitle('Rpm');
 plot2d(time, rpm);
+
+subplot(3,1,3)
+xtitle('Filtered Rpm');
+//plot2d(time, f_rpm);
+plot2d(rpm, throttle);