diff --git a/conf/messages.xml b/conf/messages.xml
index 3a69a1fb30..1e13a74278 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1615,6 +1615,7 @@
     <field name="link_imu_nb_err" type="uint32"/>
     <field name="blmc_nb_err"  type="uint8"/>
     <field name="rc_status"    type="uint8" values="OK|LOST|REALLY_LOST"/>
+    <field name="rc_rate"      type="uint8" unit="Hz"/>
     <field name="gps_status"   type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
     <field name="ap_mode"      type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV"/>
     <field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
diff --git a/sw/ground_segment/tmtc/booz_server.ml b/sw/ground_segment/tmtc/booz_server.ml
index 98187d7f75..a076cb9d27 100644
--- a/sw/ground_segment/tmtc/booz_server.ml
+++ b/sw/ground_segment/tmtc/booz_server.ml
@@ -182,6 +182,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
           Wind.update ac_name a.gspeed a.course
   | "ROTORCRAFT_STATUS" ->
       a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
+      a.fbw.rc_rate   <- ivalue "frame_rate";
       a.gps_mode      <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
       a.ap_mode       <- check_index (get_pprz_mode (ivalue "ap_mode")) ap_modes "BOOZ_AP_MODE";
       a.kill_mode     <- ivalue "ap_motors_on" == 0;
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 1375c9dd28..ce4a4fe2ba 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -103,6 +103,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
     a.last_msg_date <- U.gettimeofday ();
   match msg.Pprz.name with
     "GPS" ->
+      a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
+      if a.gps_mode = _3D then begin
       let p = { LL.utm_x = fvalue "utm_east" /. 100.;
         utm_y = fvalue "utm_north" /. 100.;
         utm_zone = ivalue "utm_zone" } in
@@ -114,9 +116,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
       if !heading_from_course then
         a.heading <- a.course;
       a.agl     <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
-      a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
       if a.gspeed > 3. && a.ap_mode = _AUTO2 then
 	      Wind.update ac_name a.gspeed a.course
+      end
   | "GPS_LLA" ->
       let lat = ivalue "lat"
       and lon = ivalue "lon" in
@@ -143,9 +145,6 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
         Some nav_ref ->
           let x = (try fvalue "x" with _ -> fvalue "desired_x")
           and y = (try fvalue "y" with _ -> fvalue "desired_y") in
-	        (*let target_utm = LL.utm_add (LL.utm_of nav_ref) (x, y) in
-	        let target_geo = LL.of_utm WGS84 target_utm in 
-          a.desired_pos <- target_geo;*)
           a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y);
       | None -> ()
       end;
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 33dd4fd378..a66e209bad 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -151,8 +151,7 @@ let send_cam_status = fun a ->
 	  let alpha = -. a.course in
 	  let east = dx *. cos alpha -. dy *. sin alpha
 	  and north = dx *. sin alpha +. dy *. cos alpha in
-	  let utm = LL.utm_add (LL.utm_of WGS84 a.pos) (east, north) in
-	  let wgs84 = LL.of_utm WGS84 utm in
+    let wgs84 = Aircraft.add_pos_to_nav_ref (Geo a.pos) (east, north) in
 	  let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in
 	  let values = ["ac_id", Pprz.String a.id; 
 			"cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml
index 4b6d968488..0255bca0b1 100644
--- a/sw/ground_segment/tmtc/server_globals.ml
+++ b/sw/ground_segment/tmtc/server_globals.ml
@@ -8,6 +8,7 @@ let _AUTO2 = 2
 let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
 let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
 let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
+let _3D = 3
 let gps_hybrid_modes = [|"OFF";"ON"|]
 let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]