Merge pull request paparazzi#1082 from paparazzi/optional_timestamp

Improvement of Ivy efficiency

sw/ground_segment/cockpit/gcs.ml

@@ -338,6 +338,7 @@ and display_particules = ref false
 and wid = ref None
 and srtm = ref false
 and hide_fp = ref false
+and timestamp = ref false
 
 let options =
   [
@@ -371,6 +372,7 @@ let options =
     "-wid", Arg.String (fun s -> wid := Some (Int32.of_string s)), "<window id> Id of an existing window to be attached to";
     "-zoom", Arg.Set_float zoom, "Initial zoom";
     "-auto_hide_fp", Arg.Unit (fun () -> Live.auto_hide_fp true; hide_fp := true), "Automatically hide flight plans of unselected aircraft";
+    "-timestamp", Arg.Set timestamp, "Bind on timestampped telemetry messages";
   ]
 
 
@@ -771,7 +773,7 @@ let () =
     begin
       my_alert#add "Waiting for telemetry...";
       Speech.say "Waiting for telemetry...";
-      Live.listen_acs_and_msgs geomap ac_notebook my_alert !auto_center_new_ac alt_graph
+      Live.listen_acs_and_msgs geomap ac_notebook my_alert !auto_center_new_ac alt_graph !timestamp
     end;
 
   (** Display the window *)

sw/ground_segment/cockpit/live.ml

@@ -790,12 +790,12 @@ let alert_bind = fun msg cb ->
     try cb sender vs with _ -> () in
   ignore (Alert_Pprz.message_bind msg safe_cb)
 
-let tele_bind = fun msg cb ->
+let tele_bind = fun msg cb timestamp ->
   let safe_cb = fun sender vs ->
     try cb sender vs with
         AC_not_found -> () (* A/C not yet registed; silently ignore *)
       | x -> fprintf stderr "tele_bind (%s): %s\n%!" msg (Printexc.to_string x) in
-  ignore (Tele_Pprz.message_bind msg safe_cb)
+  ignore (Tele_Pprz.message_bind ~timestamp msg safe_cb)
 
 let ask_config = fun alert geomap fp_notebook ac ->
   let get_config = fun _sender values ->
@@ -1375,33 +1375,33 @@ let mark_dcshot = fun (geomap:G.widget) _sender vs ->
 (*  mark geomap ac.ac_name track !Plugin.frame *)
 
 
-let listen_dcshot = fun _geom ->
-  tele_bind "DC_SHOT" (mark_dcshot _geom)
+let listen_dcshot = fun _geom timestamp ->
+  tele_bind "DC_SHOT" (mark_dcshot _geom) timestamp
 
 let listen_error = fun a ->
   let get_error = fun _sender vs ->
     let msg = Pprz.string_assoc "message" vs in
     log_and_say a "gcs" msg in
   safe_bind "TELEMETRY_ERROR" get_error
 
-let listen_info_msg = fun a ->
+let listen_info_msg = fun a timestamp ->
   let get_msg = fun a _sender vs ->
     let ac = find_ac _sender in
     let msg_array = Pprz.assoc "msg" vs in
     log_and_say a ac.ac_name (Pprz.string_of_value msg_array) in
-  tele_bind "INFO_MSG" (get_msg a)
+  tele_bind "INFO_MSG" (get_msg a) timestamp
 
-let listen_autopilot_version_msg = fun a ->
+let listen_autopilot_version_msg = fun a timestamp ->
   let get_msg = fun a _sender vs ->
     let ac = find_ac _sender in
     let desc_array = Pprz.assoc "desc" vs in
     let version = Pprz.string_of_value desc_array in
     if ac.version <> version then
       log a ac.ac_name (sprintf "%s version:\n%s" ac.ac_name version);
     ac.version <- version in
-  tele_bind "AUTOPILOT_VERSION" (get_msg a)
+  tele_bind "AUTOPILOT_VERSION" (get_msg a) timestamp
 
-let listen_tcas = fun a ->
+let listen_tcas = fun a timestamp ->
   let get_alarm_tcas = fun a txt _sender vs ->
     let ac = find_ac _sender in
     let other_ac = get_ac vs in
@@ -1414,10 +1414,10 @@ let listen_tcas = fun a ->
       with _ -> "" in
     log_and_say a ac.ac_name (sprintf "%s : %s -> %s %s" txt ac.ac_speech_name other_ac.ac_speech_name resolve)
   in
-  tele_bind "TCAS_TA" (get_alarm_tcas a "tcas TA");
-  tele_bind "TCAS_RA" (get_alarm_tcas a "TCAS RA")
+  tele_bind "TCAS_TA" (get_alarm_tcas a "tcas TA") timestamp;
+  tele_bind "TCAS_RA" (get_alarm_tcas a "TCAS RA") timestamp
 
-let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt_graph ->
+let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt_graph timestamp ->
   (** Probe live A/Cs *)
   let probe = fun () ->
     message_request "gcs" "AIRCRAFTS" [] (fun _sender vs -> aircrafts_msg my_alert geomap ac_notebook vs) in
@@ -1437,10 +1437,10 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt
   listen_svsinfo my_alert;
   listen_alert my_alert;
   listen_error my_alert;
-  listen_info_msg my_alert;
-  listen_autopilot_version_msg my_alert;
-  listen_tcas my_alert;
-  listen_dcshot geomap;
+  listen_info_msg my_alert timestamp;
+  listen_autopilot_version_msg my_alert timestamp;
+  listen_tcas my_alert timestamp;
+  listen_dcshot geomap timestamp;
 
   (** Select the active aircraft on notebook page selection *)
   let callback = fun i ->

sw/ground_segment/cockpit/live.mli

@@ -75,8 +75,8 @@ val track_size : int ref
 val auto_hide_fp : bool -> unit
 (** Automatically hide flight plan of not selected ac *)
 
-val listen_acs_and_msgs : MapCanvas.widget -> GPack.notebook -> Pages.alert -> bool -> Gtk_tools.pixmap_in_drawin_area -> unit
-(** [listen_acs_and_msgs geomap aircraft_notebook alert_page auto_center_new_ac alt_graph] *)
+val listen_acs_and_msgs : MapCanvas.widget -> GPack.notebook -> Pages.alert -> bool -> Gtk_tools.pixmap_in_drawin_area -> bool -> unit
+(** [listen_acs_and_msgs geomap aircraft_notebook alert_page auto_center_new_ac alt_graph timestamp] *)
 
 val jump_to_block : string -> int -> unit
 (** [jump_to_block ac_id block_id] Sends a JUMP_TO_BLOCK message *)

sw/ground_segment/tmtc/messages.ml

@@ -170,7 +170,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no
   in
   bind id display
 
-let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) (ident, xml_class, sender) ->
+let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) timestamp force (ident, xml_class, sender) ->
   let class_name = (Xml.attrib xml_class "name") in
   let messages = Xml.children xml_class in
   let module P = Pprz.Messages (struct let name = class_name end) in
@@ -181,20 +181,21 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind
       let get_one = fun sender _vs ->
         if not (Hashtbl.mem senders sender) then begin
           Hashtbl.add senders sender ();
-          one_class notebook help_label window (ident,  xml_class, Some sender)
+          one_class notebook help_label window timestamp force (ident,  xml_class, Some sender)
         end in
-      List.iter
-        (fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one))
-        messages
+      if force || not (class_name = "telemetry") then (* bind to all messages in class *)
+        List.iter (fun m -> ignore (P.message_bind ~timestamp (Xml.attrib m "name") get_one)) messages
+      else (* if telemetry and not forces, only wait for ALIVE message *)
+        ignore (P.message_bind ~timestamp "ALIVE" get_one)
     | _ ->
       let class_notebook = GPack.notebook ~tab_border:0 ~tab_pos:`LEFT () in
       let l = match sender with None -> "" | Some s -> ":"^s in
       let label = GMisc.label ~text:(ident^l) () in
       ignore (notebook#append_page ~tab_label:label#coerce class_notebook#coerce);
       let bind, sender_name = match sender with
-          None -> (fun m cb -> (P.message_bind m cb)), "*"
-        | Some sender -> (fun m cb -> (P.message_bind ~sender m cb)), sender in
-
+          None -> (fun m cb -> (P.message_bind ~timestamp m cb)), "*"
+        | Some sender -> (fun m cb -> (P.message_bind ~sender ~timestamp m cb)), sender in
+      
       (** Forall messages in the class *)
       let messages = list_sort (fun x -> Xml.attrib x "name") messages in
       List.iter (fun m -> ignore (one_page sender_name class_name class_notebook notebook help_label window bind m)) messages
@@ -206,9 +207,13 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind
 let _ =
   let ivy_bus = ref Defivybus.default_ivy_bus in
   let classes = ref ["telemetry:*"] in
+  let timestamp = ref false in
+  let force = ref false in
   Arg.parse
     [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf "<ivy bus> Default is %s" !ivy_bus);
-      "-c",  Arg.String (fun x -> classes := x :: !classes), "class name"]
+      "-c",  Arg.String (fun x -> classes := x :: !classes), "class name";
+      "-timestamp", Arg.Set timestamp, "Bind to timestampped messages";
+      "-force", Arg.Set force, "Force waiting on all messages, not only ALIVE for telemetry class (increase network load)" ]
     (fun x -> prerr_endline ("WARNING: don't do anything with "^x))
     "Usage: ";
 
@@ -243,7 +248,7 @@ let _ =
       !classes in
 
   (* Insert the message classes in the notebook *)
-  List.iter (one_class notebook help_label#coerce window) xml_classes;
+  List.iter (one_class notebook help_label#coerce window !timestamp !force) xml_classes;
 
   (** Start the main loop *)
   window#show ();

sw/ground_segment/tmtc/server.ml

@@ -622,14 +622,17 @@ let register_aircraft = fun name a ->
 
 
 (** Identifying message from an A/C *)
-let ident_msg = fun log name vs ->
+let ident_msg = fun log timestamp name vs ->
   try
     if not (Hashtbl.mem aircrafts name) &&
       not (Hashtbl.mem unknown_aircrafts name) then
       let get_md5sum = fun () -> Pprz.assoc "md5sum" vs in
       let ac, messages_xml = new_aircraft get_md5sum name in
       let ac_msg_closure = ac_msg messages_xml log name ac in
-      let _b = Ivy.bind (fun _ args -> ac_msg_closure args.(1) args.(2)) (sprintf "^(([0-9]+\\.[0-9]+) )?%s +(.*)" name) in
+      let tsregexp = if timestamp then "(([0-9]+\\.[0-9]+) )?" else "" in
+      let _b =
+        Ivy.bind (fun _ args -> if timestamp then ac_msg_closure args.(1) args.(2) else ac_msg_closure "" args.(0))
+        (sprintf "^%s%s +(.*)" tsregexp name) in
       register_aircraft name ac;
       Ground_Pprz.message_send my_id "NEW_AIRCRAFT" ["ac_id", Pprz.String name]
   with
@@ -639,11 +642,11 @@ let new_color = fun () ->
   sprintf "#%02x%02x%02x" (Random.int 256) (Random.int 256) (Random.int 256)
 
 (* Waits for new aircrafts *)
-let listen_acs = fun log ->
+let listen_acs = fun log timestamp ->
   (** Wait for any message (they all are identified with the A/C) *)
-  ignore (Tm_Pprz.message_bind "ALIVE" (ident_msg log));
+  ignore (Tm_Pprz.message_bind "ALIVE" (ident_msg log timestamp));
   if !replay_old_log then
-    ignore (Tm_Pprz.message_bind "PPRZ_MODE" (ident_msg log))
+    ignore (Tm_Pprz.message_bind "PPRZ_MODE" (ident_msg log timestamp))
 
 
 let send_config = fun http _asker args ->
@@ -785,7 +788,8 @@ let ground_to_uplink = fun logging ->
 let () =
   let ivy_bus = ref Defivybus.default_ivy_bus
   and logging = ref true
-  and http = ref false in
+  and http = ref false
+  and timestamp = ref false in
 
   let options =
     [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf "Bus\tDefault is %s" !ivy_bus);
@@ -795,6 +799,7 @@ let () =
       "-kml_no_http", Arg.Set Kml.no_http, "KML without web server (local files only)";
       "-kml_port", Arg.Set_int Kml.port, (sprintf "Port for KML files (default is %d)" !Kml.port);
       "-n", Arg.Clear logging, "Disable log";
+      "-timestamp", Arg.Set timestamp, "Bind on timestampped messages";
       "-no_md5_check", Arg.Set no_md5_check, "Disable safety matching of live and current configurations";
       "-replay_old_log", Arg.Set replay_old_log, "Enable aircraft registering on PPRZ_MODE messages"] in
 
@@ -816,7 +821,7 @@ let () =
       None in
 
   (* Waits for new aircrafts *)
-  listen_acs logging;
+  listen_acs logging !timestamp;
 
   (* Forward messages from ground agents to vehicles *)
   ground_to_uplink logging;

sw/lib/ocaml/pprz.ml

@@ -613,7 +613,7 @@ module type MESSAGES = sig
   val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit
   (** [message_send sender link_id msg_name values] *)
 
-  val message_bind : ?sender:string -> string -> (string -> values -> unit) -> Ivy.binding
+  val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding
   (** [message_bind ?sender msg_name callback] *)
 
   val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding
@@ -764,20 +764,21 @@ module MessagesOfXml(Class:CLASS_Xml) = struct
         Ivy.send ( Printf.sprintf "redlink TELEMETRY_MESSAGE %s %i %s" sender the_link_id modified_msg);
       end
 
-  let message_bind = fun ?sender msg_name cb ->
+  let message_bind = fun ?sender ?(timestamp=false) msg_name cb ->
+    let tsregexp, tsoffset = if timestamp then "([0-9]+\\.[0-9]+ )?", 1 else "", 0 in
     match sender with
         None ->
           Ivy.bind
             (fun _ args ->
-              let values = try snd (values_of_string args.(2)) with exc -> prerr_endline (Printexc.to_string exc); [] in
-              cb args.(1) values)
-            (sprintf "^([0-9]+\\.[0-9]+ )?([^ ]*) +(%s( .*|$))" msg_name)
+              let values = try snd (values_of_string args.(1+tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in
+              cb args.(tsoffset) values)
+            (sprintf "^%s([^ ]*) +(%s( .*|$))" tsregexp msg_name)
       | Some s ->
         Ivy.bind
           (fun _ args ->
-            let values = try snd (values_of_string args.(1)) with  exc -> prerr_endline (Printexc.to_string exc); [] in
+            let values = try snd (values_of_string args.(tsoffset)) with  exc -> prerr_endline (Printexc.to_string exc); [] in
             cb s values)
-          (sprintf "^([0-9]+\\.[0-9]+ )?%s +(%s( .*|$))" s msg_name)
+          (sprintf "^%s%s +(%s( .*|$))" tsregexp s msg_name)
 
   let message_answerer = fun sender msg_name cb ->
     let ivy_cb = fun _ args ->

sw/lib/ocaml/pprz.mli

@@ -179,7 +179,7 @@ module type MESSAGES = sig
   val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit
   (** [message_send sender msg_name values] *)
 
-  val message_bind : ?sender:string ->string -> (string -> values -> unit) -> Ivy.binding
+  val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding
   (** [message_bind ?sender msg_name callback] *)
 
   val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding