From 9f58e7df31ac6c6fba1e9e9cd45045605e341efe Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 3 Dec 2021 22:30:08 +1100
Subject: [PATCH] autotest: add test for quadplane booting in auto

---
 .../ArduPlane_Tests/BootInAUTO/mission.txt    |  4 +++
 Tools/autotest/common.py                      |  5 ++++
 Tools/autotest/quadplane.py                   | 26 +++++++++++++++++++
 3 files changed, 35 insertions(+)
 create mode 100644 Tools/autotest/ArduPlane_Tests/BootInAUTO/mission.txt

diff --git a/Tools/autotest/ArduPlane_Tests/BootInAUTO/mission.txt b/Tools/autotest/ArduPlane_Tests/BootInAUTO/mission.txt
new file mode 100644
index 0000000000..738e35e95b
--- /dev/null
+++ b/Tools/autotest/ArduPlane_Tests/BootInAUTO/mission.txt
@@ -0,0 +1,4 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	0.000000	0.000000	50.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	47.523048	18.813797	100.000000	1
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 8d2eea63f7..04e0798f61 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -6474,6 +6474,11 @@ class AutoTest(ABC):
             **kwargs
         )
 
+    def assert_current_waypoint(self, wpnum):
+        seq = self.mav.waypoint_current()
+        if seq != wpnum:
+            raise NotAchievedException("Incorrect current wp")
+
     def wait_current_waypoint(self, wpnum, timeout=60):
         tstart = self.get_sim_time()
         while True:
diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index 40becfeb70..6a853e6d72 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -705,6 +705,28 @@ class AutoTestQuadPlane(AutoTest):
             "ConfigErrorLoop": "failing because RC values not settable",
         }
 
+    def BootInAUTO(self):
+        self.load_mission("mission.txt")
+        self.set_parameters({
+        })
+        self.set_rc(5, 1000)
+        self.wait_mode('AUTO')
+        self.reboot_sitl()
+        self.wait_ready_to_arm()
+        self.delay_sim_time(20)
+        self.assert_current_waypoint(1)
+        self.arm_vehicle()
+        self.wait_altitude(9, 11, relative=True)  # value from mission file is 10
+        distance = self.distance_to_home()
+        # this distance check is very, very loose.  At time of writing
+        # the vehicle actually pitches ~6 degrees on trakeoff,
+        # wandering over 1m.
+        if distance > 2:
+            raise NotAchievedException("wandered from home (distance=%f)" %
+                                       (distance,))
+        self.change_mode('QLAND')
+        self.wait_disarmed(timeout=60)
+
     def test_pilot_yaw(self):
         self.takeoff(10, mode="QLOITER")
         self.set_parameter("STICK_MIXING", 0)
@@ -978,6 +1000,10 @@ class AutoTestQuadPlane(AutoTest):
              "Check disarm behaviour in Q-mode",
              self.MidAirDisarmDisallowed),
 
+            ("BootInAUTO",
+             "Test behaviour when booting in auto",
+             self.BootInAUTO),
+
             ("LogUpload",
              "Log upload",
              self.log_upload),