Merge pull request #429 from uaarg/redundant_comms

Redundant comms

conf/control_panel.xml.example

@@ -20,6 +20,8 @@
       <arg flag="-b" variable="ivy_bus"/>
     </program>
 
+    <program name="Link Combiner" command="sw/ground_segment/python/redundant_link/link_combiner.py"/>
+    
     <program name="GCS" command="sw/ground_segment/cockpit/gcs">
       <arg flag="-b" variable="ivy_bus"/>
     </program>
@@ -121,6 +123,23 @@
       <program name="Server"/>
     </session>
 
+    <session name="Flight USB-serial Redundant">
+      <program name="Server"/>
+      <program name="GCS"/>
+      <program name="Data Link">
+        <arg flag="-d" constant="/dev/ttyUSB0"/>
+        <arg flag="-id" constant="1"/>
+        <arg flag="-redlink"/>
+      </program>
+      <program name="Data Link">
+        <arg flag="-d" constant="/dev/ttyUSB1"/>
+        <arg flag="-id" constant="2"/>
+        <arg flag="-redlink"/>
+      </program>
+      <program name="Link Combiner"/>
+    </session>
+
+
   </section>
 
 </control_panel>

conf/messages.xml

@@ -151,6 +151,7 @@
   </message>
 
   <message name="DOWNLINK_STATUS" id="23">
+    <field name="link_id" type="int8"/>
     <field name="run_time" type="uint32" unit="s"/>
     <field name="rx_bytes" type="uint32"/>
     <field name="rx_msgs" type="uint32"/>
@@ -558,7 +559,20 @@
     <field name="rssi_fade_margin" type="uint8" unit="dB"/>
     <field name="duty" type="uint8" unit="%"/>
   </message>
- <!-- 71 is free -->
+
+  <message name="LINK_STATUS" id="71">
+      <field name="link_id" type="uint8"/>
+      <field name="time_since_last_msg" type="float" unit="s"/>
+      <field name="run_time" type="uint32" unit="s"/>
+      <field name="rx_bytes" type="uint32"/>
+      <field name="rx_msgs" type="uint32"/>
+      <field name="rx_err" type="uint32"/>
+      <field name="rx_bytes_rate" type="float" format="%.1f"/>
+      <field name="rx_msgs_rate" type="float" format="%.1f"/>
+      <field name="ping_time" type="float" format="%.2f" unit="ms"/>
+    </message>
+
+
  <!-- 72 is free -->
  <!-- 73 is free -->
  <!-- 74 is free -->
@@ -2461,6 +2475,18 @@
     <field name="message" type="string"/>
   </message>
 
+   <message name="TELEMETRY_MESSAGE" id="34">
+    <field name="ac_id" type="string"/>
+    <field name="link_id" type="string" />
+    <field name="message" type="string" format=";sv"/>
+   </message>
+
+  <message name="DATALINK_MESSAGE" id="35">
+    <field name="ac_id" type="string"/>
+    <field name="link_id" type="string"/>
+    <field name="message" type="string" format=";sv"/>
+  </message>
+
   <message name="PLUMES" id="100">
     <field name="ids" type="string" format="csv"/>
     <field name="lats" type="string" format="csv"/>

sw/ground_segment/cockpit/gcs.glade

@@ -213,7 +213,7 @@
                                         <child>
                                           <widget class="GtkEventBox" id="eventbox_telemetry">
                                             <property name="visible">True</property>
-                                            <property name="tooltip" translatable="yes">Telemetry status: seconds since the last battery level message</property>
+                                            <property name="tooltip" translatable="yes">Telemetry status: ratio of connected links to number of links (if multiple links used) or seconds since the last message (if link lost)</property>
                                             <child>
                                               <widget class="GtkLabel" id="label_telemetry">
                                                 <property name="visible">True</property>

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;
@@ -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,7 +825,23 @@ 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
@@ -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
+  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;

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;

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;;

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
+

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.

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()

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,6 +74,9 @@ let send_message_over_ivy = fun sender name vs ->
     match !add_timestamp with
         None -> None
       | Some start_time -> Some (Unix.gettimeofday () -. start_time) in
+  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
 
 
@@ -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 "<my_addr> (%d)" !XB.my_addr);
       "-xbee_retries", Arg.Set_int XB.my_addr, (sprintf "<nb retries> (%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
 

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,7 +609,21 @@ 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

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

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