play_core.ml
(*
* Log player
*
* Copyright (C) 2004 CENA/ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*)
open Printf
module Ground_Pprz = Pprz.Messages(struct let name = "ground" end)
module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end)
let (//) = Filename.concat
let replay_dir = Env.paparazzi_home // "var" // "replay"
let dump_fp = Env.paparazzi_src // "sw" // "tools" // "generators" // "gen_flight_plan.out -dump"
let log = ref [||]
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 ac_name = ExtXml.attrib x "name" in
let ac_dir = replay_dir // "var" // "aircrafts" // ac_name in
let w = fun s ->
(* Histotical: still useful ? *)
let f = replay_dir // "conf" // ExtXml.attrib x s in
write_xml f (ExtXml.child x s);
(* Write in the conf/ directory of the A/C *)
let f = ac_dir // "conf" // ExtXml.attrib x s in
write_xml f (ExtXml.child x s);
f in
ignore (w "airframe");
ignore (w "radio");
(* test if flight plan is an original one or the dumped version *)
let orig_fp = List.exists (fun e -> compare (Xml.tag e) "flight_plan" = 0) (Xml.children x) in
if orig_fp then begin
let fp = w "flight_plan" in
(** We must "dump" the flight plan from the original one *)
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;
end
else begin
let f = ac_dir // "flight_plan.xml" in
write_xml f (ExtXml.child x "dump");
end;
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 get_log_bounds = fun () ->
let start = time_of !log.(0) in
let end_ = time_of !log.(Array.length !log - 1) in
(start, end_)
let load_log = fun 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 %s %[^\n]"
(fun t ac m ->
lines := (t,ac,m):: !lines;
if not (List.mem ac !acs) then acs := ac :: !acs
)
with
_ -> ()
done
with
End_of_file ->
close_in f;
log := Array.of_list (List.rev !lines);
store_conf (ExtXml.child xml "conf") !acs;
store_messages (ExtXml.child xml "protocol")
let timer = ref None
let was_running = ref false
let bus = ref Defivybus.default_ivy_bus
let port = ref "/dev/ttyUSB0"
let baudrate = ref "9600"
let hw_flow_control = ref false
let file_to_load = ref ""
let output_on_serial = ref false
let stop = fun () ->
match !timer with
None -> ()
| Some t -> GMain.Timeout.remove t; timer := None
let index_of_time = fun log t ->
let rec loop = fun a b ->
if a >= b then a else
let c = (a+b)/ 2 in
if t <= time_of log.(c) then loop a c else loop (c+1) b in
loop 0 (Array.length log - 1)
let run = fun serial_port log adj i0 speed no_gui ->
let rec loop = fun i ->
let (t, ac, m) = log.(i) in
(* extract message name *)
let name = Str.string_before m (Str.search_forward (Str.regexp " ") m 0) in
(* continue if message is in telemetry class *)
begin try
let _ = Tm_Pprz.message_of_name name in
Ivy.send (Printf.sprintf "replay%s %s" ac m);
Ivy.send (Printf.sprintf "time%s %f" ac t);
begin
match serial_port with
None -> ()
| Some channel ->
try
let msg_id, vs = Tm_Pprz.values_of_string m in
let payload = Tm_Pprz.payload_of_values msg_id (int_of_string ac) vs in
let buf = Pprz.Transport.packet payload in
Debug.call 'o' (fun f -> fprintf f "%s\n" (Debug.xprint buf));
fprintf channel "%s%!" buf
with
_ -> ()
end;
with _ -> (); end;
adj#set_value t;
if i + 1 < Array.length log then begin
let dt = time_of log.(i+1) -. t in
timer := Some (GMain.Timeout.add (truncate (1000. *. dt /. speed#value)) (fun () -> loop (i+1);false))
end else if no_gui then
exit 0
in
loop i0
let play = fun ?(no_gui=false) serial_port adj speed ->
stop ();
if Array.length !log > 1 then
run serial_port !log adj (index_of_time !log adj#value) speed no_gui
let init = fun () ->
Arg.parse
[ "-b", Arg.String (fun x -> bus := x), (sprintf "<ivy bus> Default is %s" !bus);
"-d", Arg.Set_string port, (sprintf "<port> Default is %s" !port);
"-o", Arg.Set output_on_serial, "Output binary messages on serial port";
"-s", Arg.Set_string baudrate, (sprintf "<baudrate> Default is %s" !baudrate);
"-shfc", Arg.Set hw_flow_control, "Enable UART hardware flow control (CTS/RTS)";]
(fun x -> file_to_load := x)
"Usage: ";
if !file_to_load <> "" then
load_log !file_to_load;
let serial_port =
if !output_on_serial then
Some (Unix.out_channel_of_descr (Serial.opendev !port (Serial.speed_of_baudrate !baudrate) !hw_flow_control))
else
None in
let adj = GData.adjustment
~value:0. ~lower:0. ~upper:1000.
~step_incr:0.5 ~page_incr:1.0 ~page_size:1.0 () in
let speed = object
val mutable v = 1. method value = v method set_value x = v <- x
end in
let world_update_time = fun _ vs ->
speed#set_value (Pprz.float_assoc "time_scale" vs)
in
ignore (Ground_Pprz.message_bind "WORLD_ENV" world_update_time);
(serial_port, adj, speed)
let main = fun () ->
Ivy.init "Paparazzi replay" "READY" (fun _ _ -> ());
Ivy.start !bus;
GMain.Main.main ()