diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c
index 26f2574f2c..6f79351198 100644
--- a/sw/airborne/beth/main_overo.c
+++ b/sw/airborne/beth/main_overo.c
@@ -122,7 +122,7 @@ static void main_periodic(int my_sig_num) {
   estimator_run(msg_in.bench_sensor.z,msg_in.bench_sensor.y,msg_in.bench_sensor.x);
  
 
-  controller.elevation_sp = ((int32_t)(foo/512.)%2) ? RadOfDeg(-15) : RadOfDeg(0);
+  controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10);
 
   control_run();
 
diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c
index 0a746f9088..b39c248b78 100644
--- a/sw/airborne/beth/overo_controller.c
+++ b/sw/airborne/beth/overo_controller.c
@@ -14,7 +14,7 @@ void control_init(void) {
   controller.elevation_sp = 0.;
 
   controller.omega_tilt_ref = RadOfDeg(200);
-  controller.omega_elevation_ref = RadOfDeg(50);
+  controller.omega_elevation_ref = RadOfDeg(120);
   controller.xi_ref = 1.;
 
   controller.tilt_ref = estimator.tilt;
@@ -25,9 +25,10 @@ void control_init(void) {
   controller.elevation_dot_ref = estimator.elevation_dot;
   controller.elevation_ddot_ref = 0.;
 
-  controller.one_over_J = 1.;
+  controller.one_over_J = 2.;
+  controller.mass = 5.;
 
-  controller.omega_cl = RadOfDeg(500);
+  controller.omega_cl = RadOfDeg(600);
   controller.xi_cl = 1.;
 
   controller.cmd_pitch_ff = 0.;
@@ -47,7 +48,7 @@ void control_run(void) {
   /*
    *  propagate reference
    */
-  const float dt_ctl = 1./500.;
+  const float dt_ctl = 1./512.;
   const float thrust_constant = 40.;
   
   controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl;
@@ -72,17 +73,18 @@ void control_run(void) {
   controller.cmd_pitch_fb = controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_tilt_dot) +
   			controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_tilt);
 
-  controller.cmd_thrust_ff = controller.one_over_J*controller.elevation_ddot_ref;
-  controller.cmd_thrust_fb = -controller.one_over_J*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot) -
-  			controller.one_over_J*(controller.omega_cl*controller.omega_cl*err_elevation);
+  controller.cmd_thrust_ff = controller.mass*controller.elevation_ddot_ref;
+  controller.cmd_thrust_fb = -controller.mass*(2*controller.xi_cl*controller.omega_cl*err_elevation_dot) -
+  			controller.mass*(controller.omega_cl*controller.omega_cl*err_elevation);
 
   controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb; 
   controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant;
+
   if (controller.cmd_thrust<0.) controller.cmd_thrust = 0;
 
   if (!(foo%100)) {
     //printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot);
-    printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
+    //printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot);
     //printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref);
   }
   foo++; 
diff --git a/sw/airborne/beth/overo_controller.h b/sw/airborne/beth/overo_controller.h
index da34aa2ea1..9aa7d06acd 100644
--- a/sw/airborne/beth/overo_controller.h
+++ b/sw/airborne/beth/overo_controller.h
@@ -23,6 +23,7 @@ struct OveroController {
 
   /* invert control law parameter */
   float one_over_J;
+  float mass;
 
   /* closed loop parameters */
   float omega_cl;