diff --git a/conf/messages.xml b/conf/messages.xml index f563e4f690..c6f245ce43 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -183,25 +183,8 @@ - - - - - - - - - - - - - - - - - - - + + @@ -238,10 +221,13 @@ - - - - + + Datalink status reported by an aircraft for the ground + + + + + @@ -601,18 +587,6 @@ - - - - - - - - - - - - @@ -2768,27 +2742,59 @@ + + Datalink status reported by Server for the GCS + Combines DATLINK_REPORT (telemetry class) and LINK_REPORT (ground class) + + + + + + + + + + + + Report a telemetry error - + + Encapsulated a telemetry class message (when using redundant link) - + + Encapsulated a datalink class message (when using redundant link) + + Datalink status reported by Link for the Server + + + + + + + + + + + + + diff --git a/conf/telemetry/default_ardrone.xml b/conf/telemetry/default_ardrone.xml index bc329e0112..2cdc68e548 100644 --- a/conf/telemetry/default_ardrone.xml +++ b/conf/telemetry/default_ardrone.xml @@ -18,6 +18,7 @@ + diff --git a/conf/telemetry/default_fixedwing.xml b/conf/telemetry/default_fixedwing.xml index 81c04cfb01..63b8e5745f 100644 --- a/conf/telemetry/default_fixedwing.xml +++ b/conf/telemetry/default_fixedwing.xml @@ -20,7 +20,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -62,7 +62,7 @@ - + diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml index 06206b55cf..433b9965fa 100644 --- a/conf/telemetry/default_fixedwing_imu.xml +++ b/conf/telemetry/default_fixedwing_imu.xml @@ -20,7 +20,7 @@ - + @@ -48,7 +48,7 @@ - + @@ -63,7 +63,7 @@ - + @@ -90,7 +90,7 @@ - + diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml index 69ebd379b1..08b13142b3 100644 --- a/conf/telemetry/default_fixedwing_imu_9k6.xml +++ b/conf/telemetry/default_fixedwing_imu_9k6.xml @@ -20,7 +20,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -61,7 +61,7 @@ - + @@ -88,7 +88,7 @@ - + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index 73806655c8..e23e09bda3 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -20,6 +20,7 @@ + diff --git a/conf/telemetry/fixedwing_control_new.xml b/conf/telemetry/fixedwing_control_new.xml index d9bf211906..9b9bb0727c 100644 --- a/conf/telemetry/fixedwing_control_new.xml +++ b/conf/telemetry/fixedwing_control_new.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/telemetry/hitl_fixedwing.xml b/conf/telemetry/hitl_fixedwing.xml index 034ed1b653..93a8be09ed 100644 --- a/conf/telemetry/hitl_fixedwing.xml +++ b/conf/telemetry/hitl_fixedwing.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/telemetry/iridium.xml b/conf/telemetry/iridium.xml index 6ca164367e..5bd30117b8 100644 --- a/conf/telemetry/iridium.xml +++ b/conf/telemetry/iridium.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/telemetry/xbee868.xml b/conf/telemetry/xbee868.xml index 95e73ffaa2..9cee6119de 100644 --- a/conf/telemetry/xbee868.xml +++ b/conf/telemetry/xbee868.xml @@ -21,7 +21,7 @@ - + diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c index 224b040384..74894ec9c9 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.c +++ b/sw/airborne/firmwares/fixedwing/autopilot.c @@ -136,14 +136,6 @@ static void send_airspeed(struct transport_tx *trans __attribute__((unused)), st #endif } -static void send_downlink(struct transport_tx *trans, struct link_device *dev) { - static uint16_t last; - uint16_t rate = (downlink.nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; - last = downlink.nb_bytes; - pprz_msg_send_DOWNLINK(trans, dev, AC_ID, - &downlink.nb_ovrn, &rate, &downlink.nb_msgs); -} - void autopilot_init(void) { pprz_mode = PPRZ_MODE_AUTO2; kill_throttle = FALSE; @@ -163,7 +155,6 @@ void autopilot_init(void) { /* register some periodic message */ register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode); - register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink); register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator); register_periodic_telemetry(DefaultPeriodic, "AIRSPEED", send_airspeed); diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index c4fbbe041c..d83050baba 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -30,6 +30,32 @@ struct downlink downlink; +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +#include "subsystems/datalink/datalink.h" +#include "mcu_periph/sys_time.h" + +static void send_downlink(struct transport_tx *trans, struct link_device *dev) { + static uint32_t last_nb_bytes = 0; + // timestamp in usec when last message was send + static uint32_t last_ts = 0.; + // current timestamp + uint32_t now_ts = get_sys_time_msec(); + // compute downlink byte rate + if (now_ts > last_ts) { + uint16_t rate = (1000 * ((uint32_t)downlink.nb_bytes - last_nb_bytes)) / (now_ts - last_ts); + last_ts = now_ts; + last_nb_bytes = downlink.nb_bytes; + + // TODO uplink nb received msg + uint16_t uplink_nb_msgs = 0; + pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID, + &datalink_time, &uplink_nb_msgs, + &downlink.nb_msgs, &rate, &downlink.nb_ovrn); + } +} +#endif + void downlink_init(void) { downlink.nb_ovrn = 0; @@ -52,5 +78,8 @@ void downlink_init(void) ivy_transport_init(); #endif +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "DATALINK_REPORT", send_downlink); +#endif } diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c index 2e76554f7d..69dd85477d 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -150,6 +150,7 @@ static void end_message(struct ivy_transport *trans, struct link_device *dev __a *(--trans->ivy_p) = '\0'; if (trans->ivy_dl_enabled) { IvySendMsg("%s", trans->ivy_buf); + downlink.nb_msgs++; } } diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index df3390a42c..9425998709 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -841,23 +841,32 @@ let get_fbw_msg = fun alarm _sender vs -> log_and_say alarm ac.ac_name (sprintf "%s, mayday, AP Failure. Switch to manual." ac.ac_speech_name) end -let get_link_status_msg = fun alarm sender vs -> - let ac = find_ac sender in - let link_id = Pprz.int_assoc "link_id" vs in - let time_since_last_msg = Pprz.float_assoc "time_since_last_msg" vs in - let rx_msgs_rate = Pprz.float_assoc "rx_msgs_rate" vs in +let get_telemetry_status = fun alarm _sender vs -> + let ac = get_ac vs in + let link_id = Pprz.string_assoc "link_id" vs in + (* Update color and lost time in the strip *) + let time_lost = Pprz.float_assoc "time_since_last_msg" vs in + let (links_up, total_links) = ac.link_page#links_ratio () in + let link_ratio_string = + if ac.link_page#multiple_links () then sprintf "%i/%i" links_up total_links else "" in + ac.strip#set_label "telemetry_status" (if time_lost > 2. then sprintf "%.0f" time_lost else link_ratio_string); + ac.strip#set_color "telemetry_status" (if time_lost > 5. then alert_color else if links_up < total_links then warning_color else ok_color); + (* Update link page *) + let rx_msgs_rate = Pprz.float_assoc "rx_bytes_rate" vs + and downlink_bytes_rate = Pprz.int_assoc "downlink_rate" vs + and uplink_lost_time = Pprz.int_assoc "uplink_lost_time" vs in let ping_time = Pprz.float_assoc "ping_time" vs in if (not (ac.link_page#link_exists link_id)) then begin ac.link_page#add_link link_id; - log_and_say alarm ac.ac_name (sprintf "%s, new link detected: %i" ac.ac_speech_name link_id) + log_and_say alarm ac.ac_name (sprintf "%s, new link detected: %s" ac.ac_speech_name link_id) end; - let link_changed = ac.link_page#update_link link_id time_since_last_msg rx_msgs_rate ping_time in + let link_changed = ac.link_page#update_link link_id time_lost ping_time rx_msgs_rate downlink_bytes_rate uplink_lost_time in let (links_up, _) = ac.link_page#links_ratio () in match (link_changed, links_up) with (_, 0) -> log_and_say alarm ac.ac_name (sprintf "%s, all links lost" ac.ac_speech_name) - | (Pages.Linkup, _)-> log_and_say alarm ac.ac_name (sprintf "%s, link %i re-connected" ac.ac_speech_name link_id) + | (Pages.Linkup, _)-> log_and_say alarm ac.ac_name (sprintf "%s, link %s re-connected" ac.ac_speech_name link_id) | (Pages.Nochange, _) -> () - | (Pages.Linkdown, _) -> log_and_say alarm ac.ac_name (sprintf "%s, link %i lost" ac.ac_speech_name link_id) + | (Pages.Linkdown, _) -> log_and_say alarm ac.ac_name (sprintf "%s, link %s lost" ac.ac_speech_name link_id) let get_engine_status_msg = fun _sender vs -> let ac = get_ac vs in @@ -884,8 +893,8 @@ let listen_engine_status_msg = fun () -> let listen_if_calib_msg = fun () -> safe_bind "INFLIGH_CALIB" get_if_calib_msg -let listen_link_status_msg = fun a -> - tele_bind "LINK_STATUS" (get_link_status_msg a) +let listen_telemetry_status = fun a -> + safe_bind "TELEMETRY_STATUS" (get_telemetry_status a) let list_separator = Str.regexp "," @@ -1337,26 +1346,6 @@ let listen_svsinfo = fun a -> safe_bind "SVSINFO" (get_svsinfo a) let message_request = Ground_Pprz.message_req -let get_ts = fun _sender vs -> - let ac = get_ac vs in - let t = Pprz.float_assoc "time_since_last_msg" vs in - if ac.link_page#multiple_links () then - begin - let (links_up, total_links) = ac.link_page#links_ratio () in - let link_ratio_string = sprintf "%i/%i" links_up total_links in - ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.0f" t else link_ratio_string); - ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else if links_up < total_links then warning_color else ok_color) - end - else - begin - ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.0f" t else " "); - ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else ok_color) - end - -let listen_telemetry_status = fun () -> - safe_bind "TELEMETRY_STATUS" get_ts - - let mark_dcshot = fun (geomap:G.widget) _sender vs -> let ac = find_ac !active_ac in let photonumber = Pprz.string_assoc "photo_nr" vs in @@ -1419,11 +1408,10 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt listen_wind_msg geomap; listen_fbw_msg my_alert; listen_engine_status_msg (); - listen_link_status_msg my_alert; + listen_telemetry_status my_alert; listen_if_calib_msg (); listen_waypoint_moved (); listen_svsinfo my_alert; - listen_telemetry_status (); listen_alert my_alert; listen_error my_alert; listen_info_msg my_alert; diff --git a/sw/ground_segment/cockpit/pages.ml b/sw/ground_segment/cockpit/pages.ml index 2347abc1e5..07d37a432f 100644 --- a/sw/ground_segment/cockpit/pages.ml +++ b/sw/ground_segment/cockpit/pages.ml @@ -312,57 +312,73 @@ class link ?(visible = fun _ -> true) (widget: GBin.frame) = ignore (GMisc.label ~text: "Link id:" ~justify: `RIGHT ~packing: (table#attach ~top:0 ~left: 0) ()); ignore (GMisc.label ~text: "Status:" ~justify: `RIGHT ~packing: (table#attach ~top:1 ~left: 0) ()); ignore (GMisc.label ~text: "Ping time [ms]:" ~justify: `RIGHT ~packing: (table#attach ~top:2 ~left: 0) ()); - ignore (GMisc.label ~text: "Rx messages/s:" ~justify: `RIGHT ~packing: (table#attach ~top:3 ~left: 0) ()); + ignore (GMisc.label ~text: "Link Rx [Byte/s]:" ~justify: `RIGHT ~packing: (table#attach ~top:3 ~left: 0) ()); + ignore (GMisc.label ~text: "Downlink [Byte/s]:" ~justify: `RIGHT ~packing: (table#attach ~top:4 ~left: 0) ()); + ignore (GMisc.label ~text: "Uplink lost [s]:" ~justify: `RIGHT ~packing: (table#attach ~top:5 ~left: 0) ()); in object val table = table val mutable links = [] (* Stores the GUI elements that need to be updated and whether the link is connected or not*) val mutable links_up = 0 (* Stores the number of links that are connected*) - method link_exists link_id = - try - let link = List.assoc link_id links in - ignore link; - true - with - Not_found -> false + method link_exists link_id = List.mem_assoc link_id links method add_link link_id = - let number_of_links = List.length links in - let _ = GMisc.label ~text: (sprintf "%i" link_id) ~packing: (table#attach ~top:0 ~left: (number_of_links+1) ) () in - let link_status_eventbox = GBin.event_box ~width: 50 ~packing: (table#attach ~top:1 ~left: (number_of_links+1) ) () in - let link_status_label = GMisc.label ~text: " " ~packing: link_status_eventbox#add () in - let rx_msgs_rate_label = GMisc.label ~text: " " ~packing: (table#attach ~top:2 ~left: (number_of_links+1) ) () in - let ping_time_label = GMisc.label ~text: " " ~packing: (table#attach ~top:3 ~left: (number_of_links+1) ) () in - let up = true in - ignore (links <- (link_id, (up, link_status_eventbox, link_status_label, rx_msgs_rate_label, ping_time_label)) :: links); - links_up <- links_up + 1; + (* replace the no_id link if needed *) + if List.mem_assoc "no_id" links + then begin + let (_, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6) = List.assoc "no_id" links in + links <- (link_id, (true, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6)) :: (List.remove_assoc "no_id" links) + end + else begin + let number_of_links = List.length links in + let _ = GMisc.label ~text: (sprintf "%s" link_id) ~packing: (table#attach ~top:0 ~left: (number_of_links+1) ) () in + let link_status_eventbox = GBin.event_box ~width: 50 ~packing: (table#attach ~top:1 ~left: (number_of_links+1) ) () in + let link_status_label = GMisc.label ~text: " " ~packing: link_status_eventbox#add () in + let ping_time_label = GMisc.label ~text: " " ~packing: (table#attach ~top:2 ~left: (number_of_links+1) ) () in + let rx_bytes_rate_label = GMisc.label ~text: " " ~packing: (table#attach ~top:3 ~left: (number_of_links+1) ) () in + let downlink_bytes_rate_label = GMisc.label ~text: " " ~packing: (table#attach ~top:4 ~left: (number_of_links+1) ) () in + let uplink_lost_label = GMisc.label ~text: " " ~packing: (table#attach ~top:5 ~left: (number_of_links+1) ) () in + let up = true in + ignore (links <- (link_id, (up, link_status_eventbox, link_status_label, ping_time_label, rx_bytes_rate_label, downlink_bytes_rate_label, uplink_lost_label)) :: links); + links_up <- links_up + 1; + end - method update_link link_id time_since_last_msg rx_msgs_rate ping_time = - let (up, link_status_event_box, link_status_label, rx_msgs_rate_label, ping_time_label) = List.assoc link_id links in + method update_link link_id time_since_last_msg ping_time rx_bytes_rate downlink_bytes_rate uplink_lost_time = + let (up, link_status_event_box, link_status_label, ping_time_label, rx_bytes_rate_label, downlink_bytes_rate_label, uplink_lost_label) = List.assoc link_id links in begin if visible widget then begin (* display only if page is visible *) let link_status_string = sprintf "%.0f" time_since_last_msg in if link_status_label#text <> link_status_string then (* Updating the link status light*) begin link_status_label#set_label (if time_since_last_msg > 2. then link_status_string else " "); - let color = (if time_since_last_msg > 5. then "red" else "green") in + let color = (if time_since_last_msg > 5. then "red" else if uplink_lost_time > 10 then "orange" else "green") in link_status_event_box#coerce#misc#modify_bg [`NORMAL, `NAME color]; end; - let rx_msgs_rate_string = sprintf "%.1f" rx_msgs_rate in - if rx_msgs_rate_label#text <> rx_msgs_rate_string then (* Updating the rx_msgs_rate field*) - rx_msgs_rate_label#set_label rx_msgs_rate_string; - let ping_time_string = sprintf "%.1f" ping_time in if ping_time_label#text <> ping_time_string then (* Updating the ping_time field*) ping_time_label#set_label ping_time_string; + + let rx_bytes_rate_string = sprintf "%.1f" rx_bytes_rate in + if rx_bytes_rate_label#text <> rx_bytes_rate_string then (* Updating the rx_bytes_rate field*) + rx_bytes_rate_label#set_label rx_bytes_rate_string; + + let downlink_bytes_rate_string = sprintf "%d" downlink_bytes_rate in + if downlink_bytes_rate_label#text <> downlink_bytes_rate_string then (* Updating the downlink_bytes_rate field*) + downlink_bytes_rate_label#set_label downlink_bytes_rate_string; + + let uplink_lost_string = sprintf "%d" uplink_lost_time in + if uplink_lost_label#text <> uplink_lost_string then (* Updating the uplink_lost_time field*) + uplink_lost_label#set_label uplink_lost_string; + + () end; let update_list = fun list_to_update up -> - let (_, dummy1, dummy2, dummy3, dummy4) = List.assoc link_id list_to_update in - (link_id, (up, dummy1, dummy2, dummy3, dummy4)) :: List.remove_assoc link_id list_to_update in + let (_, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6) = List.assoc link_id list_to_update in + (link_id, (up, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6)) :: List.remove_assoc link_id list_to_update in if up then (* Updating the stored value of whether this link is connected or not*) if time_since_last_msg > 5. then begin @@ -387,6 +403,6 @@ class link ?(visible = fun _ -> true) (widget: GBin.frame) = (links_up, List.length links) method multiple_links () = - if (List.length links) > 0 then true else false + (List.length links) > 1 end;; diff --git a/sw/ground_segment/cockpit/pages.mli b/sw/ground_segment/cockpit/pages.mli index b5a112fa7a..63f72d9f52 100644 --- a/sw/ground_segment/cockpit/pages.mli +++ b/sw/ground_segment/cockpit/pages.mli @@ -64,9 +64,9 @@ class rc_settings : type link_change = Linkup | Nochange | Linkdown class link : ?visible:(GBin.frame -> bool) -> GBin.frame -> object - method link_exists : int -> bool - method add_link : int -> unit - method update_link : int -> float -> float -> float -> link_change + method link_exists : string -> bool + method add_link : string -> unit + method update_link : string -> float -> float -> float -> int -> int -> link_change method links_ratio : unit -> (int * int) method multiple_links : unit -> bool end diff --git a/sw/ground_segment/python/redundant_link/link_combiner.py b/sw/ground_segment/python/redundant_link/link_combiner.py index 4256d4fec2..5343d05615 100755 --- a/sw/ground_segment/python/redundant_link/link_combiner.py +++ b/sw/ground_segment/python/redundant_link/link_combiner.py @@ -27,8 +27,6 @@ import logging import sys import os import argparse -from time import time -import threading from ivy.std_api import * PPRZ_HOME = os.getenv("PAPARAZZI_HOME") @@ -121,20 +119,9 @@ class Link: def __init__(self, name, ac_id, buffer_size=10, verbose=0): self.buffer = Circular_Buffer(buffer_size) self.name = name - self.time_of_last_message = time() self.verbose = verbose self.acs = [ac_id] #Storing a list of the aircrafts that use this link. Usually it's just one. - # The following are stored values from the DOWNLINK_STATUS message: - self.run_time = 0 - self.rx_bytes = 0 - self.rx_msgs = 0 - self.rx_err = 0 - self.rx_bytes_rate = 0 - self.rx_msgs_rate = 0 - self.ping_time = 0 - - def checkBuffer(self,message): return self.buffer.contains(message.message()) @@ -147,49 +134,12 @@ class Link: def removeFromBuffer(self,message): self.buffer.remove(message.message()) - def updateTimeOfLastMessage(self): - self.time_of_last_message = time() - - def timeSinceLastMessage(self): - return time() - self.time_of_last_message - def acAc(self, ac_id): self.acs = self.acs + [ac_id] def aircrafts(self): return self.acs - def sendLinkStatusMessage(self): - for ac_id in self.acs: - values = ( self.name, - self.timeSinceLastMessage(), - self.run_time, - self.rx_bytes, - self.rx_msgs, - self.rx_err, - self.rx_bytes_rate, - self.rx_msgs_rate, - self.ping_time) - - IvySendMsg("%s LINK_STATUS %s %f %s %s %s %s %s %s %s" % ((ac_id,) + values)) - threading.Timer(LINK_STATUS_PERIOD, self.sendLinkStatusMessage).start() - - def updateStatus(self, downlink_status_message): - - if downlink_status_message.name() != "DOWNLINK_STATUS": - raise(Exception("function called with message of name other than DOWNLINK_STATUS")) - - message_values = downlink_status_message.values() - - self.run_time = message_values['run_time'] - self.rx_bytes = message_values['rx_bytes'] - self.rx_msgs = message_values['rx_msgs'] - self.rx_err = message_values['rx_err'] - self.rx_bytes_rate = message_values['rx_bytes_rate'] - self.rx_msgs_rate = message_values['rx_msgs_rate'] - self.ping_time = message_values['ping_time'] - - class Link_Combiner: @@ -221,16 +171,11 @@ class Link_Combiner: if message.linkName() not in self.links: #Adding a new link self.links[message.linkName()] = Link(message.linkName(), message.sender(), BUFFER_SIZE) # print("NEW LINK DETECTED: %s" %message.linkName(), file=sys.stderr) - self.repeatSendLinkStatusMessage(message) #Processing messages from an already added link link = self.links[message.linkName()] self.sendMessage(message) self.bufferMessage(message) - if message.name() != "DOWNLINK_STATUS": - link.updateTimeOfLastMessage() - else: - link.updateStatus(message) if message.sender() not in link.aircrafts(): link.addAc(message.sender()) @@ -273,11 +218,6 @@ class Link_Combiner: def bufferMessage(self, message): self.links[message.linkName()].addToBuffer(message) - def repeatSendLinkStatusMessage(self, message): - link_name = message.linkName() - self.links[link_name].sendLinkStatusMessage() - - def main(): messages_xml_map.ParseMessages() @@ -286,17 +226,14 @@ def main(): #Command line options parser = argparse.ArgumentParser(description="Link_Combiner listens to the ivy messages received from multiple Link agents (set each of their -id options to a unique number), and sends a combined stream of messages to the other agents.") parser.add_argument("-b", "-buffer_size", "--buffer_size", help="The number of elements messages to be stored in the circular buffer for each link", default=10) - parser.add_argument("-t", "-link_status_period", "--link_status_period", help="The number of miliseconds in between LINK_STATUS messages being sent to the GCS", default=1000) args = parser.parse_args() global BUFFER_SIZE - global LINK_STATUS_PERIOD BUFFER_SIZE = int(args.buffer_size) #The number of elements messages to be stored in the circular buffer for each link. - LINK_STATUS_PERIOD = float(args.link_status_period)/1000 #The number of seconds in between LINK_STATUS messages being sent to the GCS. - link_combiner = Link_Combiner() if __name__ == '__main__': main() + diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml index 334b2961c3..2d854e2992 100644 --- a/sw/ground_segment/tmtc/aircraft.ml +++ b/sw/ground_segment/tmtc/aircraft.ml @@ -59,6 +59,38 @@ let svinfo_init = fun () -> age = 0 } +type datalink_status = { + mutable uplink_lost_time : int; + mutable uplink_msgs : int; + mutable downlink_msgs : int; + mutable downlink_rate : int; + } +type link_status = { + rx_lost_time : int; + rx_bytes : int; + rx_msgs : int; + rx_bytes_rate : float; + tx_msgs : int; + ping_time : float + } + +let datalink_status_init = fun () -> + { + uplink_lost_time = 9999; + uplink_msgs = 0; + downlink_msgs = 0; + downlink_rate = 0; + } +let link_status_init = fun () -> + { + rx_lost_time = 9999; + rx_bytes = 0; + rx_msgs = 0; + rx_bytes_rate = 0.; + tx_msgs = 0; + ping_time = 9999. + } + type inflight_calib = { mutable if_mode : int; (* DOWN|OFF|UP *) mutable if_val1 : float; @@ -155,6 +187,8 @@ type aircraft = { dl_setting_values : float array; mutable nb_dl_setting_values : int; mutable survey : (Latlong.geographic * Latlong.geographic) option; + datalink_status : datalink_status; + link_status : (int, link_status) Hashtbl.t; mutable last_msg_date : float; mutable time_since_last_survey_msg : float; mutable dist_to_wp : float; @@ -188,6 +222,7 @@ let new_aircraft = fun id name fp airframe -> horiz_mode = UnknownHorizMode; horizontal_mode = 0; waypoints = Hashtbl.create 3; survey = None; last_msg_date = 0.; dist_to_wp = 0.; + datalink_status = datalink_status_init (); link_status = Hashtbl.create 1; time_since_last_survey_msg = 1729.; inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.} } diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli index 36528997df..bda45823c1 100644 --- a/sw/ground_segment/tmtc/aircraft.mli +++ b/sw/ground_segment/tmtc/aircraft.mli @@ -48,6 +48,24 @@ type svinfo = { mutable age : int } val svinfo_init : unit -> svinfo + +type datalink_status = { + mutable uplink_lost_time : int; + mutable uplink_msgs : int; + mutable downlink_msgs : int; + mutable downlink_rate : int; + } +type link_status = { + rx_lost_time : int; + rx_bytes : int; + rx_msgs : int; + rx_bytes_rate : float; + tx_msgs : int; + ping_time : float + } +val datalink_status_init : unit -> datalink_status +val link_status_init : unit -> link_status + type horiz_mode = Circle of Latlong.geographic * int | Segment of Latlong.geographic * Latlong.geographic @@ -121,6 +139,8 @@ type aircraft = { dl_setting_values : float array; mutable nb_dl_setting_values : int; mutable survey : (Latlong.geographic * Latlong.geographic) option; + datalink_status : datalink_status; + link_status : (int, link_status) Hashtbl.t; mutable last_msg_date : float; mutable time_since_last_survey_msg : float; mutable dist_to_wp : float; diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml index b65be589ab..ac639a5b7e 100644 --- a/sw/ground_segment/tmtc/fw_server.ml +++ b/sw/ground_segment/tmtc/fw_server.ml @@ -350,5 +350,10 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> "resolve", Pprz.Int (ivalue "resolve") ] in Dl_Pprz.message_send "ground_dl" "TCAS_RESOLVE" vs + | "DATALINK_REPORT" -> + a.datalink_status.uplink_lost_time <- ivalue "uplink_lost_time"; + a.datalink_status.uplink_msgs <- ivalue "uplink_nb_msgs"; + a.datalink_status.downlink_rate <- ivalue "downlink_rate"; + a.datalink_status.downlink_msgs <- ivalue "downlink_nb_msgs" | _ -> () diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index 4ccacfc1c3..6198009b10 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -79,6 +79,13 @@ let send_message_over_ivy = fun sender name vs -> else Tm_Pprz.message_send ?timestamp sender name vs +let send_ground_over_ivy = fun sender name vs -> + let timestamp = + match !add_timestamp with + None -> None + | Some start_time -> Some (Unix.gettimeofday () -. start_time) in + Ground_Pprz.message_send ?timestamp sender name vs + (*********** Monitoring *************************************************) type status = { @@ -87,6 +94,7 @@ type status = { mutable rx_byte : int; mutable rx_msg : int; mutable rx_err : int; + mutable tx_msg : int; mutable ms_since_last_msg : int; mutable last_ping : float; (* s *) mutable last_pong : float; (* s *) @@ -99,6 +107,7 @@ let dead_aircraft_time_ms = 5000 let initial_status = { last_rx_byte = 0; last_rx_msg = 0; rx_byte = 0; rx_msg = 0; rx_err = 0; + tx_msg = 0; ms_since_last_msg = dead_aircraft_time_ms; last_ping = 0.; last_pong = 0.; udp_peername = None @@ -153,16 +162,19 @@ let send_status_msg = status.last_rx_msg <- status.rx_msg; status.last_rx_byte <- status.rx_byte; status.ms_since_last_msg <- status.ms_since_last_msg + status_msg_period; - let vs = ["link_id", Pprz.Int !link_id; + let vs = ["ac_id", Pprz.Int ac_id; + "link_id", Pprz.Int !link_id; "run_time", Pprz.Int t; - "rx_bytes_rate", Pprz.Float byte_rate; - "rx_msgs_rate", Pprz.Float msg_rate; - "rx_err", Pprz.Int status.rx_err; + "rx_lost_time", Pprz.Int (1000 * status.ms_since_last_msg); "rx_bytes", Pprz.Int status.rx_byte; "rx_msgs", Pprz.Int status.rx_msg; + "rx_err", Pprz.Int status.rx_err; + "rx_bytes_rate", Pprz.Float byte_rate; + "rx_msgs_rate", Pprz.Float msg_rate; + "tx_msgs", Pprz.Int 0; "ping_time", Pprz.Float (1000. *. (status.last_pong -. status.last_ping)) ] in - send_message_over_ivy (string_of_int ac_id) "DOWNLINK_STATUS" vs) + send_ground_over_ivy "link" "LINK_REPORT" vs) statuss @@ -304,6 +316,11 @@ let udp_send = fun fd payload peername -> let send = fun ac_id device payload _priority -> Debug.call 's' (fun f -> fprintf f "%d\n" ac_id); if live_aircraft ac_id then + let _ = try + let s = Hashtbl.find statuss ac_id in + s.tx_msg <- s.tx_msg + 1; + () + with Not_found -> () in match udp_peername ac_id with Some (Unix.ADDR_INET (peername, _port)) -> udp_send device.fd payload peername @@ -319,6 +336,7 @@ let send = fun ac_id device payload _priority -> let broadcast = fun device payload _priority -> + Hashtbl.iter (fun _ s -> s.tx_msg <- s.tx_msg + 1) statuss; if !udp then Hashtbl.iter (* Sending to all alive A/C *) (fun ac_id status -> diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 1ecee825fc..a184cc612e 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -277,13 +277,35 @@ let send_wind = fun a -> let send_telemetry_status = fun a -> let id = a.id in - try - let vs = - ["ac_id", Pprz.String id; - "time_since_last_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date)] in - Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs - with - _exc -> () + let tl_payload = fun link_id datalink_status link_status -> + [ "ac_id", Pprz.String id; + "link_id", Pprz.String link_id; + "time_since_last_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date); (* don't use rx_lost_time from LINK_REPORT so it also works in simulation *) + "rx_bytes", Pprz.Int link_status.rx_bytes; + "rx_msgs", Pprz.Int link_status.rx_msgs; + "rx_bytes_rate", Pprz.Float link_status.rx_bytes_rate; + "tx_msgs", Pprz.Int link_status.tx_msgs; + "uplink_lost_time", Pprz.Int datalink_status.uplink_lost_time; + "uplink_msgs", Pprz.Int datalink_status.uplink_msgs; + "downlink_msgs", Pprz.Int datalink_status.downlink_msgs; + "downlink_rate", Pprz.Int datalink_status.downlink_rate; + "ping_time", Pprz.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.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 @@ -674,7 +696,7 @@ let jump_block = fun logging _sender vs -> Dl_Pprz.message_send dl_id "BLOCK" vs; log logging ac_id "BLOCK" vs -(** Got a RAW_DATALINK,send its contents *) +(** Got a RAW_DATALINK, send its contents *) let raw_datalink = fun logging _sender vs -> let ac_id = Pprz.string_assoc "ac_id" vs and m = Pprz.string_assoc "message" vs in @@ -686,6 +708,24 @@ let raw_datalink = fun logging _sender vs -> Dl_Pprz.message_send dl_id msg.Pprz.name vs; log logging ac_id msg.Pprz.name vs +(** Got a LINK_REPORT, update state but don't send (done asynchronously) *) +let link_report = fun logging _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs + and link_id = Pprz.int_assoc "link_id" vs in + try + let ac = Hashtbl.find aircrafts ac_id in + let link_status = { + Aircraft.rx_lost_time = Pprz.int_assoc "rx_lost_time" vs; + rx_bytes = Pprz.int_assoc "rx_bytes" vs; + rx_msgs = Pprz.int_assoc "rx_msgs" vs; + rx_bytes_rate = Pprz.float_assoc "rx_bytes_rate" vs; + tx_msgs = Pprz.int_assoc "tx_msgs" vs; + ping_time = Pprz.float_assoc "ping_time" vs; + } in + Hashtbl.replace ac.link_status link_id link_status; + 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 -> @@ -694,7 +734,8 @@ let ground_to_uplink = fun logging -> 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 "RAW_DATALINK" raw_datalink; + bind_log_and_send "LINK_REPORT" link_report (* main loop *)