diff --git a/conf/messages.xml b/conf/messages.xml
index 9f67facbf0..7146bd93ba 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -641,10 +641,13 @@
     <field name="pitch_fb" type="float"/>
     <field name="thrust_ff" type="float"/>
     <field name="thrust_fb" type="float"/>
+    <field name="tilt_sp" type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
     <field name="tilt_ref" type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
     <field name="tilt_dot_ref" type="float" alt_unit="deg/s" alt_unit_coef="57.29578"/>
+    <field name="elevation_sp " type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
     <field name="elevation_ref " type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
     <field name="elevation_dot_ref" type="float" alt_unit="deg/s" alt_unit_coef="57.29578"/>
+    <field name="azimuth_sp" type="float" alt_unit="deg" alt_unit_coef="57.29578"/>
   </message>
 
  <!--109 is free --> 
diff --git a/sw/airborne/beth/main_overo.c b/sw/airborne/beth/main_overo.c
index a28fa13aaf..97b33869da 100644
--- a/sw/airborne/beth/main_overo.c
+++ b/sw/airborne/beth/main_overo.c
@@ -197,8 +197,9 @@ static void main_periodic(int my_sig_num) {
 			&controller.cmd_pitch,&controller.cmd_thrust,
 			&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
 			&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
-  			&controller.tilt_ref,&controller.tilt_dot_ref,
-			&controller.elevation_ref,&controller.elevation_dot_ref);});
+  			&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
+			&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
+			&controller.azimuth_sp);});
 
   //file_logger_periodic();
 
diff --git a/sw/airborne/beth/overo_controller.c b/sw/airborne/beth/overo_controller.c
index 1a09881fe1..e6e68879a5 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 = RadOfDeg(10);
   controller.azimuth_sp = 0.;
 
-  controller.omega_tilt_ref = RadOfDeg(200);
+  controller.omega_tilt_ref = RadOfDeg(600);
   controller.omega_elevation_ref = RadOfDeg(120);
   controller.omega_azimuth_ref = RadOfDeg(60);
   controller.xi_ref = 1.;
@@ -34,7 +34,7 @@ void control_init(void) {
 
   controller.one_over_J = 2.;
   controller.mass = 5.;
-  controller.azim_gain = 1.;
+  controller.azim_gain = 0.05;
 
   controller.omega_cl = RadOfDeg(600);
   controller.xi_cl = 1.;
@@ -107,8 +107,9 @@ void control_run(void) {
   controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) +
                         controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth);
 
-  controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb + 
-                            controller.azim_gain * (controller.cmd_azimuth_fb + controller.cmd_azimuth_ff); 
+  controller.cmd_pitch =  controller.cmd_pitch_ff + controller.cmd_pitch_fb;// + 
+  controller.tilt_sp = 
+                            controller.azim_gain * (-controller.cmd_azimuth_fb );//+ controller.cmd_azimuth_ff); 
   controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant;
   controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation));