diff --git a/sw/airborne/tl_autopilot.c b/sw/airborne/tl_autopilot.c
index be1d934ab2..c496046c8f 100644
--- a/sw/airborne/tl_autopilot.c
+++ b/sw/airborne/tl_autopilot.c
@@ -34,8 +34,9 @@ void tl_autopilot_periodic_task(void) {
     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();
+    tl_control_agl_run();
     SetCommands(tl_control_commands);
     break;
   }
diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c
index 4f1b1ca2f9..cc43e79451 100644
--- a/sw/airborne/tl_control.c
+++ b/sw/airborne/tl_control.c
@@ -24,8 +24,8 @@ float tl_control_agl_sp;
 
 pprz_t tl_control_commands[COMMANDS_NB];
 
-#define TL_CONTROL_RATE_R_PGAIN -120.
-#define TL_CONTROL_RATE_R_DGAIN    5.
+#define TL_CONTROL_RATE_R_PGAIN -100.
+#define TL_CONTROL_RATE_R_DGAIN    4.
 #define TL_CONTROL_RATE_R_IGAIN    0.2
 
 /* setpoints for max stick throw in degres per second */
@@ -51,9 +51,9 @@ float tl_control_attitude_psi_sum_err;
 #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 */
+#define TL_CONTROL_AGL_PGAIN -400 /* PPRZ / m */
+#define TL_CONTROL_AGL_DGAIN -550 /* PPRZ / m */
+#define TL_CONTROL_AGL_IGAIN -1 /* PPRZ / m */
 #define TL_MAX_AGL_ERROR 1
 
 
@@ -63,7 +63,6 @@ float tl_control_attitude_psi_sum_err;
 #define TL_CONTROL_TRIM_R 300
 int16_t tl_control_trim_r;
 
-uint8_t tl_z_mode;
 float tl_control_agl_pgain;
 float tl_control_agl_dgain;
 float tl_control_agl_igain;
@@ -112,8 +111,6 @@ void tl_control_init(void) {
   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;
 }
 
 
@@ -157,29 +154,20 @@ void tl_control_rate_run(void) {
 }
 
 // #define DO_STEPS 
-// #define DO_VERTICAL_STEPS
+#define DO_VERTICAL_STEPS
 
 void tl_control_attitude_read_setpoints_from_rc(void) {
   tl_control_attitude_phi_sp = -rc_values[RADIO_ROLL];
   tl_control_attitude_theta_sp =  rc_values[RADIO_PITCH];
 
-  switch (tl_z_mode) {
-  case TL_Z_MODE_RC:
-    tl_control_power_sp = rc_values[RADIO_THROTTLE];
-    break;
-  case TL_Z_MODE_RM:
-    //tl_control_agl_sp = Max (0, 3. * (rc_values[RADIO_THROTTLE]/MAX_PPRZ - MAX_PPRZ/10)); /* 2m max */
 #ifdef DO_VERTICAL_STEPS
-    if (cpu_time_sec % 30 < 15)
-    tl_control_agl_sp = -1.5;
+  if (cpu_time_sec % 30 < 15)
+    tl_control_agl_sp = -3;
   else
-    tl_control_agl_sp = -2.0;
+    tl_control_agl_sp = -3.0;
 #else
     tl_control_agl_sp = -3. * rc_values[RADIO_THROTTLE]/MAX_PPRZ;
 #endif
-    break;
-  }
-
 
 #ifndef DO_STEPS
   if (!estimator_in_flight)
@@ -245,26 +233,19 @@ void tl_control_nav_read_setpoints_from_rc(void) {
 }
 
 void tl_control_agl_run(void) {
-  switch(tl_z_mode) {
-  case TL_Z_MODE_RC:
-    /* Do nothing */
-    break;
-  case TL_Z_MODE_RM: {
-    float agl_err = tl_estimator_agl - tl_control_agl_sp;
-    Bound(agl_err, -TL_MAX_AGL_ERROR, TL_MAX_AGL_ERROR);
-    
-    if (estimator_in_flight)
-      tl_control_agl_sum_err += agl_err;
-    else
-      tl_control_agl_sum_err = 0;
-    tl_control_power_sp = tl_estimator_cruise_power - /* minus because Z pointing downward */
-      ( tl_control_agl_pgain * agl_err + 
-	tl_control_agl_dgain * tl_estimator_agl_dot + 
-	tl_control_agl_igain * tl_control_agl_sum_err);
-    if (rc_values[RADIO_THROTTLE] < MAX_PPRZ/10) /* FIXME */
-      tl_control_power_sp = 0;
-  }
-  }
+  float agl_err = tl_estimator_agl - tl_control_agl_sp;
+  Bound(agl_err, -TL_MAX_AGL_ERROR, TL_MAX_AGL_ERROR);
+  
+  if (estimator_in_flight)
+    tl_control_agl_sum_err += agl_err;
+  else
+    tl_control_agl_sum_err = 0;
+  tl_control_power_sp = tl_estimator_cruise_power - /* minus because Z pointing downward */
+    ( tl_control_agl_pgain * agl_err + 
+      tl_control_agl_dgain * tl_estimator_agl_dot + 
+      tl_control_agl_igain * tl_control_agl_sum_err);
+  if (rc_values[RADIO_THROTTLE] < MAX_PPRZ/10 || kill_throttle) /* FIXME */
+    tl_control_power_sp = 0;
 }
 
 
diff --git a/sw/airborne/tl_estimator.c b/sw/airborne/tl_estimator.c
index 3854c0ed16..dabd67437b 100644
--- a/sw/airborne/tl_estimator.c
+++ b/sw/airborne/tl_estimator.c
@@ -6,8 +6,10 @@
 #include "tl_vfilter.h"
 
 #include "tl_telemetry.h"
+#include "led.h"
 
 bool_t estimator_in_flight;
+bool_t tl_estimator_set_ground_pressure;
 uint16_t estimator_flight_time;
 
 float tl_estimator_u;
@@ -47,6 +49,10 @@ static const float R_accel = 0.5;
 #define TL_PSI_KALM_UNINIT  0
 #define TL_PSI_KALM_RUNNING 1
 
+#define BARO_ALPHA 1
+
+float tl_baro_alpha;
+
 uint8_t tl_psi_kalm_status;
 float   tl_psi_kalm_psi;
 float   tl_psi_kalm_r;
@@ -78,6 +84,8 @@ void tl_estimator_init(void) {
   tl_estimator_u_dot = 0.;
   tl_estimator_v_dot = 0.;
   tl_estimator_cruise_power = TL_ESTIMATOR_CRUISE_POWER;
+  tl_estimator_set_ground_pressure = TRUE;
+  tl_baro_alpha = BARO_ALPHA;
 }
 
 void tl_estimator_use_gps(void) {
@@ -109,6 +117,8 @@ void tl_estimator_use_gyro(void) {
   }
   tl_vf_predict(tl_imu_accel);
 
+  DOWNLINK_SEND_VF_PREDICT(&tl_imu_accel);
+
   tl_estimator_agl = tl_vf_z;
   tl_estimator_agl_dot = tl_vf_zdot;
 }
@@ -116,8 +126,11 @@ void tl_estimator_use_gyro(void) {
 void tl_estimator_use_imu(void) {
   float estimator_psi_measure = -atan2(tl_imu_hx, -tl_imu_hy) + MAGNETIC_DECLINATION;
   
-  if (!estimator_in_flight)
+  if (tl_estimator_set_ground_pressure) {
     estimator_ground_pressure = tl_imu_pressure;
+    estimator_z_baro = 0;
+  }
+
   estimator_z_baro = (estimator_ground_pressure - (float)tl_imu_pressure)*0.084;
 
   if (tl_psi_kalm_status == TL_PSI_KALM_UNINIT) {
@@ -131,6 +144,7 @@ void tl_estimator_use_imu(void) {
   tl_vf_update(-tl_imu_rm);
 #else
   tl_vf_update(-estimator_z_baro);
+  DOWNLINK_SEND_VF_UPDATE(&estimator_z_baro, &tl_imu_rm);
 #endif
 }
 
diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h
index a66aeae1bd..6178e2779d 100644
--- a/sw/airborne/tl_estimator.h
+++ b/sw/airborne/tl_estimator.h
@@ -4,6 +4,7 @@
 #include "std.h"
 
 extern bool_t estimator_in_flight;
+extern bool_t tl_estimator_set_ground_pressure;
 extern uint16_t estimator_flight_time;
 
 extern float tl_estimator_u;
@@ -37,6 +38,7 @@ void tl_estimator_use_gyro(void);
 void tl_estimator_use_imu(void);
 void tl_estimator_periodic_task(void);
 
+extern float tl_baro_alpha;
 extern float   tl_psi_kalm_psi;
 extern float   tl_psi_kalm_bias;
 extern float   tl_psi_kalm_P[2][2];
diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c
index 20e8136950..f620ac5e88 100644
--- a/sw/airborne/tl_main.c
+++ b/sw/airborne/tl_main.c
@@ -61,8 +61,13 @@ static inline void tl_main_init( void ) {
   uart0_init_tx();
   uart1_init_tx();
 
+  // gps_init();
+
   int_enable();
 
+  // sys_time_usleep(500000);
+  //  gps_configure_uart();
+
   DOWNLINK_SEND_BOOT(&cpu_time_sec);
 }
 
diff --git a/sw/airborne/tl_nav.c b/sw/airborne/tl_nav.c
index 2eb4c2e01b..d231614d1d 100644
--- a/sw/airborne/tl_nav.c
+++ b/sw/airborne/tl_nav.c
@@ -4,6 +4,7 @@
 #include "tl_estimator.h"
 #include "tl_control.h"
 #include "gps.h"
+#include "radio_control.h"
 
 float tl_nav_goto_h_pgain;
 float tl_nav_goto_h_dgain;