Tools: autotest: add test for Rover SmartRTL

Tools/autotest/apmrover2.py

@@ -1709,6 +1709,48 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.mavproxy.send('module load wp\n')
         self.mavproxy.expect("Loaded module wp")
 
+    def wait_distance_to_home(self, distance, accuracy=5):
+        tstart = self.get_sim_time()
+        timeout = 30
+        while True:
+            if self.get_sim_time_cached() - tstart > timeout:
+                raise AutoTestTimeoutException("Failed to get home")
+            self.mav.recv_match(type='VFR_HUD', blocking=True)
+            self.progress("Dist from home: %.02f" % self.distance_to_home(use_cached_home=True))
+            if abs(distance-self.distance_to_home(use_cached_home=True)) <= accuracy:
+                break
+
+    def drive_smartrtl(self):
+        self.change_mode("STEERING")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        # drive two sides of a square, make sure we don't go back through
+        # the middle of the square
+        self.progress("Driving North")
+        self.reach_heading_manual(0)
+        self.set_rc(3, 2000)
+        self.delay_sim_time(5)
+        self.set_rc(3, 1000)
+        self.wait_groundspeed(0, 1)
+        loc = self.mav.location()
+        self.progress("Driving East")
+        self.set_rc(3, 2000)
+        self.reach_heading_manual(90)
+        self.set_rc(3, 2000)
+        self.delay_sim_time(5)
+        self.set_rc(3, 1000)
+
+        self.progress("Entering smartrtl")
+        self.change_mode("SMART_RTL")
+
+        self.progress("Ensure we go via intermediate point")
+        self.wait_distance_to_location(loc, 0, 5)
+
+        self.progress("Ensure we get home")
+        self.wait_distance_to_home(5, accuracy=2)
+
+        self.disarm_vehicle()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -1733,6 +1775,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             ("DriveRTL",
              "Drive an RTL Mission", self.drive_rtl_mission),
 
+            ("SmartRTL",
+             "Test SmartRTL",
+             self.drive_smartrtl),
+
             ("DriveSquare",
              "Learn/Drive Square with Ch7 option",
              self.drive_square),

Tools/autotest/common.py

@@ -1600,6 +1600,22 @@ class AutoTest(ABC):
                                           % (delta, distance))
         raise WaitDistanceTimeout("Failed to attain distance %u" % distance)
 
+    def wait_distance_to_location(self, location, distance_min, distance_max, timeout=30):
+        last_distance_message = 0
+        tstart = self.get_sim_time()
+        while self.get_sim_time_cached() < tstart + timeout:
+            pos = self.mav.location()
+            delta = self.get_distance(location, pos)
+            if self.get_sim_time_cached() - last_distance_message >= 1:
+                self.progress("Distance=%.2f meters want <%.2f and >%.2f" %
+                              (delta, distance_min, distance_max))
+                last_distance_message = self.get_sim_time_cached()
+            if (delta >= distance_min and delta <= distance_max):
+                self.progress("Attained distance %.02f meters OK" % delta)
+                return True
+        raise WaitDistanceTimeout("Failed to attain distance <%.2f and >%.2f" %
+                                  (distance_min, distance_max))
+
     def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operator.eq):
         """wait for channel value comparison (default condition is equality)"""
         channel_field = "servo%u_raw" % channel
@@ -2168,11 +2184,11 @@ class AutoTest(ABC):
             next_to_request += 1
             remaining_to_receive.discard(m.seq)
 
-    def poll_home_position(self, quiet=False):
+    def poll_home_position(self, quiet=False, timeout=30):
         old = self.mav.messages.get("HOME_POSITION", None)
         tstart = self.get_sim_time()
         while True:
-            if self.get_sim_time_cached() - tstart > 30:
+            if self.get_sim_time_cached() - tstart > timeout:
                 raise NotAchievedException("Failed to poll home position")
             if not quiet:
                 self.progress("Sending MAV_CMD_GET_HOME_POSITION")