diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py index 765757064c..80a085b2e3 100755 --- a/MAVProxy/mavproxy.py +++ b/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 diff --git a/MAVProxy/modules/lib/mp_module.py b/MAVProxy/modules/lib/mp_module.py index 9c64bf7657..cde15b72eb 100644 --- a/MAVProxy/modules/lib/mp_module.py +++ b/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): diff --git a/MAVProxy/modules/mavproxy_rc.py b/MAVProxy/modules/mavproxy_rc.py index 375d65d943..5919c6d4aa 100644 --- a/MAVProxy/modules/mavproxy_rc.py +++ b/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('