From 471e0e206c4254595899acd7d55537c2e4032258 Mon Sep 17 00:00:00 2001
From: George Zogopoulos <tailwhipx4@gmail.com>
Date: Tue, 21 May 2024 19:34:43 +0200
Subject: [PATCH] autotest: Add catapult test

An autotest for a catapult launch has been added for Plane.
An auxiliary `set_servo` method has been added to `vehicle_test_suite.py`.
For now, it is placed as known failing test.
---
 .../CatapultTakeoff/catapult.txt              | 13 +++++++
 Tools/autotest/arduplane.py                   | 39 +++++++++++++++++++
 Tools/autotest/vehicle_test_suite.py          |  4 ++
 3 files changed, 56 insertions(+)
 create mode 100644 Tools/autotest/ArduPlane_Tests/CatapultTakeoff/catapult.txt

diff --git a/Tools/autotest/ArduPlane_Tests/CatapultTakeoff/catapult.txt b/Tools/autotest/ArduPlane_Tests/CatapultTakeoff/catapult.txt
new file mode 100644
index 00000000000000..0aa9cd624db827
--- /dev/null
+++ b/Tools/autotest/ArduPlane_Tests/CatapultTakeoff/catapult.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	100.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361229	149.163025	100.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364563	149.163773	100.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	1.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 ba7a2eed20db4a..50bdd8b4a50f1a 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -4160,6 +4160,43 @@ def LandingDrift(self):
 
         self.wait_disarmed(timeout=180)
 
+    def CatapultTakeoff(self):
+        '''Test that a catapult takeoff works correctly'''
+
+        self.customise_SITL_commandline(
+            [],
+            model='plane-catapult',
+            defaults_filepath=self.model_defaults_filepath("plane")
+        )
+        self.set_parameters({
+            "TKOFF_THR_MINACC": 3.0,
+            "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
+        })
+
+        # Record desired target speed.
+        min_speed = self.get_parameter("AIRSPEED_CRUISE")
+        max_throttle = self.get_parameter("TKOFF_THR_MAX")
+        if max_throttle == 0:
+            max_throttle = self.get_parameter("THR_MAX")
+
+        # Load and start mission.
+        self.load_mission("catapult.txt", strict=True)
+        self.change_mode('AUTO')
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        # Throw the catapult.
+        self.set_servo(7, 2000)
+
+        # Ensure that after the airspeed has been reached, the throttle recedes.
+        max_pwm = max_throttle*20
+        self.wait_airspeed(min_speed-1, 100, minimum_duration=5, timeout=10)
+        self.wait_servo_channel_value(3, max_pwm, timeout=0.1, comparator=operator.lt)
+        # Wait for landing waypoint.
+        self.wait_current_waypoint(11, timeout=1200)
+        self.wait_disarmed(120)
+
     def DCMFallback(self):
         '''Really annoy the EKF and force fallback'''
         self.reboot_sitl()
@@ -5426,6 +5463,7 @@ def tests(self):
             self.AHRS_ORIENTATION,
             self.AHRSTrim,
             self.LandingDrift,
+            self.CatapultTakeoff,
             self.ForcedDCM,
             self.DCMFallback,
             self.MAVFTP,
@@ -5474,4 +5512,5 @@ def disabled_tests(self):
             "LandingDrift": "Flapping test. See https://github.com/ArduPilot/ardupilot/issues/20054",
             "InteractTest": "requires user interaction",
             "ClimbThrottleSaturation": "requires https://github.com/ArduPilot/ardupilot/pull/27106 to pass",
+            "CatapultTakeoff": "Known bug. See https://github.com/ArduPilot/ardupilot/issues/27147",
         }
diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py
index b20e4dc37b4e33..f28bbaa71a0188 100644
--- a/Tools/autotest/vehicle_test_suite.py
+++ b/Tools/autotest/vehicle_test_suite.py
@@ -5406,6 +5406,10 @@ def set_rc(self, chan, pwm, timeout=20):
         """Setup a simulated RC control to a PWM value"""
         self.set_rc_from_map({chan: pwm}, timeout=timeout)
 
+    def set_servo(self, chan, pwm):
+        """Replicate the functionality of MAVProxy: servo set <ch> <pwm>"""
+        self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=chan, p2=pwm)
+
     def location_offset_ne(self, location, north, east):
         '''move location in metres'''
         print("old: %f %f" % (location.lat, location.lng))