From 75e8424e3fa09d6e651472ead4662919910d4b64 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 9 Dec 2017 15:32:34 +1100
Subject: [PATCH] Tools: autotest: add a test for driving a simple RTL mission

In particular, we weren't setting NAV_CONTROLLER_OUTPUT.wp_dist
correctly before a recent commit from Randy
---
 Tools/autotest/ArduRover-Missions/rtl.txt |  5 +++
 Tools/autotest/apmrover2.py               | 47 +++++++++++++++++++++++
 2 files changed, 52 insertions(+)
 create mode 100644 Tools/autotest/ArduRover-Missions/rtl.txt

diff --git a/Tools/autotest/ArduRover-Missions/rtl.txt b/Tools/autotest/ArduRover-Missions/rtl.txt
new file mode 100644
index 0000000000..6fc14153b5
--- /dev/null
+++ b/Tools/autotest/ArduRover-Missions/rtl.txt
@@ -0,0 +1,5 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	40.071377	-105.229790	1583.430054	1
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071712	-105.229912	0.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071701	-105.229294	0.000000	1
+3	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py
index 4ea4b04095..ca051b2f22 100644
--- a/Tools/autotest/apmrover2.py
+++ b/Tools/autotest/apmrover2.py
@@ -165,6 +165,44 @@ def drive_brake(mavproxy, mav):
 vinfo = vehicleinfo.VehicleInfo()
 
 
+def drive_rtl_mission(mavproxy, mav):
+    mission_filepath = os.path.join(testdir, "ArduRover-Missions", "rtl.txt")
+    mavproxy.send('wp load %s\n' % mission_filepath)
+    mavproxy.expect('Flight plan received')
+    mavproxy.send('switch 4\n')  # auto mode
+    set_rc(mavproxy, mav, 3, 1500)
+    wait_mode(mav, 'AUTO')
+    mavproxy.expect('Executing RTL')
+
+    m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
+                       blocking=True,
+                       timeout=0.1)
+    if m is None:
+        progress("Did not receive NAV_CONTROLLER_OUTPUT message")
+        return False
+
+    wp_dist_min = 5
+    if m.wp_dist < wp_dist_min:
+        progress("Did not start at least 5 metres from destination")
+        return False
+
+    progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" %
+             (m.wp_dist, wp_dist_min,))
+
+    wait_mode(mav, 'HOLD')
+
+    pos = mav.location()
+    home_distance = get_distance(HOME, pos)
+    home_distance_max = 5
+    if home_distance > home_distance_max:
+        progress("Did not get home (%u metres distant > %u)" %
+              (home_distance,home_distance_max))
+        return False
+    mavproxy.send('switch 6\n')
+    wait_mode(mav, 'MANUAL')
+    progress("RTL Mission OK")
+    return True
+
 def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
     """Drive APMrover2 in SITL.
 
@@ -256,6 +294,15 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa
         if not arm_vehicle(mavproxy, mav):
             progress("Failed to ARM")
             failed = True
+
+        progress("#")
+        progress("########## Drive an RTL mission  ##########")
+        progress("#")
+        # Drive a square in learning mode
+        if not drive_rtl_mission(mavproxy, mav):
+            progress("Failed RTL mission")
+            failed = True
+
         progress("#")
         progress("########## Drive a square and save WPs with CH7 switch  ##########")
         progress("#")