new display of GPS levels

conf/messages.xml

@@ -575,6 +575,7 @@
       <field name="cno"   type="string" format="csv" unit="dbHz"/>
       <field name="elev"  type="string" format="csv" unit="deg"/>
       <field name="azim"  type="string" format="csv" unit="deg"/>
+      <field name="msg_age"  type="string" format="csv" unit="s"/>
   </message>
 
   <message name="FLY_BY_WIRE" id="17">

sw/airborne/ap_downlink.h

@@ -171,10 +171,8 @@
 
 #define PERIODIC_SEND_TUNE_ROLL() DOWNLINK_SEND_TUNE_ROLL(&estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
 
-#ifdef GPS
+#if defined GPS || defined SITL
 #define PERIODIC_SEND_GPS_SOL() DOWNLINK_SEND_GPS_SOL(&gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV)
-#elif SITL
-#define PERIODIC_SEND_GPS_SOL() {}
 #endif
 
 

sw/airborne/sim/sim_gps.c

@@ -23,6 +23,10 @@ uint8_t gps_utm_zone;
 int32_t gps_lat, gps_lon;
 struct svinfo gps_svinfos[GPS_NB_CHANNELS];
 uint8_t gps_nb_channels = 0;
+uint16_t gps_PDOP;
+uint32_t gps_Pacc, gps_Sacc;
+uint8_t gps_numSV;
+
 
 value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) {
   gps_mode = (Bool_val(m) ? 3 : 0);
@@ -46,10 +50,12 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
     gps_svinfos[i].svid = 7 + i;
     gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
     gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360;
-    gps_svinfos[i].cno = 40 + sin(time/100.) * 10.;
-    gps_svinfos[i].flags = 0x01;
+    gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
+    gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
     gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
   }
+  gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.);
+  gps_numSV = 7;
       
   use_gps_pos(); /* From main.c */
   return Val_unit;

sw/ground_segment/cockpit/live.ml

@@ -836,21 +836,30 @@ let get_infrared = fun _sender vs ->
     
 let listen_infrared = fun () -> safe_bind "INFRARED" get_infrared
 
-let get_svsinfo = fun a _sender vs ->
+let get_svsinfo = fun alarm _sender vs ->
   let ac = get_ac vs in
   let gps_page = ac.gps_page in
-  let svid = Str.split list_separator (Pprz.string_assoc "svid" vs)
-  and cn0 = Str.split list_separator (Pprz.string_assoc "cno" vs)
-  and flags = Str.split list_separator (Pprz.string_assoc "flags" vs) in 
-  
-  list_iter3
-    (fun id cno flags ->
-      if id <> "0" then gps_page#svsinfo id cno (int_of_string flags))
-    svid cn0 flags;
+  let svids = Str.split list_separator (Pprz.string_assoc "svid" vs)
+  and cn0s = Str.split list_separator (Pprz.string_assoc "cno" vs)
+  and flagss = Str.split list_separator (Pprz.string_assoc "flags" vs)
+  and ages = Str.split list_separator (Pprz.string_assoc "msg_age" vs) in 
+
+  let a = Array.create (List.length svids) (0,0,0,0) in
+  let rec loop = fun i s c f ages ->
+    match (s, c, f, ages) with
+      [], [], [], [] -> ()
+    | s::ss, c::cs, f::fs, age::ages ->
+	a.(i) <- (int_of_string s, int_of_string c, int_of_string f, int_of_string age);
+	loop (i+1) ss cs fs ages
+    | _ -> assert false in
+  loop 0 svids cn0s flagss ages;
 
   let pacc = Pprz.int_assoc "pacc" vs in
+  
+  gps_page#svsinfo pacc a;
+
   if pacc > 1500 && pacc < 9999 then
-    log_and_say a "gcs" (sprintf "GPS acc: %d m" (pacc / 100))
+    log_and_say alarm "gcs" (sprintf "GPS acc: %d m" (pacc / 100))
     
 let listen_svsinfo = fun a -> safe_bind "SVSINFO" (get_svsinfo a)
 

sw/ground_segment/cockpit/pages.ml

@@ -111,39 +111,65 @@ end
 (*****************************************************************************)
 class gps ?(visible = fun _ -> true) (widget: GBin.frame) =
   let sw = GBin.scrolled_window ~hpolicy:`AUTOMATIC ~vpolicy:`AUTOMATIC ~packing:widget#add () in
-  let table = GPack.table
-      ~rows: 1
-      ~columns: 3
-      ~row_spacings: 5
-      ~col_spacings: 40
-      ~packing:sw#add_with_viewport
-      () in
-  let update_color = fun flags_eb flags ->
-    let color = if flags land 0x01 = 1 then "green" else "red" in
-    flags_eb#coerce#misc#modify_bg [`NORMAL, `NAME color] in
+
+  let da = GMisc.drawing_area ~show:true ~packing:sw#add () in
 
 object
   val mutable active_cno = []
   val mutable active_flags = []
 
-  method svsinfo svid cno flags =
-    if not (List.mem_assoc svid active_cno) then
-      let rows = table#rows in
-      let _svid_label = GMisc.label 
-	  ~text: ("sat "^ svid) ~packing: (table#attach ~top: rows ~left: 0) () in
-      let cno_label = GMisc.label 
-	  ~text:(cno^" dB Hz") ~packing: (table#attach ~top: rows ~left: 1) () in
-      let flags_eb = GBin.event_box ~width: 20 ~packing:(table#attach ~top: rows ~left: 2) ()
-      in
-      update_color flags_eb flags;
-      active_cno <- (svid, cno_label)::active_cno;
-      active_flags <- (svid, flags_eb)::active_flags;
-      table#set_rows (table#rows +1)
-    else if visible widget then
-      let cno_label = List.assoc svid active_cno in
-      let flags_eb = List.assoc svid active_flags in
-      cno_label#set_label (cno^" dB Hz");
-      update_color flags_eb flags
+  method svsinfo pacc a =
+    if visible widget then
+      let {Gtk.width=width; height=height} = da#misc#allocation in
+      
+      (* Background *)
+      let dr = GDraw.pixmap ~width ~height ~window:da () in
+      dr#set_foreground (`NAME "white");
+      dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
+      
+      let context = da#misc#create_pango_context in
+      context#set_font_by_name ("sans " ^ string_of_int 10);
+      let layout = context#create_layout in
+      
+      let n = Array.length a in
+      let sep_size = 3 in
+      let indic_size = min 25 ((width-(n+1)*sep_size)/n) in
+      let max_cn0 = 50 in
+
+      Pango.Layout.set_text layout "Dummy";
+      let (_, h) = Pango.Layout.get_pixel_size layout in
+
+      let size = fun cn0 -> (max 20 cn0 - 20) * 2 in
+
+      let y = sep_size + h + (size max_cn0) in
+      for i = 0 to n - 1 do
+	let (id, cn0, flags, age) = a.(i) in
+	let x = sep_size + i * (sep_size+indic_size) in
+
+	(* level *)
+	Pango.Layout.set_text layout (sprintf "% 2d" cn0);
+	dr#put_layout ~x ~y:0 ~fore:`BLACK layout;
+
+	(* bar *)
+	let color = if age > 5 then "grey" else if flags land 0x01 = 1 then "green" else "red" in
+	dr#set_foreground (`NAME color);
+	let height = size cn0 in
+	dr#rectangle ~filled:true ~x ~y:(y-height) ~width:indic_size ~height ();
+	(* SV id *)
+	Pango.Layout.set_text layout (sprintf "% 2d" id);
+	dr#put_layout ~x ~y ~fore:`BLACK layout
+      done;
+
+      (* Pacc *)
+      let max_pacc = 2000 in
+      dr#set_foreground (`NAME "red");
+      let w = min width ((pacc*width)/max_pacc) in
+      dr#rectangle ~filled:true ~x:0 ~y:(y+h) ~width:w ~height:h ();
+      Pango.Layout.set_text layout (sprintf "Pos accuracy: %.1fm" (float pacc/.100.));
+      let (_, h) = Pango.Layout.get_pixel_size layout in
+      dr#put_layout ~x:((width-w)/2) ~y:(y+h) ~fore:`BLACK layout;
+      
+      (new GDraw.drawable da#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
 end
 
 (*****************************************************************************)

sw/ground_segment/cockpit/pages.mli

@@ -13,7 +13,7 @@ class infrared : GBin.frame ->
 
 class gps : ?visible:(GBin.frame -> bool) -> GBin.frame ->
   object
-    method svsinfo : string -> string -> int -> unit
+    method svsinfo : int -> (int*int*int*int) array -> unit
   end
 
 class pfd : ?visible:(GBin.frame -> bool) -> GBin.frame ->

sw/ground_segment/tmtc/aircraft.ml

@@ -56,17 +56,20 @@ type svinfo = {
     qi : int;
     cno : int;
     elev : int;
-    azim : int
+    azim : int;
+    mutable age : int
   }
 
-let svinfo_init = {  
-    svid = 0 ;
-    flags = 0;
-    qi = 0;
-    cno = 0;
-    elev = 0;
-    azim = 0;
-  }
+let svinfo_init = fun () ->
+  {  
+     svid = 0 ;
+     flags = 0;
+     qi = 0;
+     cno = 0;
+     elev = 0;
+     azim = 0;
+     age = 0
+   }
 
 type horiz_mode = 
     Circle of Latlong.utm * int

sw/ground_segment/tmtc/aircraft.mli

@@ -47,14 +47,15 @@ type rc_mode = string
 type fbw = { mutable rc_status : rc_status; mutable rc_mode : rc_mode; }
 val gps_nb_channels : int
 type svinfo = {
-  svid : int;
-  flags : int;
-  qi : int;
-  cno : int;
-  elev : int;
-  azim : int;
-}
-val svinfo_init : svinfo
+    svid : int;
+    flags : int;
+    qi : int;
+    cno : int;
+    elev : int;
+    azim : int;
+    mutable age : int
+  }
+val svinfo_init : unit -> svinfo
 type horiz_mode =
     Circle of Latlong.utm * int
   | Segment of Latlong.utm * Latlong.utm

sw/ground_segment/tmtc/server.ml

@@ -276,6 +276,7 @@ let log_and_parse = fun logging ac_name (a:Aircraft.aircraft) msg values ->
 	cno = ivalue "CNO";
 	elev = ivalue "Elev";
 	azim = ivalue "Azim";
+	age = 0
       }
   | "CIRCLE" ->
       begin
@@ -404,7 +405,8 @@ let send_svsinfo = fun a ->
   and qi = ref ""
   and cno = ref ""
   and elev = ref ""
-  and azim = ref "" in
+  and azim = ref ""
+  and age = ref "" in
   let concat = fun ref v ->
     ref := !ref ^ string_of_int v ^ "," in
   for i = 0 to gps_nb_channels - 1 do
@@ -413,12 +415,13 @@ let send_svsinfo = fun a ->
     concat qi a.svinfo.(i).qi;
     concat cno a.svinfo.(i).cno;
     concat elev a.svinfo.(i).elev;
-    concat azim a.svinfo.(i).azim
+    concat azim a.svinfo.(i).azim;
+    concat age a.svinfo.(i).age
   done;
   let f = fun s r -> (s, Pprz.String !r) in
   let vs = ["ac_id", Pprz.String a.id; 
 	    "pacc", Pprz.Int a.gps_Pacc;
-	    f "svid" svid; f "flags" flags; f "qi" qi; 
+	    f "svid" svid; f "flags" flags; f "qi" qi;  f "msg_age" age; 
 	    f "cno" cno; f "elev" elev; f "azim" azim] in
   Ground_Pprz.message_send my_id "SVSINFO" vs
 
@@ -574,28 +577,35 @@ let send_aircraft_msg = fun ac ->
 let new_aircraft = fun id ->
   let infrared_init = { gps_hybrid_mode = 0; gps_hybrid_factor = 0. ;
 			contrast_status = "DEFAULT"; contrast_value = 0} in
-    { id = id ; roll = 0.; pitch = 0.; desired_east = 0.; desired_north = 0.; 
-      desired_course = 0.;
-	gspeed=0.; course = 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 = [];
-      desired_altitude = 0.;
-      desired_climb = 0.;
-      pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
-      nav_ref = None;
-      cam = { phi = 0.; theta = 0. ; target=(0.,0.)};
-      inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
-      infrared = infrared_init; kill_mode = false;
-      fbw = { rc_status = "???"; rc_mode = "???" };
-      svinfo = Array.create gps_nb_channels svinfo_init;
-      dl_setting_values = Array.create max_nb_dl_setting_values 42.;
+  let svsinfo_init = Array.init gps_nb_channels (fun _ -> svinfo_init ()) in
+  let update = fun () ->
+    for i = 0 to Array.length svsinfo_init - 1 do
+      svsinfo_init.(i).age <-  svsinfo_init.(i).age + 1;
+    done in
+
+  ignore (Glib.Timeout.add 1000 (fun _ -> update (); true));
+  { id = id ; roll = 0.; pitch = 0.; desired_east = 0.; desired_north = 0.; 
+    desired_course = 0.;
+    gspeed=0.; course = 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 = [];
+    desired_altitude = 0.;
+    desired_climb = 0.;
+    pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
+    nav_ref = None;
+    cam = { phi = 0.; theta = 0. ; target=(0.,0.)};
+    inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
+    infrared = infrared_init; kill_mode = false;
+    fbw = { rc_status = "???"; rc_mode = "???" };
+    svinfo = svsinfo_init;
+    dl_setting_values = Array.create max_nb_dl_setting_values 42.;
       nb_dl_setting_values = 0;
-      flight_time = 0; stage_time = 0; block_time = 0;
-      horiz_mode = UnknownHorizMode;
-      horizontal_mode = 0;
-      waypoints = Hashtbl.create 3; survey = None; last_bat_msg_date = 0.
-    }
+    flight_time = 0; stage_time = 0; block_time = 0;
+    horiz_mode = UnknownHorizMode;
+    horizontal_mode = 0;
+    waypoints = Hashtbl.create 3; survey = None; last_bat_msg_date = 0.
+  }
 
 let check_alerts = fun a ->
   let send = fun level ->