modules/router: add selective MAVLink routing module

MAVProxy/mavproxy.py

@@ -23,6 +23,7 @@
 import threading
 import time
 import traceback
+import copy
 
 try:
     reload
@@ -895,26 +896,49 @@ def process_mavlink(slave):
         return
     if msgs is None:
         return
+    router_module = mpstate.module('router')
     allow_fwd = mpstate.settings.mavfwd
     if not allow_fwd and mpstate.settings.mavfwd_disarmed and not mpstate.master(-1).motors_armed():
         allow_fwd = True
     if mpstate.status.setup_mode:
         allow_fwd = False
     if allow_fwd:
         for m in msgs:
-            target_sysid = getattr(m, 'target_system', -1)
-            if mpstate.settings.mavfwd_link > 0 and mpstate.settings.mavfwd_link <= len(mpstate.mav_master):
-                output = mpstate.mav_master[mpstate.settings.mavfwd_link-1]
+            if router_module is not None and router_module.router_enabled:
+                for link in mpstate.mav_master:
+                    if router_module.check(m, link.address):
+                        if (mpstate.settings.mavfwd_signing and
+                                link.mav.signing.sign_outgoing and
+                                (m._header.incompat_flags & mavutil.mavlink.MAVLINK_IFLAG_SIGNED) == 0):
+                            # repack the message if this is a signed link and not already signed
+                            msg_to_send = copy.copy(m) # We copy the message for not signing m
+                            msg_to_send.pack(link.mav)
+                        else:
+                            msg_to_send = m # We never copy if we don't have to
+                        link.write(msg_to_send.get_msgbuf())
+                for slave_e in mpstate.mav_outputs:
+                    if slave_e.address != slave.address:  # We check that we aren't going to send the msg to the expeditor
+                        if router_module.check(m, slave_e.address):
+                            if hasattr(slave_e, 'ws') and slave_e.ws is not None:
+                                from wsproto.connection import ConnectionState
+                                if slave_e.ws.state != ConnectionState.OPEN:  # Ensure Websocket handshake is done
+                                    continue
+                            slave_e.write(m.get_msgbuf())
             else:
-                # find best link by sysid
-                output = mpstate.master(target_sysid)
-            if (mpstate.settings.mavfwd_signing and
-                    output.mav.signing.sign_outgoing and
-                    (m._header.incompat_flags & mavutil.mavlink.MAVLINK_IFLAG_SIGNED) == 0):
-                # repack the message if this is a signed link and not already signed
-                m.pack(output.mav)
-
-            output.write(m.get_msgbuf())
+                # Fallback (no router)
+                target_sysid = getattr(m, 'target_system', -1)
+                if mpstate.settings.mavfwd_link > 0 and mpstate.settings.mavfwd_link <= len(mpstate.mav_master):
+                    output = mpstate.mav_master[mpstate.settings.mavfwd_link-1]
+                else:
+                    # find best link by sysid
+                    output = mpstate.master(target_sysid)
+                if (mpstate.settings.mavfwd_signing and
+                        output.mav.signing.sign_outgoing and
+                        (m._header.incompat_flags & mavutil.mavlink.MAVLINK_IFLAG_SIGNED) == 0):
+                    # repack the message if this is a signed link and not already signed
+                    m.pack(output.mav)
+
+                output.write(m.get_msgbuf())
             if mpstate.logqueue:
                 usec = int(time.time() * 1.0e6)
                 mpstate.logqueue.put(bytearray(struct.pack('>Q', usec) + m.get_msgbuf()))
@@ -1382,6 +1406,10 @@ def run_startup_scripts():
     parser.add_option("--default-modules", default="log,signing,wp,rally,fence,ftp,param,relay,tuneopt,arm,mode,calibration,rc,auxopt,misc,cmdlong,battery,terrain,output,adsb,layout", help='default module list')  # noqa:E501
     parser.add_option("--udp-timeout", dest="udp_timeout", default=0.0, type='float', help="Timeout for udp clients in seconds")  # noqa:E501
     parser.add_option("--retries", type=int, help="number of times to retry connection", default=3)
+    parser.add_option("--router-config", dest="router_config",
+                      help="Indicates a router_config.json file to load at the start")
+    parser.add_option("--router-use-dir", dest="router_use_dir",
+                      help="Indicates directory with JSON files (instead of Current Working Directory by default)")
 
     (opts, args) = parser.parse_args()
     if len(args) != 0:
@@ -1474,6 +1502,12 @@ def quit_handler(signum=None, frame=None):
     for sig in fatalsignals:
         signal.signal(sig, quit_handler)
 
+    if opts.router_config or opts.router_use_dir:
+        if mpstate.load_module('router', quiet=False):
+            if opts.router_use_dir:
+                mpstate.module('router').router_config_dir = opts.router_use_dir
+            if opts.router_config and mpstate.module('router').load(opts.router_config):
+                mpstate.module('router').start()
     mpstate.load_module('link', quiet=True)
 
     # load signing early to allow for use of ~/.mavproxy/signing.keys

MAVProxy/modules/mavproxy_link.py

@@ -20,6 +20,7 @@
 import sys
 import time
 import traceback
+import copy
 
 if sys.version_info[0] >= 3:
     import io as StringIO
@@ -992,6 +993,7 @@ def master_callback(self, m, master):
         '''process mavlink message m on master, sending any messages to recipients'''
         sysid = m.get_srcSystem()
         mtype = m.get_type()
+        router_module = self.mpstate.module('router')
 
         if mtype in ['HEARTBEAT', 'HIGH_LATENCY2'] and m.type != mavutil.mavlink.MAV_TYPE_GCS:
             compid = m.get_srcComponent()
@@ -1003,7 +1005,11 @@ def master_callback(self, m, master):
 
         # see if it is handled by a specialised sysid connection
         if sysid in self.mpstate.sysid_outputs:
-            self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())
+            if router_module is not None and router_module.router_enabled:
+                if router_module.check(m, self.mpstate.sysid_outputs[sysid].address):
+                    self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())
+            else:
+                self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())
             if mtype == "GLOBAL_POSITION_INT":
                 for modname in 'map', 'asterix', 'NMEA', 'NMEA2':
                     mod = self.module(modname)
@@ -1015,7 +1021,8 @@ def master_callback(self, m, master):
             master.post_message(m)
         self.status.counters['MasterIn'][master.linknum] += 1
 
-        if mtype == 'GLOBAL_POSITION_INT':
+        if mtype == 'GLOBAL_POSITION_INT' and \
+            (router_module is None or not router_module.router_enabled):
             # send GLOBAL_POSITION_INT to 2nd GCS for 2nd vehicle display
             for sysid in self.mpstate.sysid_outputs:
                 self.mpstate.sysid_outputs[sysid].write(m.get_msgbuf())
@@ -1074,12 +1081,33 @@ def master_callback(self, m, master):
             # GCS
             if self.mpstate.settings.mavfwd_rate or mtype != 'REQUEST_DATA_STREAM':
                 if mtype not in self.no_fwd_types:
-                    for r in self.mpstate.mav_outputs:
-                        if hasattr(r, 'ws') and r.ws is not None:
-                            from wsproto.connection import ConnectionState
-                            if r.ws.state != ConnectionState.OPEN:  # Ensure Websocket handshake is done
-                                continue
-                        r.write(m.get_msgbuf())
+                    if router_module is not None and router_module.router_enabled:
+                        for r in self.mpstate.mav_outputs:
+                            if router_module.check(m, r.address):
+                                if hasattr(r, 'ws') and r.ws is not None:
+                                    from wsproto.connection import ConnectionState
+                                    if r.ws.state != ConnectionState.OPEN:  # Ensure Websocket handshake is done
+                                        continue
+                                r.write(m.get_msgbuf())
+                        for link in self.mpstate.mav_master:
+                            if link != master:
+                                if router_module.check(m, link.address):
+                                    if (self.mpstate.settings.mavfwd_signing and
+                                            link.mav.signing.sign_outgoing and
+                                            (m._header.incompat_flags & mavutil.mavlink.MAVLINK_IFLAG_SIGNED) == 0):
+                                        # repack the message if this is a signed link and not already signed
+                                        msg_to_send = copy.copy(m) # We copy the message for not signing m
+                                        msg_to_send.pack(link.mav)
+                                    else:
+                                        msg_to_send = m # We never copy if we don't have to
+                                    link.write(msg_to_send.get_msgbuf())
+                    else:
+                        for r in self.mpstate.mav_outputs:
+                            if hasattr(r, 'ws') and r.ws is not None:
+                                from wsproto.connection import ConnectionState
+                                if r.ws.state != ConnectionState.OPEN:  # Ensure Websocket handshake is done
+                                    continue
+                            r.write(m.get_msgbuf())
 
             sysid = m.get_srcSystem()
             target_sysid = self.target_system