From b90166ab97d38e3b07d345860e63563722c73fff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 16 Feb 2023 16:37:55 +1100 Subject: [PATCH] Tools: use methods to determine port numbers --- Tools/autotest/common.py | 82 ++++++++++++++++++++++++------------ Tools/autotest/pysim/util.py | 7 ++- Tools/autotest/rover.py | 8 ++-- 3 files changed, 67 insertions(+), 30 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 42d0585cb0..d02015c428 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1653,13 +1653,27 @@ class AutoTest(ABC): """Allow subclasses to override SITL streamrate.""" return 10 + def adjust_ardupilot_port(self, port): + '''adjust port in case we do not wish to use the default range (5760 and 5501 etc)''' + return port + + def spare_network_port(self, offset=0): + '''returns a network port which should be able to be bound''' + if offset > 2: + raise ValueError("offset too large") + return 8000 + offset + def autotest_connection_string_to_ardupilot(self): - return "tcp:127.0.0.1:5760" + return "tcp:127.0.0.1:%u" % self.adjust_ardupilot_port(5760) + + def sitl_rcin_port(self, offset=0): + if offset > 2: + raise ValueError("offset too large") + return 5501 + offset def mavproxy_options(self): """Returns options to be passed to MAVProxy.""" ret = [ - '--sitl=127.0.0.1:5502', '--streamrate=%u' % self.sitl_streamrate(), '--target-system=%u' % self.sysid_thismav(), '--target-component=1', @@ -3466,7 +3480,7 @@ class AutoTest(ABC): self.reboot_sitl() mav2 = mavutil.mavlink_connection( - "tcp:localhost:5763", + "tcp:localhost:%u" % self.adjust_ardupilot_port(5763), robust_parsing=True, source_system=7, source_component=7, @@ -4871,7 +4885,7 @@ class AutoTest(ABC): def rc_thread_main(self): chan16 = [1000] * 16 - sitl_output = mavutil.mavudp("127.0.0.1:5501", input=False) + sitl_output = mavutil.mavudp("127.0.0.1:%u" % self.sitl_rcin_port(), input=False) buf = None while True: @@ -8017,7 +8031,7 @@ Also, ignores heartbeats not from our target system''' def defaults_filepath(self): return None - def start_mavproxy(self): + def start_mavproxy(self, sitl_rcin_port=None): self.start_mavproxy_count += 1 if self.mavproxy is not None: return self.mavproxy @@ -8029,11 +8043,17 @@ Also, ignores heartbeats not from our target system''' if self.valgrind or self.callgrind: pexpect_timeout *= 10 + if sitl_rcin_port is None: + sitl_rcin_port = self.sitl_rcin_port() + mavproxy = util.start_MAVProxy_SITL( self.vehicleinfo_key(), + master='tcp:127.0.0.1:%u' % self.adjust_ardupilot_port(5762), logfile=self.mavproxy_logfile, options=self.mavproxy_options(), - pexpect_timeout=pexpect_timeout) + pexpect_timeout=pexpect_timeout, + sitl_rcin_port=sitl_rcin_port, + ) mavproxy.expect(r'Telemetry log: (\S+)\r\n') self.logfile = mavproxy.match.group(1) self.progress("LOGFILE %s" % self.logfile) @@ -11455,9 +11475,10 @@ switch value''' '''Test AHRS NMEA Output can be read by out NMEA GPS''' self.set_parameter("SERIAL5_PROTOCOL", 20) # serial5 is NMEA output self.set_parameter("GPS_TYPE2", 5) # GPS2 is NMEA + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartE=tcp:6735", # GPS2 is NMEA.... - "--uartF=tcpclient:127.0.0.1:6735", # serial5 spews to localhost:6735 + "--uartE=tcp:%u" % port, # GPS2 is NMEA.... + "--uartF=tcpclient:127.0.0.1:%u" % port, # serial5 spews to localhost port ]) self.do_timesync_roundtrip() self.wait_gps_fix_type_gte(3) @@ -12066,10 +12087,11 @@ switch value''' "RPM1_TYPE": 10, # enable RPM output "TERRAIN_ENABLE": 0, }) + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyPassThrough(("127.0.0.1", 6735), + frsky = FRSkyPassThrough(("127.0.0.1", port), get_time=self.get_sim_time_cached) # waiting until we are ready to arm should ensure our wanted @@ -12160,10 +12182,11 @@ switch value''' "SERIAL5_PROTOCOL": 10, # serial5 is FRSky passthrough "RPM1_TYPE": 10, # enable RPM output }) + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyPassThrough(("127.0.0.1", 6735), + frsky = FRSkyPassThrough(("127.0.0.1", port), get_time=self.get_sim_time_cached) self.wait_ready_to_arm() @@ -12314,10 +12337,11 @@ switch value''' def FRSkyMAVlite(self): '''Test FrSky MAVlite serial output''' self.set_parameter("SERIAL5_PROTOCOL", 10) # serial5 is FRSky passthrough + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyPassThrough(("127.0.0.1", 6735)) + frsky = FRSkyPassThrough(("127.0.0.1", port)) frsky.connect() sport_to_mavlite = SPortToMAVlite() @@ -12588,10 +12612,11 @@ switch value''' '''Test FrSky SPort mode''' self.set_parameter("SERIAL5_PROTOCOL", 4) # serial5 is FRSky sport self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkySPort(("127.0.0.1", 6735), verbose=True) + frsky = FRSkySPort(("127.0.0.1", port), verbose=True) self.wait_ready_to_arm() # we need to start the engine to get some RPM readings, we do it for plane only @@ -12660,10 +12685,11 @@ switch value''' def FRSkyD(self): '''Test FrSkyD serial output''' self.set_parameter("SERIAL5_PROTOCOL", 3) # serial5 is FRSky output + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - frsky = FRSkyD(("127.0.0.1", 6735)) + frsky = FRSkyD(("127.0.0.1", port)) self.wait_ready_to_arm() m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1) gpi_abs_alt = int((m.alt+500) / 1000) # mm -> m @@ -12782,10 +12808,11 @@ switch value''' def LTM(self): '''Test LTM serial output''' self.set_parameter("SERIAL5_PROTOCOL", 25) # serial5 is LTM output + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - ltm = LTM(("127.0.0.1", 6735)) + ltm = LTM(("127.0.0.1", port)) self.wait_ready_to_arm() wants = { @@ -12824,10 +12851,11 @@ switch value''' '''Test DEVO serial output''' self.context_push() self.set_parameter("SERIAL5_PROTOCOL", 17) # serial5 is DEVO output + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - devo = DEVO(("127.0.0.1", 6735)) + devo = DEVO(("127.0.0.1", port)) self.wait_ready_to_arm() m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1) @@ -12896,10 +12924,11 @@ switch value''' '''Test MSP DJI serial output''' self.set_parameter("SERIAL5_PROTOCOL", 33) # serial5 is MSP DJI output self.set_parameter("MSP_OPTIONS", 1) # telemetry (unpolled) mode + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 spews to localhost:6735 + "--uartF=tcp:%u" % port # serial5 spews to localhost port ]) - msp = MSP_DJI(("127.0.0.1", 6735)) + msp = MSP_DJI(("127.0.0.1", port)) self.wait_ready_to_arm() tstart = self.get_sim_time() @@ -12923,10 +12952,11 @@ switch value''' ex = None try: self.set_parameter("SERIAL5_PROTOCOL", 23) # serial5 is RCIN input + port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:6735" # serial5 reads from to localhost:6735 + "--uartF=tcp:%u" % port # serial5 reads from to localhost port ]) - crsf = CRSF(("127.0.0.1", 6735)) + crsf = CRSF(("127.0.0.1", port)) crsf.connect() self.progress("Writing vtx_frame") diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 48e9ee38d6..5a65629653 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -600,11 +600,15 @@ def MAVProxy_version(): def start_MAVProxy_SITL(atype, aircraft=None, setup=False, - master='tcp:127.0.0.1:5762', + master=None, options=[], + sitl_rcin_port=5501, pexpect_timeout=60, logfile=sys.stdout): """Launch mavproxy connected to a SITL instance.""" + if master is None: + raise ValueError("Expected a master") + local_mp_modules_dir = os.path.abspath( os.path.join(__file__, '..', '..', '..', 'mavproxy_modules')) env = dict(os.environ) @@ -617,6 +621,7 @@ def start_MAVProxy_SITL(atype, cmd = [] cmd.append(mavproxy_cmd()) cmd.extend(['--master', master]) + cmd.extend(['--sitl', "localhost:%u" % sitl_rcin_port]) if setup: cmd.append('--setup') if aircraft is None: diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index c3b3e77d80..b8781494a8 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -624,8 +624,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) def MAVProxy_SetModeUsingSwitch(self): """Set modes via mavproxy switch""" + port = self.sitl_rcin_port(offset=1) self.customise_SITL_commandline([ - "--rc-in-port", "5502", + "--rc-in-port", str(port), ]) ex = None try: @@ -637,7 +638,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) (4, 'AUTO'), (5, 'AUTO'), # non-existant mode, should stay in RTL (6, 'MANUAL')] - mavproxy = self.start_mavproxy() + mavproxy = self.start_mavproxy(sitl_rcin_port=port) for (num, expected) in fnoo: mavproxy.send('switch %u\n' % num) self.wait_mode(expected) @@ -6181,7 +6182,8 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) def PrivateChannel(self): '''test the serial option bit specifying a mavlink channel as private''' global mav2 - mav2 = mavutil.mavlink_connection("tcp:localhost:5763", + port = self.adjust_ardupilot_port(5763) + mav2 = mavutil.mavlink_connection("tcp:localhost:%u" % port, robust_parsing=True, source_system=7, source_component=7)