diff --git a/conf/airframes/tl.xml b/conf/airframes/tl.xml
index 689abea6cd..27d832abce 100644
--- a/conf/airframes/tl.xml
+++ b/conf/airframes/tl.xml
@@ -50,7 +50,7 @@ ap.ARCH = arm7tdmi
 ap.TARGET = flt
 ap.TARGETDIR = flt
 
-ap.CFLAGS += -DCONFIG=\"tiny_1_1.h\"  -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(15e-3)' 
+ap.CFLAGS += -DCONFIG=\"tiny_1_1.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(16.666e-3)' 
 ap.srcs = tl_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 
 ap.CFLAGS += -DLED  -DLED -DTIME_LED=1
@@ -68,15 +68,19 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # transparent 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c pprz_transport.c tl_telemetry.c tl_datalink.c
+ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c pprz_transport.c tl_telemetry.c datalink.c
 
 ap.srcs += tl_autopilot.c tl_control.c tl_estimator.c
 
 ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DDOWNLINK_GPS_DEVICE=DOWNLINK_AP_DEVICE -DGPS_CONFIGURE -DUSER_GPS_CONFIGURE=\"tl_gps_configure.h\" -DGPS_BAUD=38400
 ap.srcs += gps_ubx.c gps.c latlong.c
 
+ap.CFLAGS += -DNAV
 ap.srcs += common_nav.c tl_nav.c
 
+ap.CFLAGS += -D BARO_MS5534A_W1=0xABDC -D BARO_MS5534A_W2=0x849A -D BARO_MS5534A_W3=0X939E -D BARO_MS5534A_W4=0xB259
+
+
   </makefile>
 
 </airframe>
diff --git a/conf/settings/basic.xml b/conf/settings/basic.xml
index 630d849dec..fd056920d9 100644
--- a/conf/settings/basic.xml
+++ b/conf/settings/basic.xml
@@ -16,7 +16,7 @@
     </dl_settings>
 
     <dl_settings NAME="mode">
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode">
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="estimator">
       </dl_setting>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch">
       </dl_setting>
diff --git a/conf/settings/tl.xml b/conf/settings/tl.xml
index c6a1150e85..fc0ddeafc2 100644
--- a/conf/settings/tl.xml
+++ b/conf/settings/tl.xml
@@ -4,7 +4,7 @@
   <dl_settings>
     <dl_settings NAME="flight params">
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="estimator_in_flight" shortname="in_flight" module="tl_estimator"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle" shortname="in_flight" module="tl_control"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle" shortname="kill" module="tl_control"/>
    </dl_settings>
  </dl_settings>
 </settings>
diff --git a/conf/telemetry/default.xml b/conf/telemetry/default.xml
index 879cab0a79..1d63df5980 100644
--- a/conf/telemetry/default.xml
+++ b/conf/telemetry/default.xml
@@ -3,6 +3,7 @@
 <telemetry>
   <process name="Ap">
     <mode name="default">
+      <message name="ALIVE"          period="5"/>
       <message name="ATTITUDE"       period="0.5"/>
       <message name="ESTIMATOR"      period="0.5"/>
       <message name="WP_MOVED"       period="0.5"/>
diff --git a/conf/telemetry/tl.xml b/conf/telemetry/tl.xml
index c8bc28810d..de2cda47d2 100644
--- a/conf/telemetry/tl.xml
+++ b/conf/telemetry/tl.xml
@@ -5,12 +5,16 @@
 
   <process name="Ap">
     <mode name="default">
+      <message name="ALIVE"          period="5"/>
       <message name="PPM"            period="5"/>
       <message name="RC"             period="5"/>
       <message name="COMMANDS"       period="5"/>
       <message name="ACTUATORS"      period="1"/> <!-- For trimming -->
       <message name="BAT"            period="3"/>
       <message name="ESTIMATOR"      period="1"/>
+      <message name="NAVIGATION_REF" period="9"/>
+      <message name="NAVIGATION"     period="1"/>
+      <message name="WP_MOVED"       period="0.5"/>
     </mode>
   </process>
 
diff --git a/sw/airborne/tl_main.c b/sw/airborne/tl_main.c
index 2707d3a5f3..07e2dd77f3 100644
--- a/sw/airborne/tl_main.c
+++ b/sw/airborne/tl_main.c
@@ -67,7 +67,8 @@ static inline void tl_main_periodic_task( void ) {
   if (rc_status != RC_OK)
      tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
 
-  RunOnceEvery(15, tl_nav_periodic_task());
+  /* 4Hz */
+  RunOnceEvery(15, { common_nav_periodic_task_4Hz(); tl_nav_periodic_task(); });
   
   /* run control loops */
   tl_autopilot_periodic_task();
diff --git a/sw/airborne/tl_telemetry.h b/sw/airborne/tl_telemetry.h
index 0ee4488205..a2209414a5 100644
--- a/sw/airborne/tl_telemetry.h
+++ b/sw/airborne/tl_telemetry.h
@@ -10,17 +10,36 @@
 #include "actuators.h"
 #include "tl_bat.h"
 #include "tl_estimator.h"
+#include "tl_nav.h"
+#include "tl_control.h"
 
 
 
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 
+#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE()
 #define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values)
 #define PERIODIC_SEND_PPM() DOWNLINK_SEND_PPM(PPM_NB_PULSES, ppm_pulses)
 #define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands)
 #define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators)
-#define PERIODIC_SEND_BAT() { int16_t dummy16=42; uint8_t dummy8=42; DOWNLINK_SEND_BAT(&commands[COMMAND_THROTTLE], &tl_bat_decivolt, &estimator_flight_time, &dummy8, &dummy16, &dummy16, &dummy16); }
+#define PERIODIC_SEND_BAT() { int16_t dummy16=42; DOWNLINK_SEND_BAT(&commands[COMMAND_THROTTLE], &tl_bat_decivolt, &estimator_flight_time, &kill_throttle, &block_time, &stage_time, &dummy16); }
 #define PERIODIC_SEND_ESTIMATOR() DOWNLINK_SEND_ESTIMATOR(&estimator_z, &estimator_climb)
+#define PERIODIC_SEND_NAVIGATION_REF()  DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
+
+#define DownlinkSendWp(i) { \
+  float x = nav_utm_east0 +  waypoints[i].x; \
+  float y = nav_utm_north0 + waypoints[i].y; \
+  DOWNLINK_SEND_WP_MOVED(&i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
+}
+#define PERIODIC_SEND_WP_MOVED() { \
+  static uint8_t i; \
+  i++; if (i >= nb_waypoint) i = 0; \
+  DownlinkSendWp(i); \
+}
+
+#define SEND_NAVIGATION() { int16_t pos_x = estimator_x; int16_t pos_y = estimator_y; DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &pos_x, &pos_y, &dist2_to_wp, &dist2_to_home);}
+#define PERIODIC_SEND_NAVIGATION() SEND_NAVIGATION()
+