Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 08:22:43 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity
Files
ce706bcd04fad382ed3127dbb703593af894ca81
paparazzi/sw/ground_segment/tmtc/server.ml
T
Gautier Hattenberger ce706bcd04 Server json stream (#2792) 
* [server] stream telemetry messages as JSON string
can be used by external plotter like PlotJuggler (default port)
always enabled by default

* update pprzlink

Co-authored-by: Fabien-B <fabien.bonneval@gmail.com>
2021-11-02 18:51:07 +01:00

983 lines
41 KiB
OCaml
Raw Blame History

(*
* Multi aircrafts receiver, logger and broadcaster
*
* 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.
*
*)
let my_id = "ground"
let gps_mode_3D = 3
let no_md5_check = ref false
let replay_old_log = ref false
let log_name_arg = ref ""
let udp_json_stream_addr = ref "127.0.0.1"
let udp_json_stream_port = ref 9870
let udp_json_stream_disable = ref false
open Printf
open Latlong
open Server_globals
open Aircraft_server
open Quaternion
(*open Intruder*)
module U = Unix
module LL = Latlong
module Ground = struct let name = "ground" end
module Ground_Pprz = PprzLink.Messages(Ground)
module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end)
module Alerts_Pprz = PprzLink.Messages(struct let name = "alert" end)
module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end)
let dl_id = "ground_dl" (* Hack, should be [my_id] *)
let (//) = Filename.concat
let logs_path = Env.paparazzi_home // "var" // "logs"
let conf_xml = ExtXml.parse_file (Env.paparazzi_home // "conf" // "conf.xml")
let srtm_path = Env.paparazzi_home // "data" // "srtm"
let get_indexed_value = fun ?(text="UNK") t i ->
if i >= 0 then t.(i) else text
(** The aircrafts store *)
let aircrafts = Hashtbl.create 3
(** Broadcast of the received aircrafts *)
let aircraft_msg_period = 500 (* ms *)
let wind_msg_period = 5000 (* ms *)
let aircraft_alerts_period = 1000 (* ms *)
let send_aircrafts_msg = fun _asker _values ->
assert(_values = []);
let names = String.concat "," (Hashtbl.fold (fun k _v r -> k::r) aircrafts []) ^ "," in
["ac_list", PprzLink.String names]
let expand_aicraft x =
let ac_name = ExtXml.attrib x "name" in
let handle_error_message = fun error_type msg ->
prerr_endline ("A failure occurred while processing aircraft '"^ac_name^"'");
prerr_endline (" - "^error_type^" : "^msg);
prerr_endline (" - '"^ac_name^"' will be ignored by the server");
prerr_endline " - Please remove it from 'conf.xml' or fix its parameter(s)";
flush stderr;
Xml.Element ("ignoring_aircraft",["name", ac_name],[])
in
try
(* parse aircraft *)
let ac = Aircraft.parse_aircraft ~parse_all:true "" x in
(* Add latest generated settings if any with a special tag as it is the only way to have this information.
* The settings parsed by Aircraft module will not include module settings as we don't know the target.
* It should be the correct settings, unless an aircraft is rebuilt with different parameters or target
* and with the server already running and not restarted. *)
let settings_xml = try
let xml = ExtXml.parse_file (Env.paparazzi_home // "var" // "aircrafts" // ac_name // "settings.xml") in
[Xml.Element ("generated_settings", [], Xml.children xml)]
with _ -> []
in
if List.length ac.Aircraft.xml > 0 then Xml.Element (Xml.tag x, Xml.attribs x, ac.Aircraft.xml @ settings_xml)
else failwith "Nothing to parse"
with
| Failure msg -> handle_error_message "Fail with" msg
| Xml.File_not_found file -> handle_error_message "File not found" file
| Module.Module_not_found m -> handle_error_message "Module not found" m
| Dtd.Prove_error err -> handle_error_message "Dtd error" (Dtd.prove_error err)
| Not_found -> handle_error_message "Not found" "sorry, something went wrong somewhere"
let make_element = fun t a c -> Xml.Element (t,a,c)
let log_xml = fun timeofday data_file ->
let conf_children =
List.map
(fun x -> if Xml.tag x = "aircraft" then expand_aicraft x else x)
(Xml.children conf_xml) in
let expanded_conf = make_element (Xml.tag conf_xml) (Xml.attribs conf_xml) conf_children in
make_element
"configuration"
["time_of_day", string_of_float timeofday; "data_file", data_file]
[expanded_conf; PprzLink.messages_xml ()]
let start_time = U.gettimeofday ()
(* Opens the log files *)
let logger = fun () ->
let d = U.localtime start_time in
let basename = sprintf "%02d_%02d_%02d__%02d_%02d_%02d" (d.U.tm_year mod 100) (d.U.tm_mon+1) (d.U.tm_mday) (d.U.tm_hour) (d.U.tm_min) (d.U.tm_sec) in
if not (Sys.file_exists logs_path) then begin
printf "Creating '%s'\n" logs_path; flush stdout;
Unix.mkdir logs_path 0o755
end;
let log_name = if (!log_name_arg = "") then sprintf "%s.log" basename else sprintf "%s__%s.log" !log_name_arg basename in
let data_name = if (!log_name_arg = "") then sprintf "%s.data" basename else sprintf "%s__%s.log" !log_name_arg basename in
let f = open_out (logs_path // log_name) in
(* version string with whitespace/newline at the end stripped *)
output_string f ("<!-- logged with runtime paparazzi_version " ^ Env.get_paparazzi_version () ^ " -->\n");
let build_str =
try
let f = open_in (Env.paparazzi_home ^ "/var/build_version.txt") in
let s = try input_line f with _ -> "UNKNOWN" in
close_in f;
s
with _ -> "UNKNOWN" in
output_string f ("<!-- logged with build paparazzi_version " ^ build_str ^ " -->\n");
output_string f (Xml.to_string_fmt (log_xml start_time data_name));
close_out f;
open_out (logs_path // data_name)
let time_of_timestamp = function
| Some x -> x
| None -> U.gettimeofday() -. start_time
let log = fun ?timestamp logging ac_name msg_name values ->
match logging with
Some log ->
let s = string_of_values values in
let t = time_of_timestamp timestamp in
fprintf log "%.3f %s %s %s\n" t ac_name msg_name s; flush log
| None -> ()
(* Socket configuration for streaming *)
let udp_fd = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0
let udp_sockaddr = Unix.ADDR_INET (Unix.inet_addr_of_string !udp_json_stream_addr, !udp_json_stream_port)
(** Callback for a message from a registered A/C *)
let ac_msg = fun messages_xml logging ac_name ac ->
let module Tele_Pprz = PprzLink.MessagesOfXml(struct let xml = messages_xml let name="telemetry" end) in
fun ts m ->
try
let timestamp = try Some (float_of_string ts) with _ -> None in
let (msg_id, values) = Tele_Pprz.values_of_string m in
let msg = Tele_Pprz.message_of_id msg_id in
log ?timestamp logging ac_name msg.PprzLink.name values;
if not !udp_json_stream_disable then begin
let json_name =
try
let a = Hashtbl.find aircrafts ac_name in
sprintf "%s (%s)" a.name ac_name
with _ -> ac_name
in
let t = time_of_timestamp timestamp in
let json_msg = sprintf "{ \"%s\": %s, \"timestamp\": %f }" json_name (Tele_Pprz.json_of_message msg values) t in
let len = String.length json_msg in
let n = Unix.sendto udp_fd (Bytes.of_string json_msg) 0 len [] udp_sockaddr in
assert(n = len)
end;
Parse_messages_v1.log_and_parse ac_name ac msg values
with
Telemetry_error (ac_name, msg) ->
Ground_Pprz.message_send my_id "TELEMETRY_ERROR" ["ac_id", PprzLink.String ac_name;"message", PprzLink.String msg];
prerr_endline msg
| PprzLink.Unknown_msg_name (x, c) ->
fprintf stderr "Unknown message %s in class %s from %s: %s\n%!" x c ac_name m
| x -> prerr_endline (Printexc.to_string x)
(** If you are 1km above the ground, an angle of 89 degrees between the vertical and
your camera axis allow you to look 57km away: it should be enough ! **)
let cam_max_angle = (Deg>>Rad) 89.
let send_cam_status = fun a ->
if a.gps_mode >= gps_mode_3D then
match a.nav_ref with
None -> () (* No geo ref for camera target *)
| Some nav_ref ->
let (hfv, vfv) = a.camaov in
let tr = quaternion_from_angle (hfv /. -2.0) (vfv /. 2.0) 0.0
and tl = quaternion_from_angle (hfv /. 2.0) (vfv /. 2.0) 0.0
and br = quaternion_from_angle (hfv /. -2.0) (vfv /. -2.0) 0.0
and bl = quaternion_from_angle (hfv /. 2.0) (vfv /. -2.0) 0.0 in
let gimRot = quaternion_from_angle 0.0 (-. (a.cam.tilt -. (Deg>>Rad) 90.)) (-.a.cam.pan)
and acRot = quaternion_from_angle a.roll a.pitch (-.a.heading) in
let tr_rotated = multiply_quaternion acRot (multiply_quaternion gimRot tr)
and tl_rotated = multiply_quaternion acRot (multiply_quaternion gimRot tl)
and br_rotated = multiply_quaternion acRot (multiply_quaternion gimRot br)
and bl_rotated = multiply_quaternion acRot (multiply_quaternion gimRot bl) in
let bind_max_angles a =
if abs_float a > cam_max_angle then copysign cam_max_angle a else a in
let find_point_on_ground q =
let angles = quaternion_to_angle q in
let dx = a.agl *. tan(bind_max_angles angles.r)
and dy = a.agl *. tan(bind_max_angles angles.p) in
let utmx = dx *. cos angles.y -. dy *. sin angles.y
and utmy = dx *. sin angles.y +. dy *. cos angles.y in
Aircraft_server.add_pos_to_nav_ref (Geo a.pos) (utmx, utmy) in
let geo_1 = find_point_on_ground tr_rotated
and geo_2 = find_point_on_ground tl_rotated
and geo_3 = find_point_on_ground bl_rotated
and geo_4 = find_point_on_ground br_rotated in
let lats = sprintf "%f,%f,%f,%f," ((Rad>>Deg)geo_1.posn_lat) ((Rad>>Deg)geo_2.posn_lat) ((Rad>>Deg)geo_3.posn_lat) ((Rad>>Deg)geo_4.posn_lat) in
let longs = sprintf "%f,%f,%f,%f," ((Rad>>Deg)geo_1.posn_long) ((Rad>>Deg)geo_2.posn_long) ((Rad>>Deg)geo_3.posn_long) ((Rad>>Deg)geo_4.posn_long) in
let twgs84 = Aircraft_server.add_pos_to_nav_ref nav_ref a.cam.target in
let values = ["ac_id", PprzLink.String a.id;
"lats", PprzLink.String lats;
"longs", PprzLink.String longs;
"cam_target_lat", PprzLink.Float ((Rad>>Deg)twgs84.posn_lat);
"cam_target_long", PprzLink.Float ((Rad>>Deg)twgs84.posn_long)] in
Ground_Pprz.message_send my_id "CAM_STATUS" values
let send_if_calib = fun a ->
let if_mode = get_indexed_value if_modes a.inflight_calib.if_mode in
let values = ["ac_id", PprzLink.String a.id;
"if_mode", PprzLink.String if_mode;
"if_value1", PprzLink.Float a.inflight_calib.if_val1;
"if_value2", PprzLink.Float a.inflight_calib.if_val2] in
Ground_Pprz.message_send my_id "INFLIGH_CALIB" values
let send_fbw = fun a ->
let values = [ "ac_id", PprzLink.String a.id;
"rc_mode", PprzLink.String a.fbw.rc_mode;
"rc_status", PprzLink.String a.fbw.rc_status;
"rc_rate", PprzLink.Int a.fbw.rc_rate ] in
Ground_Pprz.message_send my_id "FLY_BY_WIRE" values
let send_dl_values = fun a ->
if a.nb_dl_setting_values > 0 then
let csv = ref "" in
for i = 0 to a.nb_dl_setting_values - 1 do
match a.dl_setting_values.(i) with
| None -> csv := sprintf "%s?," !csv
| Some s -> csv := sprintf "%s%f," !csv s
done;
let vs = ["ac_id", PprzLink.String a.id; "values", PprzLink.String !csv] in
Ground_Pprz.message_send my_id "DL_VALUES" vs
let send_svsinfo = fun a ->
let svid = ref ""
and flags= ref ""
and qi = ref ""
and cno = ref ""
and elev = ref ""
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
concat svid a.svinfo.(i).svid;
concat flags a.svinfo.(i).flags;
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 age a.svinfo.(i).age
done;
let f = fun s r -> (s, PprzLink.String !r) in
let vs = ["ac_id", PprzLink.String a.id;
"pacc", PprzLink.Int a.gps_Pacc;
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
let send_horiz_status = fun a ->
match a.horiz_mode with
Circle (geo, r) ->
let vs = [ "ac_id", PprzLink.String a.id;
"circle_lat", PprzLink.Float ((Rad>>Deg)geo.posn_lat);
"circle_long", PprzLink.Float ((Rad>>Deg)geo.posn_long);
"radius", PprzLink.Int r ] in
Ground_Pprz.message_send my_id "CIRCLE_STATUS" vs
| Segment (geo1, geo2) ->
let vs = [ "ac_id", PprzLink.String a.id;
"segment1_lat", PprzLink.Float ((Rad>>Deg)geo1.posn_lat);
"segment1_long", PprzLink.Float ((Rad>>Deg)geo1.posn_long);
"segment2_lat", PprzLink.Float ((Rad>>Deg)geo2.posn_lat);
"segment2_long", PprzLink.Float ((Rad>>Deg)geo2.posn_long) ] in
Ground_Pprz.message_send my_id "SEGMENT_STATUS" vs
| UnknownHorizMode -> ()
let send_survey_status = fun a ->
match a.survey with
None ->
Ground_Pprz.message_send my_id "SURVEY_STATUS" ["ac_id", PprzLink.String a.id]
| Some (geo1, geo2) ->
let vs = [ "ac_id", PprzLink.String a.id;
"south_lat", PprzLink.Float ((Rad>>Deg)geo1.posn_lat);
"west_long", PprzLink.Float ((Rad>>Deg)geo1.posn_long);
"north_lat", PprzLink.Float ((Rad>>Deg)geo2.posn_lat);
"east_long", PprzLink.Float ((Rad>>Deg)geo2.posn_long) ] in
Ground_Pprz.message_send my_id "SURVEY_STATUS" vs
let send_wind = fun a ->
let id = a.id in
try
let (wind_cap_rad, wind_polar, mean, stddev, _nb_sample) = Wind.get id in
let vs =
["ac_id", PprzLink.String id;
"dir", PprzLink.Float ((Rad>>Deg)wind_cap_rad);
"wspeed", PprzLink.Float wind_polar;
"mean_aspeed", PprzLink.Float mean;
"stddev", PprzLink.Float stddev] in
Ground_Pprz.message_send my_id "WIND" vs
with
_exc -> ()
let send_telemetry_status = fun a ->
let id = a.id in
let tl_payload = fun link_id datalink_status link_status ->
let time_since_last_msg = if link_id = "no_id" then
U.gettimeofday () -. a.last_msg_date
else
let tslm = U.gettimeofday () -. a.last_msg_date in
let tsllm = U.gettimeofday () -. link_status.last_link_status_date +. (float_of_int link_status.rx_lost_time) in
if tslm > tsllm then tslm else tsllm (* greatest time between last message and last message from this link *)
in
[ "ac_id", PprzLink.String id;
"link_id", PprzLink.String link_id;
"time_since_last_msg", PprzLink.Float (time_since_last_msg); (* don't use rx_lost_time from LINK_REPORT so it also works in simulation *)
"rx_bytes", PprzLink.Int64 (Int64.of_int link_status.rx_bytes);
"rx_msgs", PprzLink.Int64 (Int64.of_int link_status.rx_msgs);
"rx_bytes_rate", PprzLink.Float link_status.rx_bytes_rate;
"tx_msgs", PprzLink.Int64 (Int64.of_int link_status.tx_msgs);
"uplink_lost_time", PprzLink.Int64 (Int64.of_int datalink_status.uplink_lost_time);
"uplink_msgs", PprzLink.Int datalink_status.uplink_msgs;
"downlink_msgs", PprzLink.Int datalink_status.downlink_msgs;
"downlink_rate", PprzLink.Int datalink_status.downlink_rate;
"ping_time", PprzLink.Float link_status.ping_time]
in
(* if no link send anyway for rx_lost_time with special link id *)
if Hashtbl.length a.link_status = 0 then
begin
let vs = tl_payload "no_id" a.datalink_status (Aircraft_server.link_status_init ()) in
Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
end
else
(* send telemetry status for each link *)
Hashtbl.iter (fun link_id link_status ->
try
let vs = tl_payload (string_of_int link_id) a.datalink_status link_status in
Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
with
_exc -> ()
) a.link_status
let send_moved_waypoints = fun a ->
Hashtbl.iter
(fun wp_id wp ->
let geo = wp.wp_geo in
let vs =
["ac_id", PprzLink.String a.id;
"wp_id", PprzLink.Int wp_id;
"long", PprzLink.Float ((Rad>>Deg)geo.posn_long);
"lat", PprzLink.Float ((Rad>>Deg)geo.posn_lat);
"alt", PprzLink.Float wp.altitude;
"ground_alt", PprzLink.Float (try float (Srtm.of_wgs84 geo) with _ -> a.ground_alt)] in
Ground_Pprz.message_send my_id "WAYPOINT_MOVED" vs)
a.waypoints
let send_aircraft_msg = fun ac ->
try
let a = Hashtbl.find aircrafts ac in
let f = fun x -> PprzLink.Float x in
let wgs84 = try a.pos with _ -> LL.make_geo 0. 0. in
let values = ["ac_id", PprzLink.String ac;
"roll", f (Geometry_2d.rad2deg a.roll);
"pitch", f (Geometry_2d.rad2deg a.pitch);
"heading", f (Geometry_2d.rad2deg a.heading);
"lat", f ((Rad>>Deg)wgs84.posn_lat);
"long", f ((Rad>>Deg) wgs84.posn_long);
"unix_time", f a.unix_time;
"itow", PprzLink.Int64 a.itow;
"speed", f a.gspeed;
"airspeed", f a.airspeed; (* negative value is sent if no airspeed available *)
"course", f (Geometry_2d.rad2deg a.course);
"alt", f a.alt;
"agl", f a.agl;
"climb", f a.climb] in
Ground_Pprz.message_send my_id "FLIGHT_PARAM" values;
(** send ACINFO messages if more than one A/C registered *)
if Hashtbl.length aircrafts > 1 then
begin
let cm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate (100. *. f))) in
let cm_of_m = fun f -> PprzLink.Int (truncate (100. *. f)) in
if a.vehicle_type = FixedWing then
let pos = LL.utm_of WGS84 a.pos in
let ac_info = ["ac_id", PprzLink.String ac;
"utm_east", cm_of_m_32 pos.utm_x;
"utm_north", cm_of_m_32 pos.utm_y;
"utm_zone", PprzLink.Int pos.utm_zone;
"course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg a.course)));
"alt", cm_of_m_32 a.alt;
"speed", cm_of_m a.gspeed;
"climb", cm_of_m a.climb;
"itow", PprzLink.Int64 a.itow] in
Dl_Pprz.message_send dl_id "ACINFO" ac_info;
else
let deg7_of_rad = fun f -> PprzLink.Int32 (Int32.of_float (Geometry_2d.rad2deg (f *. 1e7))) in
let ac_info_lla = ["ac_id", PprzLink.String ac;
"lat", deg7_of_rad a.pos.posn_lat;
"lon", deg7_of_rad a.pos.posn_long;
"course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg a.course)));
"alt", cm_of_m_32 a.alt;
"speed", cm_of_m a.gspeed;
"climb", cm_of_m a.climb;
"itow", PprzLink.Int64 a.itow] in
Dl_Pprz.message_send dl_id "ACINFO_LLA" ac_info_lla;
end;
if !Kml.enabled then
Kml.update_ac a;
begin
match a.nav_ref with
Some nav_ref ->
let values = ["ac_id", PprzLink.String ac;
"cur_block", PprzLink.Int a.cur_block;
"cur_stage", PprzLink.Int a.cur_stage;
"stage_time", PprzLink.Int64 (Int64.of_int a.stage_time);
"block_time", PprzLink.Int64 (Int64.of_int a.block_time);
"target_lat", f ((Rad>>Deg)a.desired_pos.posn_lat);
"target_long", f ((Rad>>Deg)a.desired_pos.posn_long);
"target_alt", PprzLink.Float a.desired_altitude;
"target_climb", PprzLink.Float a.desired_climb;
"target_course", PprzLink.Float ((Rad>>Deg)a.desired_course);
"dist_to_wp", PprzLink.Float a.dist_to_wp
] in
Ground_Pprz.message_send my_id "NAV_STATUS" values
| None -> () (* No nav_ref yet *)
end;
let values = ["ac_id", PprzLink.String ac;
"throttle", f a.throttle;
"throttle_accu", f a.throttle_accu;
"rpm", f a.rpm;
"temp", f a.temp;
"bat", f a.bat;
"amp", f a.amp;
"charge", f a.charge] in
Ground_Pprz.message_send my_id "ENGINE_STATUS" values;
let ap_mode = get_indexed_value ~text:(if a.ap_mode = -2 then "FAIL" else "UNK") (modes_of_aircraft a) a.ap_mode in
let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
let lat_mode = get_indexed_value lat_modes a.lateral_mode in
let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
let gps_mode = get_indexed_value gps_modes a.gps_mode in
let state_filter_mode = get_indexed_value state_filter_modes a.state_filter_mode
and kill_mode = if a.kill_mode then "ON" else "OFF" in
let values = ["ac_id", PprzLink.String ac;
"flight_time", PprzLink.Int64 (Int64.of_int a.flight_time);
"ap_mode", PprzLink.String ap_mode;
"gaz_mode", PprzLink.String gaz_mode;
"lat_mode", PprzLink.String lat_mode;
"horiz_mode", PprzLink.String horiz_mode;
"gps_mode", PprzLink.String gps_mode;
"state_filter_mode", PprzLink.String state_filter_mode;
"kill_mode", PprzLink.String kill_mode
] in
Ground_Pprz.message_send my_id "AP_STATUS" values;
send_cam_status a;
send_if_calib a;
send_fbw a;
send_svsinfo a;
send_horiz_status a;
a.time_since_last_survey_msg <- a.time_since_last_survey_msg +. float aircraft_msg_period /. 1000.;
if a.time_since_last_survey_msg > 5. then (* FIXME Two missed messages *)
a.survey <- None;
send_survey_status a;
send_dl_values a;
send_moved_waypoints a;
if !Kml.enabled then
Kml.update_waypoints a;
send_telemetry_status a
with
Not_found -> prerr_endline ac
| x -> prerr_endline (Printexc.to_string x)
(** Check if it is a replayed A/C (c.f. sw/logalizer/play.ml) *)
let replayed = fun ac_id ->
let n = String.length ac_id in
if n > 6 && String.sub ac_id 0 6 = "replay" then
(true, String.sub ac_id 6 (n - 6), "/var/replay/", ExtXml.parse_file (Env.paparazzi_home // "var/replay/conf/conf.xml"))
else
(false, ac_id, "", conf_xml)
(* Store of unknown received A/C ids. To be able to report an error only once *)
let unknown_aircrafts = Hashtbl.create 5
(* Intruders (external aircrafts), e.g. received via ADS-B *)
let intruders = Hashtbl.create 3
let get_conf = fun real_id id conf_xml ->
try
ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "ac_id" = id)
with
Not_found ->
Hashtbl.add unknown_aircrafts real_id ();
failwith (sprintf "Error: A/C '%s' not found" id)
let check_md5sum = fun ac_name alive_md5sum aircraft_conf_dir ->
(* Read the digest generated by sw/tools/gen_aircraft *)
let f = open_in (aircraft_conf_dir // "aircraft.md5") in
let md5sum = input_line f in
close_in f;
try
match alive_md5sum with
PprzLink.Array array ->
let n = Array.length array in
assert(n = String.length md5sum / 2);
for i = 0 to n - 1 do
let x = int_of_string (sprintf "0x%c%c" md5sum.[2*i] md5sum.[2*i+1]) in
assert (x = PprzLink.int_of_value array.(i))
done
| _ -> failwith "Array expected here"
with _ ->
try
match alive_md5sum with
PprzLink.Array array ->
let n = Array.length array in
assert(n = String.length md5sum / 2);
for i = 0 to n - 1 do
let x = 0 in
assert (x = PprzLink.int_of_value array.(i))
done;
fprintf stderr "MD5 is ZERO, be carefull with configurations\n%!"
| _ -> failwith "Array expected here"
with _ ->
let error_message = sprintf "WARNING: live md5 signature for %s does not match current configuration, please reload your code (disable check with -no_md5_check option)" ac_name in
if !no_md5_check then
fprintf stderr "%s; continuing anyway as requested\n%!" error_message
else
failwith error_message
let new_aircraft = fun get_alive_md5sum real_id ->
let is_replayed, id, root_dir, conf_xml = replayed real_id in
let conf = get_conf real_id id conf_xml in
let ac_name = ExtXml.attrib conf "name" in
let var_aircraft_dir = Env.paparazzi_home // root_dir // "var" // "aircrafts" // ac_name in
if not (Sys.file_exists var_aircraft_dir) then begin
(* Let's look for a backup configuration with the md5 signature *)
end;
let fp_file = var_aircraft_dir // "flight_plan.xml" in
let xml_fp = ExtXml.child (ExtXml.parse_file fp_file) "flight_plan" in
let aircraft_conf_dir = var_aircraft_dir // "conf" in
let airframe_file = aircraft_conf_dir // ExtXml.attrib conf "airframe" in
let airframe_xml = ExtXml.parse_file airframe_file in
if not is_replayed then
check_md5sum real_id (get_alive_md5sum ()) aircraft_conf_dir;
let ac = Aircraft_server.new_aircraft real_id ac_name xml_fp airframe_xml in
let update = fun () ->
for i = 0 to Array.length ac.svinfo - 1 do
ac.svinfo.(i).age <- ac.svinfo.(i).age + 1;
done in
ignore (ac.ap_modes <- try
let ac = Aircraft.parse_aircraft "" airframe_xml in
match ac.Aircraft.autopilots with
| None -> None
| Some [(_, ap)] -> Some (modes_from_autopilot ap.Autopilot.xml)
| _ -> None (* more than one *)
with _ -> None);
ignore (Glib.Timeout.add 1000 (fun _ -> update (); true));
let messages_xml = ExtXml.parse_file (Env.paparazzi_home // root_dir // "var" // "messages.xml") in
ac, messages_xml
let check_alerts = fun a ->
let bat_section = ExtXml.child a.airframe ~select:(fun x -> Xml.attrib x "name" = "BAT") "section" in
let fvalue = fun name default ->
try ExtXml.float_attrib (ExtXml.child bat_section ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value" with _ -> default in
let catastrophic_level = fvalue "CATASTROPHIC_BAT_LEVEL" 9.
and critic_level = fvalue "CRITIC_BAT_LEVEL" 10.
and warning_level = fvalue "LOW_BAT_LEVEL" 10.5 in
let send = fun level ->
let vs = [ "ac_id", PprzLink.String a.id;
"level", PprzLink.String level;
"value", PprzLink.Float a.bat] in
Alerts_Pprz.message_send my_id "BAT_LOW" vs in
if a.bat < 1. then send "INVALID"
else if a.bat < catastrophic_level then send "CATASTROPHIC"
else if a.bat < critic_level then send "CRITIC"
else if a.bat < warning_level then send "WARNING"
let wind_clear = fun _sender vs ->
Wind.clear (PprzLink.string_assoc "ac_id" vs)
let periodic = fun period cb ->
Glib.Timeout.add period (fun () -> cb (); true)
let register_periodic = fun ac x ->
ac.periodic_callbacks <- x :: ac.periodic_callbacks
(** add the periodic airprox check for the aircraft (name) on all aircraft *)
(** already known *)
let periodic_airprox_check = fun name ->
let thisac = Hashtbl.find aircrafts name in
let aircrafts2list = fun () ->
let list = ref [] in
Hashtbl.iter (fun name _a -> list := name :: !list) aircrafts;
!list in
let ac_names = List.filter (fun a -> a <> name) (aircrafts2list ()) in
let list_ac = List.map (fun name -> Hashtbl.find aircrafts name) ac_names in
let check_airprox = fun ac ->
try
match Airprox.check_airprox thisac ac with
None -> ()
| Some level ->
let vs =
["ac_id", PprzLink.String (thisac.id ^ "," ^ ac.id) ; "level", PprzLink.String level] in
Alerts_Pprz.message_send my_id "AIR_PROX" vs
with
x -> fprintf stderr "check_airprox: %s\n%!" (Printexc.to_string x)
in
List.iter
(fun ac ->
register_periodic thisac (periodic aircraft_alerts_period (fun () -> check_airprox ac))
) list_ac
let register_aircraft = fun name a ->
Hashtbl.add aircrafts name a;
register_periodic a (periodic aircraft_msg_period (fun () -> send_aircraft_msg name));
register_periodic a (periodic aircraft_alerts_period (fun () -> check_alerts a));
periodic_airprox_check name;
register_periodic a (periodic wind_msg_period (fun () -> send_wind a));
Wind.new_ac name 36;
ignore(Ground_Pprz.message_bind "WIND_CLEAR" wind_clear);
if !Kml.enabled then
Kml.build_files a
(** Identifying message from an A/C *)
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 () -> PprzLink.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 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", PprzLink.String name]
with
exc -> prerr_endline (Printexc.to_string exc)
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 timestamp ->
(** Wait for any message (they all are identified with the A/C) *)
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 timestamp))
(* Remove aicraft on AIRCRAFT_DIE message. *)
let remove_aircraft = fun _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs in
Hashtbl.remove aircrafts ac_id
let send_intruder_acinfo = fun id intruder ->
let cm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate (100. *. f))) in
let cm_of_m = fun f -> PprzLink.Int (truncate (100. *. f)) in
let pos = LL.utm_of WGS84 intruder.Intruder.pos in
(* TODO: find a better way to map intruders to AC_IDs *)
let ac_id = 200 + ((int_of_string id) mod 50) in
let ac_info = ["ac_id", PprzLink.Int ac_id;
"utm_east", cm_of_m_32 pos.utm_x;
"utm_north", cm_of_m_32 pos.utm_y;
"course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg intruder.Intruder.course)));
"alt", cm_of_m_32 intruder.Intruder.alt;
"speed", cm_of_m intruder.Intruder.gspeed;
"climb", cm_of_m intruder.Intruder.climb;
"itow", PprzLink.Int64 intruder.Intruder.itow] in
Dl_Pprz.message_send dl_id "ACINFO" ac_info
let periodic_handle_intruders = fun () ->
(* remove old intruders after 10s *)
Hashtbl.iter
(fun id i ->
if (U.gettimeofday () -. i.Intruder.unix_time) > 10.0 then begin
(*prerr_endline (sprintf "remove intruder %s" id);*)
Hashtbl.remove intruders id
end;
) intruders;
(* send ACINFO for each active intruder *)
Hashtbl.iter (send_intruder_acinfo) intruders
let add_intruder = fun vs ->
let id = PprzLink.string_assoc "id" vs in
let name = PprzLink.string_assoc "name" vs in
let intruder = Intruder.new_intruder id name in
Hashtbl.add intruders id intruder
let update_intruder = fun logging _sender vs ->
try
let id = PprzLink.string_assoc "id" vs in
(*prerr_endline (sprintf "update_intruder %s" id);*)
if not (Hashtbl.mem intruders id) then
add_intruder vs;
let i = Hashtbl.find intruders id in
let lat = PprzLink.int_assoc "lat" vs
and lon = PprzLink.int_assoc "lon" vs in
let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
i.Intruder.pos <- geo;
i.Intruder.alt <- float (PprzLink.int_assoc "alt" vs) /. 1000.;
i.Intruder.course <- PprzLink.float_assoc "course" vs;
i.Intruder.gspeed <- PprzLink.float_assoc "speed" vs;
i.Intruder.climb <- PprzLink.float_assoc "climb" vs;
i.Intruder.unix_time <- U.gettimeofday ();
log logging "ground" "INTRUDER" vs
with
Failure msg ->
prerr_endline ("Error parsing INTRUDER message: "^msg)
(* listen for intruders and log them *)
let listen_intruders = fun log ->
ignore(Ground_Pprz.message_bind "INTRUDER" (update_intruder log))
let send_config = fun http _asker args ->
let real_id = PprzLink.string_assoc "ac_id" args in
try
let _is_replayed, ac_id, root_dir, conf_xml = replayed real_id 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:%d" !hostname !port
else
sprintf "file://%s" Env.paparazzi_home in
let prefix = fun s -> sprintf "%s/%s%s" protocol root_dir s in
(** Expanded flight plan and settings have been compiled in var/ *)
let fp = prefix ("var" // "aircrafts" // ac_name // "flight_plan.xml")
and af = prefix ("conf" // ExtXml.attrib conf "airframe")
and rc = prefix ("conf" // ExtXml.attrib conf "radio")
and settings = prefix ("var" // "aircrafts" // ac_name // "settings.xml") 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", PprzLink.String real_id;
"flight_plan", PprzLink.String fp;
"airframe", PprzLink.String af;
"radio", PprzLink.String rc;
"settings", PprzLink.String settings;
"default_gui_color", PprzLink.String col;
"ac_name", PprzLink.String ac_name ]
with
Not_found ->
failwith (sprintf "ground UNKNOWN %s" real_id)
let ivy_server = fun http ->
ignore (Ground_Pprz.message_answerer my_id "AIRCRAFTS" send_aircrafts_msg);
ignore (Ground_Pprz.message_answerer my_id "CONFIG" (send_config http))
(** Convert to cm, with rounding *)
let cm_of_m = fun f -> PprzLink.Int (truncate ((100. *. f) +. 0.5))
(** Convert to mm, with rounding *)
let mm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate ((1000. *. f) +. 0.5)))
(** Got a ground.MOVE_WAYPOINT and send a datalink.MOVE_WP *)
let move_wp = fun logging _sender vs ->
let f = fun a -> List.assoc a vs
and ac_id = PprzLink.string_assoc "ac_id" vs
and deg7 = fun f -> PprzLink.Int32 (Int32.of_float (PprzLink.float_assoc f vs *. 1e7)) in
let vs = [ "wp_id", f "wp_id";
"ac_id", PprzLink.String ac_id;
"lat", deg7 "lat";
"lon", deg7 "long";
"alt", mm_of_m_32 (PprzLink.float_assoc "alt" vs) ] in
Dl_Pprz.message_send dl_id "MOVE_WP" vs;
log logging ac_id "MOVE_WP" vs
(** Got a DL_EMERGENCY_CMD, and send an EMERGENCY_CMD *)
let emergency_cmd = fun logging _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs in
let vs = [ "ac_id", PprzLink.String ac_id;
"cmd", List.assoc "cmd" vs] in
Dl_Pprz.message_send dl_id "EMERGENCY_CMD" vs;
log logging ac_id "EMERGENCY_CMD" vs
(** Got a DL_SETTING, and send an SETTING *)
let setting = fun logging _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs in
let vs = [ "index", List.assoc "index" vs;
"ac_id", PprzLink.String ac_id;
"value", List.assoc "value" vs] in
Dl_Pprz.message_send dl_id "SETTING" vs;
log logging ac_id "SETTING" vs;
try
(* mark the setting as not yet confirmed *)
let ac = Hashtbl.find aircrafts ac_id in
let idx = PprzLink.int_of_value (List.assoc "index" vs) in
ac.dl_setting_values.(idx) <- None
with Not_found -> ()
(** Got a GET_DL_SETTING, and send an GET_SETTING *)
let get_setting = fun logging _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs in
let vs = [ "index", List.assoc "index" vs;
"ac_id", PprzLink.String ac_id ] in
Dl_Pprz.message_send dl_id "GET_SETTING" vs;
log logging ac_id "GET_SETTING" vs;
try
(* mark the setting as not yet confirmed *)
let ac = Hashtbl.find aircrafts ac_id in
let idx = PprzLink.int_of_value (List.assoc "index" vs) in
ac.dl_setting_values.(idx) <- None
with Not_found -> ()
(** Got a JUMP_TO_BLOCK, and send an BLOCK *)
let jump_block = fun logging _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs in
let vs = ["block_id", List.assoc "block_id" vs; "ac_id", PprzLink.String ac_id] in
Dl_Pprz.message_send dl_id "BLOCK" vs;
log logging ac_id "BLOCK" vs
(** Got a RAW_DATALINK, send its contents *)
let raw_datalink = fun logging _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs
and m = Bytes.of_string (PprzLink.string_assoc "message" vs) in
for i = 0 to Bytes.length m - 1 do
if Bytes.get m i = ';' then Bytes.set m i ' '
done;
let msg_id, vs = Dl_Pprz.values_of_string (Bytes.to_string m) in
let msg = Dl_Pprz.message_of_id msg_id in
Dl_Pprz.message_send dl_id msg.PprzLink.name vs;
log logging ac_id msg.PprzLink.name vs
(** Got a LINK_REPORT, update state but don't send (done asynchronously) *)
let link_report = fun logging _sender vs ->
let ac_id = PprzLink.string_assoc "ac_id" vs
and link_id = int_of_string (PprzLink.string_assoc "link_id" vs) in
try
let ac = Hashtbl.find aircrafts ac_id in
let link_status = {
last_link_status_date = U.gettimeofday ();
Aircraft_server.rx_lost_time = PprzLink.int_assoc "rx_lost_time" vs;
rx_bytes = PprzLink.int_assoc "rx_bytes" vs;
rx_msgs = PprzLink.int_assoc "rx_msgs" vs;
rx_bytes_rate = PprzLink.float_assoc "rx_bytes_rate" vs;
tx_msgs = PprzLink.int_assoc "tx_msgs" vs;
ping_time = PprzLink.float_assoc "ping_time" vs;
} in
Hashtbl.replace ac.link_status link_id link_status;
log logging ac_id "LINK_REPORT" vs
with _ -> ()
(** Get the 'ground' uplink messages, log them and send 'datalink' messages *)
let ground_to_uplink = fun logging ->
let bind_log_and_send = fun name handler ->
ignore (Ground_Pprz.message_bind name (handler logging)) in
bind_log_and_send "MOVE_WAYPOINT" move_wp;
bind_log_and_send "DL_EMERGENCY_CMD" emergency_cmd;
bind_log_and_send "DL_SETTING" setting;
bind_log_and_send "GET_DL_SETTING" get_setting;
bind_log_and_send "JUMP_TO_BLOCK" jump_block;
bind_log_and_send "RAW_DATALINK" raw_datalink;
bind_log_and_send "LINK_REPORT" link_report
(* main loop *)
let () =
let ivy_bus = ref Defivybus.default_ivy_bus
and logging = ref true
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);
"-http", Arg.Set http, "Send http: URLs (default is file:)";
"-hostname", Arg.Set_string hostname, "<hostname> Set the address for the http server";
"-port", Arg.Set_int port, (sprintf "<port> Set http port for serving XML and KML files (default is %d)" !port);
"-kml", Arg.Set Kml.enabled, "Enable KML file updating";
"-kml_no_http", Arg.Set Kml.no_http, "KML without web server (local files only)";
"-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";
"-log_name", Arg.Set_string log_name_arg, "Name for output log (Time stamp will be";
"-udp_json_stream_addr", Arg.Set_string udp_json_stream_addr, "Address of the server to stream udp JSON telemetry messages (default is localhost)";
"-udp_json_stream_port", Arg.Set_int udp_json_stream_port, "Port of the server to stream udp JSON telemetry messages (default is 9870)";
"-udp_json_stream_disable", Arg.Set udp_json_stream_disable, "Disable udp JSON stream";
"-replay_old_log", Arg.Set replay_old_log, "Enable aircraft registering on PPRZ_MODE messages"] in
Arg.parse
options
(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;
Ivy.init "Paparazzi server" "READY" (fun _ _ -> ());
Ivy.start !ivy_bus;
let logging =
if !logging then
(* Opens the log file *)
Some (logger ())
else
None in
(* Waits for new aircrafts *)
listen_acs logging !timestamp;
ignore(Ground_Pprz.message_bind "AIRCRAFT_DIE" remove_aircraft);
(* wait for new external vehicles/intruders *)
listen_intruders logging;
(* Forward messages from ground agents to vehicles *)
ground_to_uplink logging;
(* call periodic_handle_intruders every second *)
ignore (Glib.Timeout.add 1000 (fun () -> periodic_handle_intruders (); true));
(* Waits for client configurations requests on the Ivy bus *)
ivy_server !http;
let loop = Glib.Main.create true in
while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done
Reference in New Issue View Git Blame Copy Permalink