diff --git a/conf/airframes/microjet6.xml b/conf/airframes/microjet6.xml
index f0deb11836..25f02e5754 100755
--- a/conf/airframes/microjet6.xml
+++ b/conf/airframes/microjet6.xml
@@ -245,8 +245,8 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
 
-#ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
-#ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
+ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
+ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
 
 ap.srcs += nav_line.c
 
diff --git a/conf/airframes/tl.xml b/conf/airframes/tl.xml
index 27d832abce..62a6e3932b 100644
--- a/conf/airframes/tl.xml
+++ b/conf/airframes/tl.xml
@@ -39,6 +39,11 @@
     <set command="CAM"    value="@GAIN1"/>
    </rc_commands>
 
+  <section name="horizontal_control">
+    <define name="NAV_GOTO_H_PGAIN" value="RadOfDeg(10)" unit="rad/m"/>
+    <define name="NAV_GOTO_H_DGAIN" value="0"/>
+  </section>
+
 
   <makefile>
 ARCHI=arm7
diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml
index 297bc9ae1d..365643f047 100644
--- a/conf/airframes/twinjet1.xml
+++ b/conf/airframes/twinjet1.xml
@@ -224,7 +224,7 @@ ap.srcs += infrared.c estimator.c
 ap.CFLAGS += -DGYRO -DADXRS150
 ap.srcs += gyro.c
 
-ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
+ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DWIND_INFO
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
 ap.srcs += nav_line.c
diff --git a/conf/flight_plans/tl.xml b/conf/flight_plans/tl.xml
index f854efedec..75bd1817aa 100644
--- a/conf/flight_plans/tl.xml
+++ b/conf/flight_plans/tl.xml
@@ -20,6 +20,8 @@
       <stay wp="HOME"/>
     </block>
     <block name="target">
+      <call fun="NavSetWaypointHere(WP_TARGET)"/>
+      <set value="0" var="kill_throttle"/>
       <stay wp="TARGET"/>
     </block>
   </blocks>
diff --git a/sw/airborne/common_nav.h b/sw/airborne/common_nav.h
index 30949c208d..ffc478217f 100644
--- a/sw/airborne/common_nav.h
+++ b/sw/airborne/common_nav.h
@@ -69,4 +69,10 @@ void common_nav_periodic_task_4Hz(void);
 
 #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
 
+#define NavSetWaypointHere(_wp) ({ \
+  waypoints[_wp].x = estimator_x; \
+  waypoints[_wp].y = estimator_y; \
+  FALSE; \
+})
+
 #endif /* COMMON_NAV_H */
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index 26d7455d6d..0ec82feac6 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -101,12 +101,6 @@ void nav_home(void);
 void nav_init(void);
 void nav_without_gps(void);
 
-#define NavSetWaypointHere(_wp) ({ \
-  waypoints[_wp].x = estimator_x; \
-  waypoints[_wp].y = estimator_y; \
-  FALSE; \
-})
-
 extern float nav_circle_trigo_qdr; /** Angle from center to mobile */
 extern void nav_circle_XY(float x, float y, float radius);
 
diff --git a/sw/airborne/tl_control.c b/sw/airborne/tl_control.c
index 82c3bb174f..77109683e9 100644
--- a/sw/airborne/tl_control.c
+++ b/sw/airborne/tl_control.c
@@ -25,9 +25,9 @@ pprz_t tl_control_commands[COMMANDS_NB];
 #define TL_CONTROL_RATE_R_MAX_SP  100.
 
 
-/* float tl_control_attitude_phi_sp; */
-/* float tl_control_attitude_theta_sp; */
-/* float tl_control_attitude_psi_sp; */
+float tl_control_attitude_phi_sp;
+float tl_control_attitude_theta_sp;
+float tl_control_attitude_psi_sp;
 /* float tl_control_attitude_phi_theta_pgain; */
 /* float tl_control_attitude_phi_theta_dgain; */
 /* float tl_control_attitude_psi_pgain; */
diff --git a/sw/airborne/tl_estimator.c b/sw/airborne/tl_estimator.c
index 147b995d33..46fc4f7342 100644
--- a/sw/airborne/tl_estimator.c
+++ b/sw/airborne/tl_estimator.c
@@ -5,6 +5,9 @@
 bool_t estimator_in_flight;
 uint16_t estimator_flight_time;
 
+float tl_estimator_u;
+float tl_estimator_v;
+
 float estimator_x; /* m */
 float estimator_y; /* m */
 float estimator_z; /* altitude in m */
@@ -13,7 +16,11 @@ float estimator_speed; /* m/s */
 float estimator_climb; /* m/s */
 float estimator_course; /* rad, CCW */
 
+float estimator_psi; /* rad, CCW */ 
+
 void tl_estimator_init(void) {
+  tl_estimator_u = 0.;
+  tl_estimator_v = 0.;
 }
 
 void tl_estimator_use_gps(void) {
@@ -29,3 +36,12 @@ void tl_estimator_use_gps(void) {
   estimator_climb = gps_climb / 100.;
   estimator_course = RadOfDeg(gps_course / 10.);
 }
+
+void tl_estimator_to_body_frame(float east, float north,
+				float *front, float *right) {
+  float c = cos(estimator_psi);
+  float s = sin(estimator_psi);
+  
+  *front = c * north + s * east;
+  *right = - s * north + c * east;
+}
diff --git a/sw/airborne/tl_estimator.h b/sw/airborne/tl_estimator.h
index 2066267f1f..7e73b31b51 100644
--- a/sw/airborne/tl_estimator.h
+++ b/sw/airborne/tl_estimator.h
@@ -6,6 +6,9 @@
 extern bool_t estimator_in_flight;
 extern uint16_t estimator_flight_time;
 
+extern float tl_estimator_u;
+extern float tl_estimator_v;
+
 extern float estimator_x; /* m */
 extern float estimator_y; /* m */
 extern float estimator_z; /* altitude in m */
@@ -14,9 +17,13 @@ extern float estimator_speed; /* m/s */
 extern float estimator_climb; /* m/s */
 extern float estimator_course; /* rad, CCW */
 
+extern float estimator_psi; /* rad, CCW */ 
+
 
 void tl_estimator_init(void);
 void tl_estimator_use_gps(void);
 
+void tl_estimator_to_body_frame(float east, float north, float *front, float *right);
+
 
 #endif /* TL_ESTIMATOR_H */
diff --git a/sw/airborne/tl_nav.c b/sw/airborne/tl_nav.c
index ceb4dcc44c..a5d7bd9f08 100644
--- a/sw/airborne/tl_nav.c
+++ b/sw/airborne/tl_nav.c
@@ -5,11 +5,20 @@
 #include "tl_control.h"
 #include "gps.h"
 
-#define NavGotoWaypoint(_wp) {}
+float tl_nav_goto_h_pgain;
+float tl_nav_goto_h_dgain;
+
+#define TL_NAV_GOTO_MAX_PHI_COMMAND   RadOfDeg(30.)
+#define TL_NAV_GOTO_MAX_THETA_COMMAND RadOfDeg(30.)
+
+static void nav_goto_waypoint(uint8_t wp);
+
+#define NavGotoWaypoint(_wp) { nav_goto_waypoint(_wp); }
 #define NavVerticalAutoThrottleMode(_foo) {}
 #define NavVerticalAltitudeMode(_foo, _bar) {}
 
 static void nav_home(void) {
+  kill_throttle = TRUE;
 }
 
 #define NAV_C
@@ -18,6 +27,8 @@ static void nav_home(void) {
 void tl_nav_init(void) {
   nav_block = 0;
   nav_stage = 0;
+  tl_nav_goto_h_pgain = NAV_GOTO_H_PGAIN;
+  tl_nav_goto_h_dgain = NAV_GOTO_H_DGAIN;
 }
 
 void tl_nav_periodic_task(void) {
@@ -29,6 +40,31 @@ void nav_init_stage( void ) {
   stage_time = 0;
 }
 
+static void fly_to_xy(float x_sp, float y_sp) {
+  /* get a position error vector              */
+  float x_error = estimator_x - x_sp;
+  float y_error = estimator_y - y_sp;
 
+  /* Normalize the error */
+  float norm_error = sqrt(x_error*x_error+ y_error*y_error);
+  x_error /= norm_error;
+  y_error /= norm_error;
 
+  /* convert to body frame */
+  float y_unit_err_body, x_unit_err_body;
+  tl_estimator_to_body_frame(x_error, y_error, &x_unit_err_body, &y_unit_err_body);
 
+  /* Compute command */
+  float tl_nav_goto_phi_command = - tl_nav_goto_h_pgain * y_unit_err_body * norm_error +                            - tl_nav_goto_h_dgain *  tl_estimator_v;
+  float tl_nav_goto_theta_command =    tl_nav_goto_h_pgain * x_unit_err_body * norm_error + tl_nav_goto_h_dgain *  tl_estimator_u;
+  
+  Bound(tl_nav_goto_phi_command, -TL_NAV_GOTO_MAX_PHI_COMMAND, TL_NAV_GOTO_MAX_PHI_COMMAND);
+  Bound(tl_nav_goto_theta_command, -TL_NAV_GOTO_MAX_THETA_COMMAND, TL_NAV_GOTO_MAX_THETA_COMMAND);
+
+  tl_control_attitude_phi_sp = tl_nav_goto_phi_command;
+  tl_control_attitude_theta_sp = tl_nav_goto_theta_command;
+}
+
+static void nav_goto_waypoint(uint8_t wp) {
+  fly_to_xy(waypoints[wp].x, waypoints[wp].y);
+}