diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 75df606457..e22c978b20 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -561,38 +561,47 @@ let listen_acs = fun log -> (** Wait for any message (they all are identified with the A/C) *) ignore (Ivy.bind (fun _ args -> ident_msg log args.(0)) "^(.*) PPRZ_MODE") +(** c.f. sw/logalizer/play.ml *) +let replayed = fun s -> + let n = String.length s in + if n > 6 && String.sub s 0 6 = "replay" then + Some (String.sub s 6 (n - 6)) + else + None + let send_config = fun http _asker args -> - match args with - ["ac_id", Pprz.String ac_id] -> begin - try - let conf = ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "ac_id" = ac_id) in - let ac_name = ExtXml.attrib conf "name" in - let protocol = - if http then - sprintf "http://%s:8889" (Unix.gethostname ()) - else - sprintf "file://%s" Env.paparazzi_home in - let prefix = fun s -> sprintf "%s/%s" protocol s in - (** Expanded flight plan has been compiled in var/ *) - let fp = prefix ("var" // ac_name // "flight_plan.xml") - and af = prefix ("conf" // ExtXml.attrib conf "airframe") - and rc = prefix ("conf" // ExtXml.attrib conf "radio") in - let col = try Xml.attrib conf "gui_color" with _ -> new_color () in - let ac_name = try Xml.attrib conf "name" with _ -> "" in - ["ac_id", Pprz.String ac_id; - "flight_plan", Pprz.String fp; - "airframe", Pprz.String af; - "radio", Pprz.String rc; - "default_gui_color", Pprz.String col; - "ac_name", Pprz.String ac_name - ] - with - Not_found -> - failwith (sprintf "ground UNKNOWN %s" ac_id) - end - | _ -> - let s = String.concat " " (List.map (fun (a,v) -> a^"="^Pprz.string_of_value v) args) in - failwith (sprintf "Error, Receive.send_config: %s" s) + let ac_id' = Pprz.string_assoc "ac_id" args in + try + let ac_id, root_dir, conf_xml = + match replayed ac_id' with + Some ac_id -> + ac_id, "var/replay/", Xml.parse_file (Env.paparazzi_home // "var/replay/conf/conf.xml") + | None -> ac_id', "", conf_xml in + + let conf = ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "ac_id" = ac_id) in + let ac_name = ExtXml.attrib conf "name" in + let protocol = + if http then + sprintf "http://%s:8889" (Unix.gethostname ()) + else + sprintf "file://%s" Env.paparazzi_home in + let prefix = fun s -> sprintf "%s/%s%s" protocol root_dir s in + (** Expanded flight plan has been compiled in var/ *) + let fp = prefix ("var" // ac_name // "flight_plan.xml") + and af = prefix ("conf" // ExtXml.attrib conf "airframe") + and rc = prefix ("conf" // ExtXml.attrib conf "radio") in + let col = try Xml.attrib conf "gui_color" with _ -> new_color () in + let ac_name = try Xml.attrib conf "name" with _ -> "" in + ["ac_id", Pprz.String ac_id; + "flight_plan", Pprz.String fp; + "airframe", Pprz.String af; + "radio", Pprz.String rc; + "default_gui_color", Pprz.String col; + "ac_name", Pprz.String ac_name + ] + with + Not_found -> + failwith (sprintf "ground UNKNOWN %s" ac_id') let ivy_server = fun http -> ignore (Ground_Pprz.message_answerer my_id "AIRCRAFTS" send_aircrafts_msg); @@ -612,7 +621,7 @@ let _ = "-n", Arg.Clear logging, "Disable log"; "-http", Arg.Set http, "Send http: URLs (default is file:)"] in Arg.parse (options) - (fun x -> Printf.fprintf stderr "Warning: Don't do anything with %s\n" x) + (fun x -> Printf.fprintf stderr "%s: Warning: Don't do anything with '%s' argument\n" Sys.argv.(0) x) "Usage: "; Srtm.add_path srtm_path; diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index e2edb9e8f2..c24ad40cc5 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -428,9 +428,13 @@ module Messages(Class:CLASS) = struct let ivy_cb = fun _ args -> let asker = args.(0) and asker_id = args.(1) in - let values = cb asker (snd (values_of_string args.(2))) in - let m = string_of_message (snd (message_of_name msg_name)) values in - Ivy.send (sprintf "%s %s %s" asker_id sender m) in + try (** Against [cb] exceptions *) + let values = cb asker (snd (values_of_string args.(2))) in + let m = string_of_message (snd (message_of_name msg_name)) values in + Ivy.send (sprintf "%s %s %s" asker_id sender m) + with + exc -> fprintf stderr "Pprz.answerer %s:%s: %s\n%!" sender msg_name (Printexc.to_string exc) + in Ivy.bind ivy_cb (sprintf "^([^ ]*) +([^ ]*) +(%s_REQ.*)" msg_name) let gen_id = let r = ref 0 in fun () -> incr r; !r diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index af65d1be2e..886cc92927 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -29,7 +29,4 @@ clean: rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play play : play.ml - $(OCAMLC) -o $@ unix.cma glibivy-ocaml.cma -I +lablgtk2 -I ../lib/ocaml lablgtk.cma lib-pprz.cma gtkInit.cmo $^ # to check - cat ../../pprz_src_test.sh > $@ - echo 'exec lablgtk2 unix.cma glibivy-ocaml.cma -I $$PAPARAZZI_SRC/sw/lib/ocaml lib-pprz.cma $$PAPARAZZI_SRC/sw/logalizer/$< $$*' >> $@ - chmod a+x $@ + $(OCAMLC) -custom -o $@ unix.cma glibivy-ocaml.cma -I +lablgtk2 -I ../lib/ocaml lablgtk.cma lib-pprz.cma gtkInit.cmo $^ diff --git a/sw/logalizer/play.ml b/sw/logalizer/play.ml index 23067b544b..c5356ad9c1 100644 --- a/sw/logalizer/play.ml +++ b/sw/logalizer/play.ml @@ -24,18 +24,75 @@ * *) +open Printf + module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +let (//) = Filename.concat +let replay_dir = Env.paparazzi_home // "var" // "replay" +let dump_fp = Env.paparazzi_src // "sw" // "tools" // "gen_flight_plan.out -dump" + + let log = ref [||] -let load_log = fun window (adj:GData.adjustment) name -> - let f = Ocaml_tools.open_compress name in +let write_xml = fun f xml -> + let d = Filename.dirname f in + ignore (Sys.command (sprintf "mkdir -p %s" d)); + let m = open_out f in + fprintf m "%s" (Xml.to_string_fmt xml); + close_out m + + +let store_conf = fun conf acs -> + let l = + List.fold_right (fun x r -> + if ExtXml.tag_is x "aircraft" then + if List.mem (ExtXml.attrib x "ac_id") acs then + let w = fun s -> + let f = replay_dir // ExtXml.attrib x s in + write_xml f (ExtXml.child x s); + f in + ignore (w "airframe"); + ignore (w "radio"); + let fp = w "flight_plan" in + (** We must "dump" the flight plan from the original one *) + let ac_name = ExtXml.attrib x "name" in + let ac_dir = replay_dir // "var" // ac_name in + ignore (Sys.command (sprintf "mkdir -p %s" ac_dir)); + let dump = ac_dir // "flight_plan.xml" in + let c = sprintf "%s %s > %s" dump_fp fp dump in + if Sys.command c <> 0 then + failwith c; + Xml.Element ("aircraft", Xml.attribs x, [])::r + else r + else (** Keep ground section *) + x::r) + (Xml.children conf) [] in + let orig_conf = Xml.Element ("conf", [], l) in + write_xml (replay_dir // "conf" // "conf.xml") orig_conf + +let store_messages = fun protocol -> + write_xml (replay_dir // "conf" // "messages.xml") protocol + +let time_of = fun (t, _, _) -> t + +let load_log = fun window (adj:GData.adjustment) xml_file -> + let xml = Xml.parse_file xml_file in + let data_file = ExtXml.attrib xml "data_file" in + + let f = Ocaml_tools.find_file [Filename.dirname xml_file] data_file in + let f = Ocaml_tools.open_compress f in let lines = ref [] in + let acs = ref [] in try while true do let l = input_line f in try - Scanf.sscanf l "%f %[^\n]" (fun t m -> lines := (t,m):: !lines) + Scanf.sscanf l "%f %s %[^\n]" + (fun t ac m -> + lines := (t,ac,m):: !lines; + if not (List.mem ac !acs) then acs := ac :: !acs + ) with _ -> () done @@ -43,10 +100,14 @@ let load_log = fun window (adj:GData.adjustment) name -> End_of_file -> close_in f; log := Array.of_list (List.rev !lines); - let start = fst !log.(0) in - let end_ = fst !log.(Array.length !log - 1) in + let start = time_of !log.(0) in + let end_ = time_of !log.(Array.length !log - 1) in adj#set_bounds ~lower:start ~upper:end_ (); - window#set_title (Filename.basename name) + window#set_title (Filename.basename xml_file); + + store_conf (ExtXml.child xml "conf") !acs; + store_messages (ExtXml.child xml "protocol") + let timer = ref None @@ -59,7 +120,7 @@ let stop = fun () -> let file_dialog ~title ~callback () = - let sel = GWindow.file_selection ~title ~filename:"*.data[.*]" ~modal:true () in + let sel = GWindow.file_selection ~title ~filename:"*.log" ~modal:true () in ignore (sel#cancel_button#connect#clicked ~callback:sel#destroy); ignore (sel#ok_button#connect#clicked @@ -78,15 +139,15 @@ let index_of_time log t = let rec loop = fun a b -> if a >= b then a else let c = (a+b)/ 2 in - if t <= fst log.(c) then loop a c else loop (c+1) b in + if t <= time_of log.(c) then loop a c else loop (c+1) b in loop 0 (Array.length log - 1) let rec run log adj i speed = - let (t, m) = log.(i) in - Ivy.send (Printf.sprintf "%s" m); + let (t, ac, m) = log.(i) in + Ivy.send (Printf.sprintf "replay%s %s" ac m); adj#set_value t; if i + 1 < Array.length log then - let dt = fst log.(i+1) -. t in + let dt = time_of log.(i+1) -. t in timer := Some (GMain.Timeout.add (truncate (1000. *. dt /. speed#value)) (fun () -> run log adj (i+1) speed; false)) let play adj speed = @@ -109,7 +170,7 @@ let _ = val mutable v = 1. method value = v method set_value x = v <- x end in - let bus = ref "127.255.255.255:3333" in + let bus = ref "127.255.255.255:2010" in Arg.parse [ "-b", Arg.String (fun x -> bus := x), "Bus\tDefault is 127.255.255.25:2010"] (fun x -> load_log window adj x)