diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index e079d33fc3..8716a8b632 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/sw/ground_segment/tmtc/aircraft.ml
@@ -82,6 +82,7 @@ type aircraft = {
     mutable itow : int32; (* ms *)
     mutable roll    : float;
     mutable pitch   : float;
+    mutable heading  : float; (* rad, CW 0=N *)
     mutable nav_ref    : Latlong.utm option;
     mutable desired_east    : float;
     mutable desired_north    : float;
@@ -89,7 +90,7 @@ type aircraft = {
     mutable desired_course : float;
     mutable desired_climb : float;
     mutable gspeed  : float; (* m/s *)
-    mutable course  : float; (* rad *)
+    mutable course : float; (* rad *)
     mutable alt     : float;
     mutable agl     : float;
     mutable climb   : float;
@@ -133,7 +134,7 @@ let new_aircraft = fun id name fp airframe ->
   let svsinfo_init = Array.init gps_nb_channels (fun _ -> svinfo_init ()) in
   { id = id ; name = name; roll = 0.; pitch = 0.; desired_east = 0.; desired_north = 0.; flight_plan = fp; airframe = airframe; dist_to_wp = 0.;
     desired_course = 0.;
-    gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0;
+    gspeed=0.; course = 0.; heading = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0;
     throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 42.; amp = 0.; energy = 0; ap_mode= -1; agl = 0.;
     gaz_mode= -1; lateral_mode= -1;
     gps_mode =0; gps_Pacc = 0; periodic_callbacks = [];
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index 6b011d8440..cb8b4d6dfa 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/sw/ground_segment/tmtc/aircraft.mli
@@ -68,6 +68,7 @@ type aircraft = {
     mutable itow : int32;
     mutable roll : float;
     mutable pitch : float;
+    mutable heading : float; (* rad *)
     mutable nav_ref : Latlong.utm option;
     mutable desired_east : float;
     mutable desired_north : float;
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 1faba3bfbc..de8d1b4ef4 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -105,7 +105,6 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
 		 utm_zone = ivalue "utm_zone" };
       a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
       a.itow <- Int32.of_float (fvalue "itow");
-      (*Printf.fprintf stderr "itow %lu %ld\n" a.itow a.itow;*)
       a.gspeed  <- fvalue "speed" /. 100.;
       a.course  <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
       a.agl     <- a.alt -. float (try Srtm.of_utm a.pos with _ -> 0);
@@ -127,8 +126,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
   | "NAVIGATION_REF" ->
       a.nav_ref <- Some { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = ivalue "utm_zone" }
   | "ATTITUDE" ->
-      a.roll <- (Deg>>Rad) (fvalue "phi");
-      a.pitch <- (Deg>>Rad) (fvalue "theta")
+      a.heading  <- norm_course (fvalue "psi");
+      a.roll <- fvalue "phi";
+      a.pitch <- fvalue "theta"
   | "NAVIGATION" -> 
       a.cur_block <- ivalue "cur_block";
       a.cur_stage <- ivalue "cur_stage";