autotest: customise drive_rtl_mission for sailboats

Tools/autotest/sailboat.py

@@ -11,6 +11,9 @@ import os
 
 from rover import AutoTestRover
 
+from common import AutoTestTimeoutException
+from common import PreconditionFailedException
+
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
 
@@ -29,6 +32,48 @@ class AutoTestSailboat(AutoTestRover):
             self.frame = 'sailboat'
         super(AutoTestSailboat, self).init()
 
+    def drive_rtl_mission(self, timeout=120):
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        self.load_mission("rtl.txt")
+        self.change_mode("AUTO")
+
+        tstart = self.get_sim_time()
+        while True:
+            now = self.get_sim_time_cached()
+            if now - tstart > timeout:
+                raise AutoTestTimeoutException("Didn't see wp 3")
+            m = self.mav.recv_match(type='MISSION_CURRENT',
+                                    blocking=True,
+                                    timeout=1)
+            self.progress("MISSION_CURRENT: %s" % str(m))
+            if m.seq == 3:
+                break
+
+        self.drain_mav()
+
+        m = self.assert_receive_message('NAV_CONTROLLER_OUTPUT', timeout=1)
+
+        wp_dist_min = 5
+        if m.wp_dist < wp_dist_min:
+            raise PreconditionFailedException(
+                "Did not start at least %f metres from destination (is=%f)" %
+                (wp_dist_min, m.wp_dist))
+
+        self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" %
+                      (m.wp_dist, wp_dist_min,))
+
+        # wait for mission to complete
+        self.wait_statustext("Mission Complete", timeout=70)
+
+        # sailboat loiters around RTL point indefinitely:
+        self.wait_groundspeed(0.5, 3, timeout=20, minimum_duration=10)
+
+        self.disarm_vehicle()
+
+        self.progress("RTL Mission OK")
+
     def tests(self):
         '''return list of all tests'''
         ret = ([])