diff --git a/conf/messages.xml b/conf/messages.xml
index 127a0350fa..6a18052f99 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -575,6 +575,7 @@
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 006d4bc986..f04709af16 100644
--- a/sw/airborne/ap_downlink.h
+++ b/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
diff --git a/sw/airborne/sim/sim_gps.c b/sw/airborne/sim/sim_gps.c
index a64d80a315..28bf18adde 100644
--- a/sw/airborne/sim/sim_gps.c
+++ b/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;
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index f3be2bcd06..e9fd646c81 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/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)
diff --git a/sw/ground_segment/cockpit/pages.ml b/sw/ground_segment/cockpit/pages.ml
index 7772063a2e..df14e23c4d 100644
--- a/sw/ground_segment/cockpit/pages.ml
+++ b/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
(*****************************************************************************)
diff --git a/sw/ground_segment/cockpit/pages.mli b/sw/ground_segment/cockpit/pages.mli
index 19a4952508..bd39cb3f8c 100644
--- a/sw/ground_segment/cockpit/pages.mli
+++ b/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 ->
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index 8d00d82ea1..a242a4d3be 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/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
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index deac457ab3..86f48b3800 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/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
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 49aca75d96..bf491459eb 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/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 ->