From 36159537ef14fa2d938ea71c0367147203402a5a Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Sun, 3 Jun 2007 13:55:34 +0000 Subject: [PATCH] new display of GPS levels --- conf/messages.xml | 1 + sw/airborne/ap_downlink.h | 4 +- sw/airborne/sim/sim_gps.c | 10 +++- sw/ground_segment/cockpit/live.ml | 29 ++++++---- sw/ground_segment/cockpit/pages.ml | 82 +++++++++++++++++++---------- sw/ground_segment/cockpit/pages.mli | 2 +- sw/ground_segment/tmtc/aircraft.ml | 21 ++++---- sw/ground_segment/tmtc/aircraft.mli | 17 +++--- sw/ground_segment/tmtc/server.ml | 58 +++++++++++--------- 9 files changed, 139 insertions(+), 85 deletions(-) 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 ->