From b6f1891f9f622232e0e20052c69c77fc907c998e Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 6 Dec 2022 13:57:28 +1100
Subject: [PATCH] autotest: add test for loiter-to-alt

---
 .../MAV_CMD_NAV_LOITER_TO_ALT/mission.txt       | 10 ++++++++++
 Tools/autotest/quadplane.py                     | 17 +++++++++++++++++
 2 files changed, 27 insertions(+)
 create mode 100644 Tools/autotest/ArduPlane_Tests/MAV_CMD_NAV_LOITER_TO_ALT/mission.txt

diff --git a/Tools/autotest/ArduPlane_Tests/MAV_CMD_NAV_LOITER_TO_ALT/mission.txt b/Tools/autotest/ArduPlane_Tests/MAV_CMD_NAV_LOITER_TO_ALT/mission.txt
new file mode 100644
index 0000000000..2edfbcbdc2
--- /dev/null
+++ b/Tools/autotest/ArduPlane_Tests/MAV_CMD_NAV_LOITER_TO_ALT/mission.txt
@@ -0,0 +1,10 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-27.274439	151.290064	343.100006	1
+1	0	3	84	0.000000	0.000000	0.000000	0.000000	-27.272768	151.288529	30.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-27.273631	151.289380	30.000000	1
+3	0	3	31	0.000000	0.000000	0.000000	0.000000	-27.275714	151.291065	120.000000	1
+4	0	3	17	0.000000	0.000000	0.000000	0.000000	-27.278535	151.298429	120.000000	1
+5	0	3	31	0.000000	-100.000000	0.000000	0.000000	-27.277746	151.298205	50.000000	1
+6	0	3	16	0.000000	0.000000	0.000000	0.000000	-27.274219	151.294322	40.000000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-27.273160	151.290969	30.000000	1
+8	0	3	85	0.000000	0.000000	0.000000	0.000000	-27.272804	151.288573	0.000000	1
diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index a18fe353bc..b1200e2b56 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -933,6 +933,22 @@ class AutoTestQuadPlane(AutoTest):
         self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -1)
         self.disarm_vehicle()
 
+    def MAV_CMD_NAV_LOITER_TO_ALT(self, target_system=1, target_component=1):
+        '''ensure consecutive loiter to alts work'''
+        self.load_mission('mission.txt')
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.wait_current_waypoint(4, timeout=240)
+        self.assert_altitude(120, accuracy=5, relative=True)
+        self.delay_sim_time(30)
+        self.assert_altitude(120, accuracy=5, relative=True)
+        self.set_current_waypoint(5)
+        self.wait_altitude(altitude_min=65, altitude_max=75, relative=True)
+        if self.current_waypoint() != 5:
+            raise NotAchievedException("Should pass 90m before passing waypoint 5")
+        self.wait_disarmed(timeout=300)
+
     def Mission(self):
         '''fly the OBC 2016 mission in Dalby'''
         self.fly_mission(
@@ -962,5 +978,6 @@ class AutoTestQuadPlane(AutoTest):
             self.MidAirDisarmDisallowed,
             self.BootInAUTO,
             self.Ship,
+            self.MAV_CMD_NAV_LOITER_TO_ALT,
         ])
         return ret