diff --git a/sw/airborne/arm7/tl_imu.h b/sw/airborne/arm7/tl_imu.h
index 0a9aa27db8..1f083216c8 100644
--- a/sw/airborne/arm7/tl_imu.h
+++ b/sw/airborne/arm7/tl_imu.h
@@ -29,13 +29,20 @@
 #define HX_GAIN    -1.
 #define HX_NEUTRAL  0.
 #define HX_CHAN     1
-#define HY_GAIN    -1.
-#define HY_NEUTRAL  0
+#define HY_GAIN    -1.06
+#define HY_NEUTRAL  -75
 #define HY_CHAN     0
 #define HZ_GAIN    -1.
 #define HZ_NEUTRAL  0
 #define HZ_CHAN     2
 
+/* http://www.ngdc.noaa.gov/seg/geomag/declination.shtml
+  LA
+*/
+#define MAGNETIC_DECLINATION RadOfDeg(13)
+
+
+
 
 #define TL_IMU_IDLE             0
 #define TL_IMU_SENDING_BARO_REQ 1
diff --git a/sw/airborne/tl_autopilot.c b/sw/airborne/tl_autopilot.c
index c7f5a638b0..be1d934ab2 100644
--- a/sw/airborne/tl_autopilot.c
+++ b/sw/airborne/tl_autopilot.c
@@ -27,10 +27,14 @@ void tl_autopilot_periodic_task(void) {
     tl_control_agl_run();
     SetCommands(tl_control_commands);
     break;
+  case TL_AP_MODE_SPEED:
+    tl_control_speed_run();
+    tl_control_attitude_run();
+    SetCommands(tl_control_commands);
+    break;
   case TL_AP_MODE_NAV:
     /* 4Hz */
-    RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
-
+    //    RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
     tl_control_attitude_run();
     SetCommands(tl_control_commands);
     break;
@@ -52,6 +56,9 @@ void tl_autopilot_on_rc_event(void) {
   case TL_AP_MODE_ATTITUDE:
     tl_control_attitude_read_setpoints_from_rc();
     break;
+  case TL_AP_MODE_SPEED:
+    tl_control_speed_read_setpoints_from_rc();
+    break;
   case TL_AP_MODE_NAV:
     tl_control_nav_read_setpoints_from_rc();
     break;
diff --git a/sw/airborne/tl_autopilot.h b/sw/airborne/tl_autopilot.h
index 9857db1a0a..b080ad62e5 100644
--- a/sw/airborne/tl_autopilot.h
+++ b/sw/airborne/tl_autopilot.h
@@ -8,6 +8,7 @@
 #define TL_AP_MODE_RATE     2
 #define TL_AP_MODE_ATTITUDE 3
 #define TL_AP_MODE_NAV      4
+#define TL_AP_MODE_SPEED    5
 
 extern uint8_t tl_autopilot_mode;
 
@@ -27,15 +28,6 @@ extern void tl_autopilot_on_rc_event(void);
 
 #define TL_AP_MODE_OF_PPRZ(mode)				\
   ((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_RATE :		\
-   TL_AP_MODE_ATTITUDE )
-
-#if 0
-#define TL_AP_MODE_OF_PPRZ(mode)				\
-  ((mode) < TRESHOLD_RATE_PPRZ ? TL_AP_MODE_ATTITUDE :		\
-   TL_AP_MODE_NAV )
-#endif
-
-
-
+   TL_AP_MODE_AUTO )
 
 #endif /* TL_AUTOPILOT_H */
diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c
index bfd22578f5..4f1b1ca2f9 100644
--- a/sw/airborne/tl_control.c
+++ b/sw/airborne/tl_control.c
@@ -47,6 +47,10 @@ float tl_control_attitude_psi_sum_err;
 #define TL_CONTROL_ATTITUDE_PSI_IGAIN    -0.20
 #define TL_CONTROL_ATTITUDE_PSI_TRIM    350
 
+#define TL_CONTROL_SPEED_PGAIN  -1000.
+#define TL_CONTROL_SPEED_DGAIN  0.
+#define TL_CONTROL_SPEED_IGAIN   -0.20
+
 #define TL_CONTROL_AGL_PGAIN -1050 /* PPRZ / m */
 #define TL_CONTROL_AGL_DGAIN -1150 /* PPRZ / m */
 #define TL_CONTROL_AGL_IGAIN -2 /* PPRZ / m */
@@ -66,6 +70,14 @@ float tl_control_agl_igain;
 float tl_control_agl_sum_err;
 
 
+#define TL_MAX_SPEED_ERROR 2
+float tl_control_u_sp;
+float tl_control_v_sp;
+float tl_control_speed_pgain;
+float tl_control_speed_dgain;
+float tl_control_speed_igain;
+float tl_control_speed_u_sum_err;
+float tl_control_speed_v_sum_err;
 
 void tl_control_init(void) {
 
@@ -95,12 +107,17 @@ void tl_control_init(void) {
   tl_control_agl_igain = TL_CONTROL_AGL_IGAIN;
   tl_control_agl_sum_err = 0;
 
+  tl_control_speed_pgain = TL_CONTROL_SPEED_PGAIN;
+  tl_control_speed_dgain = TL_CONTROL_SPEED_DGAIN;
+  tl_control_speed_igain = TL_CONTROL_SPEED_IGAIN;
+  tl_control_speed_u_sum_err = 0;
+  tl_control_speed_v_sum_err = 0;
+
   tl_z_mode = TL_Z_MODE_RM;
 }
 
 
 void tl_control_rate_read_setpoints_from_rc(void) {
-
   tl_control_p_sp = -rc_values[RADIO_ROLL];
   tl_control_q_sp =  rc_values[RADIO_PITCH];
   tl_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
@@ -249,3 +266,40 @@ void tl_control_agl_run(void) {
   }
   }
 }
+
+
+void tl_control_speed_read_setpoints_from_rc(void) {
+  tl_control_v_sp = (2. * rc_values[RADIO_ROLL])/MAX_PPRZ;
+  tl_control_u_sp = (2. * -rc_values[RADIO_PITCH])/MAX_PPRZ;
+
+  if (!estimator_in_flight)
+    tl_control_attitude_psi_sp = estimator_psi;
+  else {
+    tl_control_attitude_psi_sp += -rc_values[RADIO_YAW] * RadOfDeg(TL_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
+    NormRadAngle(tl_control_attitude_psi_sp);
+  }
+  tl_control_power_sp = rc_values[RADIO_THROTTLE];
+}
+
+void tl_control_speed_run(void) {
+  float err_u = tl_estimator_u - tl_control_u_sp;
+  Bound(err_u, -TL_MAX_SPEED_ERROR, TL_MAX_SPEED_ERROR);
+  float err_v = tl_estimator_v - tl_control_v_sp;
+  Bound(err_v, -TL_MAX_SPEED_ERROR, TL_MAX_SPEED_ERROR);
+
+  if(estimator_in_flight) {
+    tl_control_speed_u_sum_err += err_u;
+    tl_control_speed_v_sum_err += err_v;
+  } else {
+    tl_control_speed_u_sum_err = 0;
+    tl_control_speed_v_sum_err = 0;
+  }
+  
+  tl_control_attitude_theta_sp = - (tl_control_speed_pgain *  err_u + 
+			   tl_control_speed_dgain * tl_estimator_u_dot +
+			   tl_control_speed_igain * tl_control_speed_u_sum_err );
+
+  tl_control_attitude_phi_sp = - (tl_control_speed_pgain * err_v + 
+			   tl_control_speed_dgain * tl_estimator_v_dot +
+			   tl_control_speed_igain * tl_control_speed_v_sum_err );
+}
diff --git a/sw/airborne/tl_control.h b/sw/airborne/tl_control.h
index 0f3fc7b25d..0be4151612 100644
--- a/sw/airborne/tl_control.h
+++ b/sw/airborne/tl_control.h
@@ -12,7 +12,9 @@ extern void tl_control_rate_run(void);
 extern void tl_control_attitude_read_setpoints_from_rc(void);
 extern void tl_control_attitude_run(void);
 extern void tl_control_agl_run(void);
+extern void tl_control_speed_run(void);
 extern void tl_control_nav_read_setpoints_from_rc(void);
+extern void tl_control_speed_read_setpoints_from_rc(void);
 
 extern bool_t kill_throttle;
 
@@ -51,6 +53,13 @@ extern float tl_control_agl_igain;
 extern float tl_control_agl_sum_err;
 extern float tl_control_agl_sp;
 
+extern float tl_control_u_sp;
+extern float tl_control_v_sp;
+extern float tl_control_speed_pgain;
+extern float tl_control_speed_dgain;
+extern float tl_control_speed_igain;
+extern float tl_control_speed_u_sum_err;
+extern float tl_control_speed_v_sum_err;
 
 extern int16_t tl_control_trim_r;
 
diff --git a/sw/airborne/tl_estimator.c b/sw/airborne/tl_estimator.c
index ee3d5dbc74..3854c0ed16 100644
--- a/sw/airborne/tl_estimator.c
+++ b/sw/airborne/tl_estimator.c
@@ -12,6 +12,8 @@ uint16_t estimator_flight_time;
 
 float tl_estimator_u;
 float tl_estimator_v;
+float tl_estimator_u_dot;
+float tl_estimator_v_dot;
 
 float estimator_x; /* m */
 float estimator_y; /* m */
@@ -73,6 +75,8 @@ void tl_estimator_init(void) {
   compute_dcm();
   tl_estimator_u = 0.;
   tl_estimator_v = 0.;
+  tl_estimator_u_dot = 0.;
+  tl_estimator_v_dot = 0.;
   tl_estimator_cruise_power = TL_ESTIMATOR_CRUISE_POWER;
 }
 
@@ -110,7 +114,7 @@ void tl_estimator_use_gyro(void) {
 }
 
 void tl_estimator_use_imu(void) {
-  float estimator_psi_measure = -atan2(tl_imu_hy, tl_imu_hx);
+  float estimator_psi_measure = -atan2(tl_imu_hx, -tl_imu_hy) + MAGNETIC_DECLINATION;
   
   if (!estimator_in_flight)
     estimator_ground_pressure = tl_imu_pressure;
diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h
index 76e0060a40..a66aeae1bd 100644
--- a/sw/airborne/tl_estimator.h
+++ b/sw/airborne/tl_estimator.h
@@ -8,6 +8,8 @@ extern uint16_t estimator_flight_time;
 
 extern float tl_estimator_u;
 extern float tl_estimator_v;
+extern float tl_estimator_u_dot;
+extern float tl_estimator_v_dot;
 
 extern float estimator_x; /* m */
 extern float estimator_y; /* m */
diff --git a/sw/airborne/tl_gps_configure.h b/sw/airborne/tl_gps_configure.h
index 38885d80b4..039c7339f7 100644
--- a/sw/airborne/tl_gps_configure.h
+++ b/sw/airborne/tl_gps_configure.h
@@ -1,7 +1,7 @@
 static bool_t user_gps_configure(bool_t cpt) {
   switch (cpt) {
   case 0:
-    UbxSend_CFG_NAV(NAV_DYN_PEDESTRIAN, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
+    UbxSend_CFG_NAV(NAV_DYN_STATIONARY, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
     break;
   case 1:
     UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c
index 8c24bcd8fa..20e8136950 100644
--- a/sw/airborne/tl_main.c
+++ b/sw/airborne/tl_main.c
@@ -89,7 +89,6 @@ static inline void tl_main_periodic_task( void ) {
   if (rc_status != RC_OK)
      tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
 
- 
   tl_telemetry_periodic_task();
 
   SetActuatorsFromCommands(commands);
diff --git a/sw/airborne/tl_telemetry.h b/sw/airborne/tl_telemetry.h
index 889012d18d..cd1e0594b9 100644
--- a/sw/airborne/tl_telemetry.h
+++ b/sw/airborne/tl_telemetry.h
@@ -66,9 +66,11 @@
     }									\
   }
 
-#define PERIODIC_SEND_TL_VERTICAL_LOOP() DOWNLINK_SEND_TL_VERTICAL_LOOP(&tl_estimator_agl, &tl_control_agl_sp, &tl_control_agl_sum_err, &tl_estimator_agl_dot, &tl_estimator_cruise_power)
+#define PERIODIC_SEND_TL_VERTICAL_LOOP() DOWNLINK_SEND_TL_VERTICAL_LOOP(&tl_estimator_agl, &tl_control_agl_sp, &tl_control_agl_sum_err, &tl_estimator_agl_dot)
 
-#define PERIODIC_SEND_TL_DEBUG() DOWNLINK_SEND_TL_DEBUG(&x_unit_err_body, &y_unit_err_body)
+#define PERIODIC_SEND_TL_SPEED_LOOP() DOWNLINK_SEND_TL_SPEED_LOOP(&tl_estimator_u, &tl_control_u_sp, &tl_control_speed_u_sum_err, &tl_estimator_v, &tl_control_v_sp, &tl_control_speed_v_sum_err)
+
+#define PERIODIC_SEND_TL_DEBUG() DOWNLINK_SEND_TL_DEBUG(&tl_autopilot_mode)
 
 
 #define PERIODIC_SEND_TL_KALM_PSI_STATE() {				\