From fecca9a559c155f441cc0de0e3df6992cf7fecd3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 2 Dec 2019 13:01:06 +1100 Subject: [PATCH] autotest: add tests for serial RangeFinder drivers --- Tools/autotest/arducopter.py | 78 ++++++++++++++++++++++++++++++++++++ Tools/autotest/common.py | 57 +++++++++++++++++++------- Tools/autotest/pysim/util.py | 2 + 3 files changed, 122 insertions(+), 15 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 47e6aa19c8..846634e5ee 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -4111,6 +4111,80 @@ class AutoTestCopter(AutoTest): self.wait_current_waypoint(0, timeout=10) self.set_rc(7, 1000) + def fly_rangefinder_drivers_fly(self, rangefinders): + '''ensure rangefinder gives height-above-ground''' + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + expected_alt = 5 + self.user_takeoff(alt_min=expected_alt) + rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True) + if rf is None: + raise NotAchievedException("Did not receive rangefinder message") + gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) + if gpi is None: + raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") + if abs(rf.distance - gpi.relative_alt/1000.0) > 1: + raise NotAchievedException("rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" % (rf.distance, gpi.relative_alt/1000.0)) + + for i in range(0, len(rangefinders)): + name = rangefinders[i] + self.progress("i=%u (%s)" % (i, name)) + ds = self.mav.recv_match( + type="DISTANCE_SENSOR", + timeout=2, + blocking=True, + condition="DISTANCE_SENSOR.id==%u" % i + ) + if ds is None: + raise NotAchievedException("Did not receive DISTANCE_SENSOR message for id==%u (%s)" % (i, name)) + self.progress("Got: %s" % str(ds)) + if abs(ds.current_distance/100.0 - gpi.relative_alt/1000.0) > 1: + raise NotAchievedException( + "distance sensor.current_distance (%f) (%s) disagrees with global-position-int.relative_alt (%s)" % + (ds.current_distance/100.0, name, gpi.relative_alt/1000.0)) + + self.land_and_disarm() + + self.progress("Ensure RFND messages in log") + if not self.current_onboard_log_contains_message("RFND"): + raise NotAchievedException("No RFND messages in log") + + + def fly_rangefinder_drivers(self): + self.set_parameter("RTL_ALT", 500) + self.set_parameter("TERRAIN_FOLLOW", 1) + drivers = [ + ("lightwareserial",8), + ("ulanding_v0", 11), + ("ulanding_v1", 11), + ("leddarone", 12), + ("maxsonarseriallv", 13), + ("nmea", 17), + ("wasp", 18), + ("benewake_tf02", 19), + ("blping", 23), + ("benewake_tfmini", 20), + ("lanbao", 26), + ("benewake_tf03", 27), + ] + while len(drivers): + do_drivers = drivers[0:3] + drivers = drivers[3:] + command_line_args = [] + for (offs, cmdline_argument, serial_num) in [(0, '--uartE', 4), + (1, '--uartF', 5), + (2, '--uartG', 6)]: + if len(do_drivers) > offs: + (sim_name, rngfnd_param_value) = do_drivers[offs] + command_line_args.append("%s=sim:%s" % + (cmdline_argument,sim_name)) + serial_param_name = "SERIAL%u_PROTOCOL" % serial_num + self.set_parameter(serial_param_name, 9) # rangefinder + self.set_parameter("RNGFND%u_TYPE" % (offs+1), rngfnd_param_value) + self.customise_SITL_commandline(command_line_args) + self.fly_rangefinder_drivers_fly([x[0] for x in do_drivers]) + def tests(self): '''return list of all tests''' ret = super(AutoTestCopter, self).tests() @@ -4325,6 +4399,10 @@ class AutoTestCopter(AutoTest): "Fly Dynamic Notches", self.fly_dynamic_notches), + ("RangeFinderDrivers", + "Test rangefinder drivers", + self.fly_rangefinder_drivers), + ("LogDownLoad", "Log download", lambda: self.log_download( diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index b71fd7a72e..cc4c48834b 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -118,6 +118,7 @@ class ArmedAtEndOfTestException(ErrorException): class Context(object): def __init__(self): self.parameters = [] + self.sitl_commandline_customised = False # https://stackoverflow.com/questions/616645/how-do-i-duplicate-sys-stdout-to-a-log-file-in-python class TeeBoth(object): @@ -446,6 +447,21 @@ class AutoTest(ABC): self.set_streamrate(self.sitl_streamrate()) self.progress("Reboot complete") + def customise_SITL_commandline(self, customisations): + '''customisations could be "--uartF=sim:nmea" ''' + self.contexts[-1].sitl_commandline_customised = True + self.stop_SITL() + self.start_SITL(customisations=customisations, wipe=False) + self.wait_heartbeat() + + def reset_SITL_commandline(self): + self.stop_SITL() + self.start_SITL(wipe=False) + + def stop_SITL(self): + self.progress("Stopping SITL") + util.pexpect_close(self.sitl) + def close(self): """Tidy up after running all tests.""" if self.use_map: @@ -454,7 +470,7 @@ class AutoTest(ABC): self.mav.close() util.pexpect_close(self.mavproxy) - util.pexpect_close(self.sitl) + self.stop_SITL() valgrind_log = util.valgrind_log_filepath(binary=self.binary, model=self.frame) @@ -2203,6 +2219,8 @@ class AutoTest(ABC): self.get_exception_stacktrace(e)) ex = e self.test_timings[desc] = time.time() - start_time + if self.contexts[-1].sitl_commandline_customised: + self.reset_SITL_commandline() self.context_pop() passed = True @@ -2273,6 +2291,28 @@ class AutoTest(ABC): self.progress("Waiting for Parameters") self.mavproxy.expect('Received [0-9]+ parameters') + def start_SITL(self, **sitl_args): + start_sitl_args = { + "breakpoints": self.breakpoints, + "disable_breakpoints": self.disable_breakpoints, + "defaults_file": self.defaults_filepath(), + "gdb": self.gdb, + "gdbserver": self.gdbserver, + "lldb": self.lldb, + "home": self.sitl_home(), + "model": self.frame, + "speedup": self.speedup, + "valgrind": self.valgrind, + "vicon": self.uses_vicon(), + "wipe": True, + } + start_sitl_args.update(**sitl_args) + self.progress("Starting SITL") + self.sitl = util.start_SITL(self.binary, **start_sitl_args) + + def sitl_is_running(self): + return self.sitl.is_alive() + def init(self): """Initilialize autotest feature.""" self.check_test_syntax(test_file=self.test_filepath()) @@ -2283,20 +2323,7 @@ class AutoTest(ABC): self.frame = self.default_frame() self.progress("Starting simulator") - self.sitl = util.start_SITL(self.binary, - breakpoints=self.breakpoints, - disable_breakpoints=self.disable_breakpoints, - defaults_file=self.defaults_filepath(), - gdb=self.gdb, - gdbserver=self.gdbserver, - lldb=self.lldb, - home=self.sitl_home(), - model=self.frame, - speedup=self.speedup, - valgrind=self.valgrind, - vicon=self.uses_vicon(), - wipe=True, - ) + self.start_SITL() self.start_mavproxy() diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index a4721eeeee..14a8dce4b2 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -241,6 +241,7 @@ def start_SITL(binary, breakpoints=[], disable_breakpoints=False, vicon=False, + customisations=[], lldb=False): """Launch a SITL instance.""" cmd = [] @@ -320,6 +321,7 @@ def start_SITL(binary, cmd.extend(['--unhide-groups']) if vicon: cmd.extend(["--uartF=sim:vicon:"]) + cmd.extend(customisations) if gdb and not os.getenv('DISPLAY'): subprocess.Popen(cmd)