From 768b2eabc48ff2da4053745ffe489d10ea4b4e29 Mon Sep 17 00:00:00 2001
From: George Zogopoulos <tailwhipx4@gmail.com>
Date: Fri, 22 Nov 2024 11:36:22 +0100
Subject: [PATCH] autotest: Created test for unsuccessful takeoff level off

---
 .../TakeoffBadLevelOff/flaps_tkoff_50.txt     | 13 ++++++
 Tools/autotest/arduplane.py                   | 41 +++++++++++++++++++
 2 files changed, 54 insertions(+)
 create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt

diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt b/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt
new file mode 100644
index 0000000000..c2f52f8929
--- /dev/null
+++ b/Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt
@@ -0,0 +1,13 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.390015	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361279	149.164230	50.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361229	149.163025	80.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364563	149.163773	80.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364384	149.164795	80.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361027	149.164093	80.000000	1
+6	0	0	177	2.000000	3.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	189	0.000000	0.000000	0.000000	0.000000	-35.362915	149.162613	60.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363136	149.162750	60.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365467	149.164215	55.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365009	149.165482	39.889999	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363041	149.165222	0.000000	1
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 0ce13e5c3e..7fabd6cd07 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -4860,6 +4860,46 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
 
         self.fly_home_land_and_disarm()
 
+    def TakeoffBadLevelOff(self):
+        '''Ensure that the takeoff can be completed under 0 pitch demand.'''
+        '''
+        When using no airspeed, the pitch level-off will eventually command 0
+        pitch demand. Ensure that the plane can climb the final 2m to deem the
+        takeoff complete.
+        '''
+
+        self.customise_SITL_commandline(
+            [],
+            model='plane-catapult',
+            defaults_filepath=self.model_defaults_filepath("plane")
+        )
+        self.set_parameters({
+            "ARSPD_USE": 0.0,
+            "PTCH_TRIM_DEG": -10.0,
+            "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
+            "TKOFF_ALT": 50.0,
+            "TKOFF_DIST": 1000.0,
+            "TKOFF_THR_MAX": 75.0,
+            "TKOFF_THR_MINACC": 3.0,
+        })
+
+        self.load_mission("flaps_tkoff_50.txt")
+        self.change_mode('AUTO')
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        # Throw the catapult.
+        self.set_servo(7, 2000)
+
+        # Wait until we've reached the takeoff altitude.
+        target_alt = 50
+        self.wait_altitude(target_alt-1, target_alt+1, relative=True, timeout=30)
+
+        self.delay_sim_time(5)
+
+        self.disarm_vehicle(force=True)
+
     def DCMFallback(self):
         '''Really annoy the EKF and force fallback'''
         self.reboot_sitl()
@@ -6452,6 +6492,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
             self.TakeoffTakeoff4,
             self.TakeoffGround,
             self.TakeoffIdleThrottle,
+            self.TakeoffBadLevelOff,
             self.ForcedDCM,
             self.DCMFallback,
             self.MAVFTP,