From ecbad5b35a74fe797e9917fd6a2ab617a3682292 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 17 Sep 2023 08:10:05 +1000
Subject: [PATCH] autotest: add test for ArduPlane fly-inverted mission item

---
 Tools/autotest/arduplane.py | 61 +++++++++++++++++++++++++++++++++++++
 1 file changed, 61 insertions(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index c3d81aae70..8e03b234d2 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -4819,6 +4819,66 @@ class AutoTestPlane(AutoTest):
         self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd)
         self._MAV_CMD_PREFLIGHT_CALIBRATION(self.run_cmd_int)
 
+    def MAV_CMD_DO_INVERTED_FLIGHT(self):
+        '''fly upside-down mission item'''
+        alt = 30
+        wps = self.create_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, alt),
+            self.create_MISSION_ITEM_INT(
+                mavutil.mavlink.MAV_CMD_DO_INVERTED_FLIGHT,
+                p1=1,
+            ),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, alt),
+            self.create_MISSION_ITEM_INT(
+                mavutil.mavlink.MAV_CMD_DO_INVERTED_FLIGHT,
+                p1=0,
+            ),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1200, 0, alt),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+        self.check_mission_upload_download(wps)
+
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+
+        self.arm_vehicle()
+
+        self.wait_current_waypoint(2)  # upright flight
+        self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", {
+            "nav_roll": 0,
+            "nav_pitch": 0,
+        }, epsilon=5)
+
+        def check_altitude(mav, m):
+            global initial_airspeed_threshold_reached
+            m_type = m.get_type()
+            if m_type != 'GLOBAL_POSITION_INT':
+                return
+            if abs(30 - m.relative_alt * 0.001) > 15:
+                raise NotAchievedException("Bad altitude while flying inverted")
+
+        self.context_push()
+        self.install_message_hook_context(check_altitude)
+
+        self.wait_current_waypoint(4)  # inverted flight
+        self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", {
+            "nav_roll": 180,
+            "nav_pitch": 9,
+        }, epsilon=10,)
+
+        self.wait_current_waypoint(6)  # upright flight
+        self.wait_message_field_values("NAV_CONTROLLER_OUTPUT", {
+            "nav_roll": 0,
+            "nav_pitch": 0,
+        }, epsilon=5)
+
+        self.context_pop()  # remove the check_altitude call
+
+        self.wait_current_waypoint(7)
+
+        self.fly_home_land_and_disarm()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -4911,6 +4971,7 @@ class AutoTestPlane(AutoTest):
             self.ExternalPositionEstimate,
             self.MAV_CMD_GUIDED_CHANGE_ALTITUDE,
             self.MAV_CMD_PREFLIGHT_CALIBRATION,
+            self.MAV_CMD_DO_INVERTED_FLIGHT,
         ])
         return ret