diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example index ac01febe42..876729f380 100644 --- a/conf/control_panel.xml.example +++ b/conf/control_panel.xml.example @@ -20,6 +20,8 @@ + + @@ -121,6 +123,23 @@ + + + + + + + + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index 70ff3c47df..d6c35e4e68 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -151,6 +151,7 @@ + @@ -558,7 +559,20 @@ - + + + + + + + + + + + + + + @@ -2461,6 +2475,18 @@ + + + + + + + + + + + + diff --git a/sw/ground_segment/cockpit/gcs.glade b/sw/ground_segment/cockpit/gcs.glade index cbef867e5c..7f00cf86ed 100644 --- a/sw/ground_segment/cockpit/gcs.glade +++ b/sw/ground_segment/cockpit/gcs.glade @@ -213,7 +213,7 @@ True - Telemetry status: seconds since the last battery level message + Telemetry status: ratio of connected links to number of links (if multiple links used) or seconds since the last message (if link lost) True diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 635874db3f..1f9583551f 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -83,6 +83,7 @@ type aircraft = { ir_page : Pages.infrared; gps_page : Pages.gps; pfd_page : Horizon.pfd; + link_page : Pages.link; misc_page : Pages.misc; dl_settings_page : Page_settings.settings option; rc_settings_page : Pages.rc_settings option; @@ -117,7 +118,7 @@ let find_ac = fun ac_id -> let active_ac = ref "" let get_ac = fun vs -> let ac_id = Pprz.string_assoc "ac_id" vs in - find_ac ac_id + find_ac ac_id let show_fp = fun ac -> ac.fp_group#show (); @@ -573,6 +574,11 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let pfd_page = new Horizon.pfd pfd_frame and _pfd_page_num = ac_notebook#page_num pfd_frame#coerce in + let link_label = GMisc.label ~text: "Link" () in + let link_frame = GBin.frame ~shadow_type: `NONE () in + ignore (ac_notebook#append_page ~tab_label: link_label#coerce link_frame#coerce); + let link_page = new Pages.link link_frame in + let misc_label = GMisc.label ~text: "Misc" () in let misc_frame = GBin.frame ~shadow_type: `NONE () in ignore (ac_notebook#append_page ~tab_label:misc_label#coerce misc_frame#coerce); @@ -647,6 +653,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id ir_page = ir_page; flight_time = 0; gps_page = gps_page; pfd_page = pfd_page; + link_page = link_page; misc_page = misc_page; dl_settings_page = dl_settings_page; rc_settings_page = rc_settings_page; @@ -818,8 +825,24 @@ 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 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) + end; + let link_changed = ac.link_page#update_link link_id time_since_last_msg rx_msgs_rate ping_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.Nochange, _) -> () + | (Pages.Linkdown, _) -> log_and_say alarm ac.ac_name (sprintf "%s, link %i lost" ac.ac_speech_name link_id) + let get_engine_status_msg = fun _sender vs -> let ac = get_ac vs in ac.strip#set_throttle ~kill:ac.in_kill_mode (Pprz.float_assoc "throttle" vs); @@ -845,6 +868,9 @@ 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 list_separator = Str.regexp "," let aircrafts_msg = fun alert (geomap:G.widget) fp_notebook acs -> @@ -1289,9 +1315,18 @@ 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 - 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) - + 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 @@ -1352,6 +1387,7 @@ 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_if_calib_msg (); listen_waypoint_moved (); listen_svsinfo my_alert; diff --git a/sw/ground_segment/cockpit/live.mli b/sw/ground_segment/cockpit/live.mli index 0eb6676191..53ed41711e 100644 --- a/sw/ground_segment/cockpit/live.mli +++ b/sw/ground_segment/cockpit/live.mli @@ -40,6 +40,7 @@ type aircraft = private { ir_page : Pages.infrared; gps_page : Pages.gps; pfd_page : Horizon.pfd; + link_page : Pages.link; misc_page : Pages.misc; dl_settings_page : Page_settings.settings option; rc_settings_page : Pages.rc_settings option; diff --git a/sw/ground_segment/cockpit/pages.ml b/sw/ground_segment/cockpit/pages.ml index 9cfbc93dea..cfc04b16da 100644 --- a/sw/ground_segment/cockpit/pages.ml +++ b/sw/ground_segment/cockpit/pages.ml @@ -1,5 +1,5 @@ (* - * Copyright (C) 2006 ENAC, Pierre-Sélim Huard, Pascal Brisset, Antoine Drouin + * Copyright (C) 2006 ENAC, Pierre-Sélim Huard, Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. * @@ -293,3 +293,96 @@ class rc_settings = fun ?(visible = fun _ -> true) xmls -> values.(i).(j).(1)#set_text s2 end +(*****************************************************************************) +(* link page *) +(*****************************************************************************) +type link_change = Linkup | Nochange | Linkdown + +class link ?(visible = fun _ -> true) (widget: GBin.frame) = + let scrolled = GBin.scrolled_window + ~hpolicy: `AUTOMATIC + ~vpolicy: `AUTOMATIC + ~packing: widget#add + () + in + let table = GPack.table ~rows:1 ~columns:1 ~row_spacings:5 ~col_spacings:10 ~packing:scrolled#add_with_viewport () in + let _init = + 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) ()); + 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 add_link link_id = + let number_of_links = List.length links in + let link_id_label = 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; + + + 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 + begin + 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 + 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 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 + if up then (* Updating the stored value of whether this link is connected or not*) + if time_since_last_msg > 5. then + begin + links <- update_list links false; + links_up <- links_up -1; + Linkdown; + end + else + Nochange + else + if time_since_last_msg < 5. then + begin + links <- update_list links true; + links_up <- links_up + 1; + Linkup + end + else + Nochange + end + + method links_ratio () = + (links_up, List.length links) + + method multiple_links () = + if (List.length links) > 0 then true else false + + end;; diff --git a/sw/ground_segment/cockpit/pages.mli b/sw/ground_segment/cockpit/pages.mli index d383a4c8fc..b5a112fa7a 100644 --- a/sw/ground_segment/cockpit/pages.mli +++ b/sw/ground_segment/cockpit/pages.mli @@ -61,3 +61,13 @@ class rc_settings : method widget : GObj.widget end +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 links_ratio : unit -> (int * int) + method multiple_links : unit -> bool + end + diff --git a/sw/ground_segment/python/redundant_link/README b/sw/ground_segment/python/redundant_link/README new file mode 100644 index 0000000000..10bc0178a7 --- /dev/null +++ b/sw/ground_segment/python/redundant_link/README @@ -0,0 +1,28 @@ +Created by Cameron Lee (cwlee1@ualberta.ca) + +This software enables multiple downlinks in order to provide redundancy in the plane to ground communication (it does not change the ground to plane communication at all). If the same data is sent by the autopilot over two or more radio links, then this software lets these two streams of data be combined into one stream. The resultant string will have all of information transmitted by the autopilot as long as at least one link receives the data. The status of each link can be monitored in the GCS in the LINK tab of the notebook. Various types of links can be used, for example: +- 900 MHz modem +- 2.4 GHz modem +- UDP packets sent over a Wi-Fi network +- Modulated audio signal in a video feed +- Satellite modem + + +TO USE: +1. First, implement the hardware to have multiple independant streams of data sent to the ground station computer. +2. In Paparazzi Center, run an instance of the Link agent for each stream of data. Configure each Link agent according to it's stream. Also, use the -id flag to give each link a unique id (an integer number) and the -redlink flag to tell the link it should be a redundant link. +3. Run the Link Combiner agent. + +For example, the following agents run in Paparazzi center will allow 2 links to be used, each of which is connected to it's own USB serial port: + +sw/ground_segment/tmtc/server +sw/ground_segment/cockpit/gcs +sw/ground_segment/tmtc/link -d /dev/ttyUSB0 -id 1 -redlink +sw/ground_segment/tmtc/link -d /dev/ttyUSB1 -id 2 -redlink +sw/ground_segment/python/redundant_link/link_combiner.py + + +HOW IT WORKS: +When the link agent is run with the -redlink flag set, instead of transmitting the data it receives over the ivy bus like normal, it encapsulates it in a TELEMETRY_MESSAGE message which also contains the link id. The Link Combiner listens to these messages from each link and sends data over the ivy bus to the other agents as if it was a link. The Link Combiner also sends the LINK_STATUS message so that the GCS can display the status of each link. + +The Link Combiner uses an algorithm to filter out duplicate messages. In other words, if a message is sent by the autopilot over both links and it is received by both links, then it's the same message and should only be handled once by other agents such as the GCS. The Link Combiner's algorithm therefore ignores a message received over any link if it's identical to a message received by another link. This is achieved by keeping a buffer of the last N messages for each link. Once a message has been received by all links, it's removed from the buffer. Also, the buffer is circular, so even if a message isn't received by all links, it will be overwritten after N more messages are received. This algorithm isn't guaranteed to be perfect, but in typical operation, it seems to work very well. And for the application of displaying aircraft data, some missing or duplicate data is acceptable. diff --git a/sw/ground_segment/python/redundant_link/link_combiner.py b/sw/ground_segment/python/redundant_link/link_combiner.py new file mode 100755 index 0000000000..a3927edf72 --- /dev/null +++ b/sw/ground_segment/python/redundant_link/link_combiner.py @@ -0,0 +1,302 @@ +#!/usr/bin/env python + +# 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. + +"""This program listens to ivy messages from the link agent (see link.ml) +when the link's -name arguement is set. It combines messages received from +any number of link agents and sends ivy messages to the Server and other +agents.""" + +from __future__ import print_function +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") +sys.path.append(PPRZ_HOME + "/sw/lib/python") +import messages_xml_map + +class Circular_Buffer: + def __init__(self, size): + self.buffer = [""]*size + self.index = 0 + self.size = size + + def add(self, contents): + self.buffer[self.index] = contents + self.incr() + + def contains(self, contents): + counter = self.index + while 1: + if self.buffer[counter] == contents: + return 1 + else: + if counter <= 0: + counter = self.size-1 #Moving the counter in the reverse direction of the index in order to test the most recent contents first (since they're most likely to match) + else: + counter -= 1 + if counter == self.index: + return 0 + + def incr(self): + if self.index >= (self.size - 1): + self.index = 0 + else: + self.index += 1 + + def remove(self, contents): + counter = self.index + while 1: + if self.buffer[counter] == contents: + self.buffer[counter] = "" + else: + if counter <= 0: + counter = self.size-1 + else: + counter -= 1 + if counter == self.index: + return + + def displayContents(self): + for counter in xrange(0,self.size): + if self.index != counter: + print(" %s" %self.buffer[counter], file=sys.stderr) + else: + print("-> %s" %self.buffer[counter], file=sys.stderr) + +class Message: + def __init__(self, sender, link_name, raw_message): + + self.link_name = link_name + self.raw_sender = sender + self.raw_message = " ".join(raw_message.split(";")) + + if self.name() not in messages_xml_map.message_dictionary['telemetry']: + raise(Exception("Error in link_combiner: unknown message name: %s" %self.name())) + + value_names = messages_xml_map.message_dictionary['telemetry'][self.name()] + values = self.raw_message.split(" ")[1:-1] + self.raw_values = {} + map(lambda k, v: self.raw_values.update({k: v}), value_names, values) + # self.raw_values = dict(zip(value_names, values)) #This doesn't work for some reason. It gives weird errors, seemingly related to the ivy bus + + def linkName(self): + return self.link_name + + def message(self): + return self.raw_message + + def sender(self): + return self.raw_sender + + def name(self): + return self.raw_message.split(" ")[1] + + def values(self): + return self.raw_values + + + +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()) + + def addToBuffer(self,message): + self.buffer.add(message.message()) + if self.verbose: + print("%s Buffer:" % self.name, file=sys.stderr) + self.buffer.displayContents(); + + 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: + + def __init__(self): + + self.links = {} + + self.initIvy() + + def initIvy(self): + # initialising the bus + IvyInit("Link_Combiner", # application name for Ivy + "READY", # ready message + 0, # main loop is local (ie. using IvyMainloop) + lambda x,y: y, # handler called on connection/deconnection + lambda x,y: y # handler called when a diemessage is received + ) + + # starting the bus + logging.getLogger('Ivy').setLevel(logging.WARN) + IvyStart("") + IvyBindMsg(self.onIvyMessage, "^([^ ]+) TELEMETRY_MESSAGE ([^ ]+) ([^ ]+) ([^ ]+)$") + + def onIvyMessage(self, agent, *larg): + + message = Message(larg[1], larg[2], larg[3]) + + 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()) + + + def sendMessage(self, message): + + in_buffer = self.checkBuffers(message) + if not in_buffer: + IvySendMsg(message.message()) + return True + else: + return False + + def checkBuffers(self, message): + #The returned value is the best guess at whether the message is a duplicate (True), or not (False). + #If the message is already in this link's buffer, then taking it as not a duplicate. So returning False. But also, removing it from all buffers. So that when they receive it, they don't do the same. + #If the message is not in this link's buffer, then checking all other buffers and only if it's not in any of them, counting the message as not a duplicate. + + match = self.links[message.linkName()].checkBuffer(message) + if match: #Removing the message from all buffers + for link_name in self.links: + self.links[link_name].removeFromBuffer(message) + return False + else: #Checking all other links' buffers + for link_name in self.links: + if link_name == message.linkName(): + continue + else: + match = self.links[link_name].checkBuffer(message) + if match: + return True + return False + + if match_count == 0: + return False + elif match_count == length(self.links): + for link_name in self.links: + self.links[link_name].removeFromBuffer(message) + + 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() + + + #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() \ No newline at end of file diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index d80d8b8490..4f74518199 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -21,7 +21,7 @@ *) (** Agent connecting a hardware modem, usually through USB/serial, with - the Ivy sowtware bus. + the Ivy software bus. *) open Latlong @@ -40,7 +40,7 @@ type transport = | Pprz2 (* Paparazzi protocol, with timestamp, A/C id, message id and CRC *) | XBee (* Maxstream protocol, API mode *) let transport_of_string = function -"pprz" -> Pprz + "pprz" -> Pprz | "pprz2" -> Pprz2 | "xbee" -> XBee | x -> invalid_arg (sprintf "transport_of_string: %s" x) @@ -53,6 +53,10 @@ type ground_device = { (* We assume here a single modem is used *) let my_id = 0 +(* Here we set the default id of the link*) +let link_id = ref 0 +let red_link = ref false + (* enable broadcast messages by default *) let ac_info = ref true @@ -70,7 +74,10 @@ let send_message_over_ivy = fun sender name vs -> match !add_timestamp with None -> None | Some start_time -> Some (Unix.gettimeofday () -. start_time) in - Tm_Pprz.message_send ?timestamp sender name vs + if !red_link then + Tm_Pprz.message_send ?timestamp ~link_id:!link_id sender name vs + else + Tm_Pprz.message_send ?timestamp sender name vs (*********** Monitoring *************************************************) @@ -146,7 +153,8 @@ 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 = ["run_time", Pprz.Int t; + let vs = ["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; @@ -453,6 +461,8 @@ let () = "-xbee_addr", Arg.Set_int XB.my_addr, (sprintf " (%d)" !XB.my_addr); "-xbee_retries", Arg.Set_int XB.my_addr, (sprintf " (%d)" !XB.nb_retries); "-xbee_868", Arg.Set Xbee.mode868, (sprintf "Enables the 868 protocol"); + "-redlink", Arg.Set red_link, (sprintf "Sets whether the link is a redundant link. Set this flag and the id flag to use multiple links"); + "-id", Arg.Set_int link_id, (sprintf "Sets the link id. If multiple links are used, each must have a unique id. Default is %i" !link_id) ] in Arg.parse options (fun _x -> ()) "Usage: "; @@ -460,6 +470,9 @@ let () = Ivy.init "Link" "READY" (fun _ _ -> ()); Ivy.start !ivy_bus; + if (!link_id <> 0) && (not !red_link) then + fprintf stderr "\nLINK WARNING: The link id was set to %i but the -redlink flag wasn't set. To use this link as a redundant link, set the -redlink flag.%!" !link_id; + try let transport = transport_of_string !transport in diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 0578816334..bab82a1fc5 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -485,10 +485,10 @@ module type MESSAGES = sig val string_of_message : ?sep:string -> message -> values -> string (** [string_of_message ?sep msg values] Default [sep] is space *) - val message_send : ?timestamp:float -> string -> string -> values -> unit - (** [message_send sender msg_name values] *) + 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 -> string -> (string -> values -> unit) -> Ivy.binding (** [message_bind ?sender msg_name callback] *) val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding @@ -530,7 +530,7 @@ module MessagesOfXml(Class:CLASS_Xml) = struct if index = String.length buffer then [] else - failwith (sprintf "Pprz.values_of_payload, too many bytes: %s" (Debug.xprint buffer)) + failwith (sprintf "Pprz.values_of_payload, too many bytes in message %s: %s" message.name (Debug.xprint buffer)) | (field_name, field_descr)::fs -> let (value, n) = value_field buffer index field_descr in (field_name, value) :: loop (index+n) fs in @@ -597,7 +597,7 @@ module MessagesOfXml(Class:CLASS_Xml) = struct formatted_string_of_value field.fformat v) msg.fields) - let message_send = fun ?timestamp sender msg_name values -> + let message_send = fun ?timestamp ?link_id sender msg_name values -> let m = snd (message_of_name msg_name) in let s = string_of_message m values in let timestamp_string = @@ -609,8 +609,22 @@ module MessagesOfXml(Class:CLASS_Xml) = struct if n > 1000 then (** FIXME: to prevent Ivy bug on long message *) fprintf stderr "Discarding long ivy message (%d bytes)\n%!" n else - Ivy.send msg - + match link_id with + None -> Ivy.send msg + | Some the_link_id -> begin + let index = ref 0 in + let modified_msg = String.copy msg in + let func = fun c -> + match c with + ' ' -> begin + String.set modified_msg !index ';'; + index := !index + 1 + end + | x -> index := !index + 1; in + String.iter func modified_msg; + 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 -> match sender with None -> diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index bec87a2546..508b927593 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -162,7 +162,7 @@ module type MESSAGES = sig val string_of_message : ?sep:string -> message -> values -> string (** [string_of_message ?sep msg values] Default [sep] is space *) - val message_send : ?timestamp:float -> string -> string -> values -> unit + 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 diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml index b6ab00f2d5..76e30e9c91 100644 --- a/sw/tools/gen_messages.ml +++ b/sw/tools/gen_messages.ml @@ -242,7 +242,7 @@ module Gen_onboard = struct sprintf "({ union { uint64_t u; double f; } _f; _f.u = (uint64_t)(%s); Swap32IfBigEndian(_f.u); _f.f; })" !s | 4 -> sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24)" pprz_type.Pprz.inttype o o o o - | _ -> failwith "unexpected size in Gen_messages.print_get_macros" in + | _ -> failwith "unexpected size in Gen_messages.print_get_macros. Possibly since a Telemetry message was defined with a field type of string." in (** To be an array or not to be an array: *) match _type with