autotest: add tests for serial RangeFinder drivers

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(

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()
 

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)