mavproxy_rc: support rc override in SITL for multiple vehicles

MAVProxy/mavproxy.py

@@ -335,7 +335,7 @@ def __init__(self):
         self.vehicle_link_map = {}
 
         # SITL output
-        self.sitl_output = None
+        self.sitl_outputs = {}
 
         self.mav_param_by_sysid = {}
         self.mav_param_by_sysid[(self.settings.target_system, self.settings.target_component)] = mavparm.MAVParmDict()
@@ -1325,7 +1325,7 @@ def run_startup_scripts():
                       default=[])
     parser.add_option("--baudrate", dest="baudrate", type='int',
                       help="default serial baud rate", default=57600)
-    parser.add_option("--sitl", dest="sitl", default=None, help="SITL output port")
+    parser.add_option("--sitl", dest="sitl", action='append', default=[], help="SITL output port")
     parser.add_option("--streamrate", dest="streamrate", default=4, type='int',
                       help="MAVLink stream rate")
     parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
@@ -1520,8 +1520,14 @@ def quit_handler(signum=None, frame=None):
             mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(opts.baudrate),
                                                                   input=False, autoreconnect=True))
 
-    if opts.sitl:
-        mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
+    for sitl in opts.sitl:
+        # Deduce the sitl instance from the port
+        SITL_PORT_0 = 5501
+        SITL_PORT_OFFSET = 10
+        sitl_split = sitl.split(":")
+        sitl_port = int(sitl_split[-1])
+        sitl_instance = (sitl_port - SITL_PORT_0) // SITL_PORT_OFFSET
+        mpstate.sitl_outputs[sitl_instance] = mavutil.mavudp(sitl, input=False)
 
     mpstate.settings.streamrate = opts.streamrate
     mpstate.settings.streamrate2 = opts.streamrate

MAVProxy/modules/lib/mp_module.py

@@ -111,8 +111,8 @@ def vehicle_name(self):
         return self.mpstate.vehicle_name
 
     @property
-    def sitl_output(self):
-        return self.mpstate.sitl_output
+    def sitl_outputs(self):
+        return self.mpstate.sitl_outputs
 
     @property
     def target_system(self):

MAVProxy/modules/mavproxy_rc.py

@@ -30,7 +30,7 @@ def __init__(self, mpstate):
         self.add_command('switch', self.cmd_switch, "flight mode switch control", ['<0|1|2|3|4|5|6>'])
         self.rc_settings = mp_settings.MPSettings(
             [('override_hz', float, 10.0)])
-        if self.sitl_output:
+        if self.sitl_outputs:
             self.rc_settings.override_hz = 20.0
         self.add_completion_function('(RCSETTING)',
                                      self.rc_settings.completion)
@@ -110,12 +110,52 @@ def mavlink_packet(self, m):
         elif m.get_type() == 'SERVO_OUTPUT_RAW' and self.servoout_gui:
             self.servoout_gui.processPacket(m)
 
+    def sitl_instance(self):
+        # Scan the vehicle_link_map for matching (sysid, compid)
+        sysid = self.mpstate.settings.target_system
+        compid = self.mpstate.settings.target_component
+        linknum = None
+        for k, v in self.mpstate.vehicle_link_map.items():
+            if (sysid, compid) in v:
+                linknum = k
+                break
+
+        if linknum is None:
+            return None
+
+        # Find the corresponding connection
+        master = None
+        for m in self.mpstate.mav_master:
+            if m.linknum == linknum:
+                master = m
+                break
+
+        if master is None:
+            return None
+
+        # Determine the sitl instance
+        MASTER_PORT_0 = 5760
+        MASTER_PORT_OFFSET = 10
+        address = master.address
+        address_split = address.split(":")
+        # Check this is a tcp connection.
+        if len(address_split) != 3 or address_split[0] != "tcp":
+            return None
+        port = int(address_split[-1])
+        instance = (port - MASTER_PORT_0) // MASTER_PORT_OFFSET
+        return instance
+
     def send_rc(self):
         '''send RC override packet'''
-        if self.sitl_output:
+        # Skip calculating the sitl instance if no sitl outputs are set
+        if self.sitl_outputs:
+            sitl_instance = self.sitl_instance()
+
+        if self.sitl_outputs and sitl_instance in self.sitl_outputs:
             chan16 = self.override[:16]
             buf = struct.pack('<HHHHHHHHHHHHHHHH', *chan16)
-            self.sitl_output.write(buf)
+            sitl_output = self.sitl_outputs[sitl_instance]
+            sitl_output.write(buf)
         else:
             chan18 = self.override[:18]
             self.master.mav.rc_channels_override_send(self.target_system,