diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index 4e24166969..254618c18f 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -105,7 +105,6 @@ float nav_glide_pitch_trim;
 
 static bool_t approaching(uint8_t, float) __attribute__ ((unused));
 static inline void fly_to_xy(float x, float y);
-static void fly_to(uint8_t wp) __attribute__ ((unused));
 static void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);
 
 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
@@ -153,10 +152,16 @@ static inline void nav_circle_XY(float x, float y, float radius) {
   nav_circle_radius = radius;
 }
 
-#define NavCircle(wp, radius) \
+
+#define NavGotoWaypoint(_wp) { \
+  horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
+  fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
+}
+
+#define NavCircleWaypoint(wp, radius) \
   nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)
 
-#define NavRoute(_start, _end) \
+#define NavSegment(_start, _end) \
   nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y)
 
 #define NavSurveyRectangleInit(_wp1, _wp2, _grid) nav_survey_rectangle_init(_wp1, _wp2, _grid)
@@ -305,7 +310,7 @@ static inline void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
     }
   } else { /* U-turn */
     if (NavCircleCount() < 0.45) {
-      NavCircle(0, survey_radius);
+      NavCircleWaypoint(0, survey_radius);
     } else {
       /* U-turn finished, back on a segment */
       survey_uturn = FALSE;
@@ -375,9 +380,6 @@ static unit_t nav_update_waypoints_alt( void ) {
 
 
 
-#define MIN_DIST2_WP (15.*15.)
-
-
 int32_t nav_utm_east0 = NAV_UTM_EAST0;
 int32_t nav_utm_north0 = NAV_UTM_NORTH0;
 uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
@@ -432,13 +434,6 @@ static inline void fly_to_xy(float x, float y) {
   h_ctl_course_setpoint = M_PI/2.-atan2(y - estimator_y, x - estimator_x);
 }
 
-/** static void fly_to(uint8_t wp)
- *  \brief Just call \a fly_to_xy with x and y of current waypoint.
- */
-static void fly_to(uint8_t wp) { 
-  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
-  fly_to_xy(waypoints[wp].x, waypoints[wp].y);
-}
 
 static void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
   float leg_x = wp_x - last_wp_x;
@@ -484,7 +479,7 @@ static inline void compute_dist2_to_home(void) {
 /** \brief Occurs when it switchs in Home mode.
  */
 void nav_home(void) {
-  NavCircle(WP_HOME, FAILSAFE_HOME_RADIUS);
+  NavCircleWaypoint(WP_HOME, FAILSAFE_HOME_RADIUS);
   /** Nominal speed */ 
   nav_pitch = 0.;
   v_ctl_mode = V_CTL_MODE_AUTO_ALT;
@@ -610,7 +605,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
   
   switch (eight_status) {
   case C1 :
-    NavCircle(c1, radius);
+    NavCircleWaypoint(c1, radius);
     if (NavQdrCloseTo(DegOfRad(qdr_out)-10)) {
       eight_status = R12;
       InitStage();
diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h
index f5c412add7..9cd4faab68 100644
--- a/sw/airborne/nav.h
+++ b/sw/airborne/nav.h
@@ -45,6 +45,10 @@ struct point {
   float a;
 };
 
+#define WaypointX(_wp) (waypoints[_wp].x)
+#define WaypointY(_wp) (waypoints[_wp].y)
+#define WaypointAlt(_wp) (waypoints[_wp].a)
+
 extern float cur_pos_x;
 extern float cur_pos_y;
 extern uint8_t nav_stage, nav_block;
@@ -57,8 +61,6 @@ extern uint8_t nav_utm_zone0;
 extern const uint8_t nb_waypoint;
 extern struct point waypoints[]; /** size == nb_waypoint + 1 */
 
-#define WaypointAlt(_wp) (waypoints[_wp].a)
-
 extern float desired_x, desired_y, altitude_shift, nav_altitude, flight_altitude, nav_glide_pitch_trim;
 
 extern pprz_t nav_throttle_setpoint;
diff --git a/sw/simulator/gps.ml b/sw/simulator/gps.ml
index 68e3a2e0d7..5706a5a032 100644
--- a/sw/simulator/gps.ml
+++ b/sw/simulator/gps.ml
@@ -38,23 +38,10 @@ type state = {
   }
 
 
-let earth_radius = 6378388.
-
-let lat0 = ref 0.
-let lon0 = ref 0. 
-let alt0 = ref 0.
-
-let set_ref = fun wgs84 alt ->
-  lat0 := wgs84.posn_lat;
-  lon0 := wgs84.posn_long;
-  alt0 := alt
-
 let climb_noise = fun c -> c +. Random.float 1.
 
   
-
-
-let state = fun () ->
+let state = fun pos0 alt0 ->
   let last_x = ref 0. and last_y = ref 0. 
   and last_t = ref 0. and last_z = ref 0. in
 
@@ -66,9 +53,9 @@ let state = fun () ->
     and course = norm_angle (pi/.2. -. atan2 dy dx)
     and climb = (z -. !last_z) /. dt in
 
-    (** FIXME, should be utm -> geo *)
-    let lat = !lat0 +. y /. earth_radius
-    and long = !lon0 +. x /.earth_radius /. cos !lat0
+    let utm0 = utm_of WGS84 !pos0 in
+    let utm = utm_add utm0 (x, y) in
+    let wgs84 = of_utm WGS84 utm
     and alt = !alt0 +. z in
 
     last_x := x;
@@ -76,10 +63,10 @@ let state = fun () ->
     last_z := z;
     last_t := t;
 
-    let course = if course < 0. then course +. 2. *. pi else course in (* ???? *)
+    let course = if course < 0. then course +. 2. *. pi else course in
     
     {
-     wgs84 = { posn_lat=lat;posn_long=long };
+     wgs84 = wgs84;
      alt = alt;
      time = t;
      climb = climb_noise climb;
diff --git a/sw/simulator/gps.mli b/sw/simulator/gps.mli
index d927c2c419..1152478dcc 100644
--- a/sw/simulator/gps.mli
+++ b/sw/simulator/gps.mli
@@ -33,8 +33,6 @@ type state = {
     gspeed : float;
     course : float; (** North = 0 *)
   }
-val state : unit -> (float * float * float -> float -> state)
-(** [state ()] Returns a function which must be called with
+val state : Latlong.geographic ref -> float ref -> (float * float * float -> float -> state)
+(** [state pos0 alt0] Returns a function which must be called with
 an updated position [xyz] and time [t] *)
-
-val set_ref : Latlong.geographic -> float -> unit
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 8619eb1abf..6e3ef815e2 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -107,7 +107,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
     let gps_period = 0.25 in
     
-    let compute_gps_state = Gps.state () in
+    let compute_gps_state = Gps.state pos0 alt0 in
 
     let initial_state = FM.init (pi/.2. -. qfu/.180.*.pi) in
 
@@ -203,7 +203,6 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     in
 
     let boot = fun () ->
-      Gps.set_ref !pos0 !alt0;
       Aircraft.boot (time_scale:>value);
       Stdlib.timer ~scale:time_scale fm_period fm_task;
       Stdlib.timer ~scale:time_scale ir_period ir_task;
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index aa6dbc0ad4..69f968f051 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -206,13 +206,13 @@ let output_hmode x wp last_wp =
 	"route" ->
 	  if last_wp = "last_wp" then
 	    fprintf stderr "WARNING: Deprecated use of 'route' using last waypoint in %s\n"(Xml.to_string x);
-	  lprintf "NavRoute(%s, %s);\n" last_wp wp
-      | "direct" -> lprintf "fly_to(%s);\n" wp
+	  lprintf "NavSegment(%s, %s);\n" last_wp wp
+      | "direct" -> lprintf "NavGotoWaypoint(%s);\n" wp
       | x -> failwith (sprintf "Unknown hmode '%s'" x)
     end;
     hmode
   with
-    ExtXml.Error _ -> lprintf "fly_to(%s);\n" wp; "direct" (* Default behaviour *)
+    ExtXml.Error _ -> lprintf "NavGotoWaypoint(%s);\n" wp; "direct" (* Default behaviour *)
 	  
 
 
@@ -360,11 +360,11 @@ let rec print_stage = fun index_of_waypoints x ->
 	begin
 	  try
 	    let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in
-	    ignore(output_hmode x wp "");
+	    ignore (output_hmode x wp "");
 	    ignore (output_vmode x wp "");
 	  with
 	    Xml2h.Error _ ->
-	      lprintf "fly_to_xy(last_x, last_y);\n";
+	      lprintf "NavGotoXY(last_x, last_y);\n";
 		ignore(output_vmode x "" "")
 	end;
 	lprintf "return;\n"
@@ -381,7 +381,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in
 	let r = parsed_attrib  x "radius" in
 	let _vmode = output_vmode x wp "" in
-	lprintf "NavCircle(%s, %s);\n" wp r;
+	lprintf "NavCircleWaypoint(%s, %s);\n" wp r;
 	begin
 	  try
 	    let c = parsed_attrib x "until" in