Repository: ArduPilot/ardupilot
Pull request #29860: Master: Prevent rotation pitch calculations from running post-rotation
Author: login: Georacer
Labels: name: MergeOnCIPass
Created: 2025-04-22T09:14:45Z | Merged: 2025-04-28T10:32:24Z

For details, see https://github.com/ArduPilot/ardupilot/pull/29834

The new autotest has been renamed in this iteration, to better fit its context.


Diff:
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 5a76293b772ee1..1544b90a678242 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -559,6 +559,9 @@ class Plane : public AP_Vehicle {
 
         // last home altitude for detecting changes
         int32_t last_home_alt_cm;
+
+        // have we finished the takeoff ratation (when it applies)?
+        bool rotation_complete;
     } auto_state;
 
 #if AP_SCRIPTING_ENABLED
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index ea47f2dc465a46..a2bc18ac85e84d 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -384,6 +384,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
     next_WP_loc.lat = home.lat + 10;
     next_WP_loc.lng = home.lng + 10;
     auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction.
+    auto_state.rotation_complete = false;
     auto_state.height_below_takeoff_to_level_off_cm = 0;
     // Flag also used to override "on the ground" throttle disable
 
diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
index 3a4b3cc66252c0..226d954dba1bd1 100644
--- a/ArduPlane/mode.cpp
+++ b/ArduPlane/mode.cpp
@@ -74,6 +74,9 @@ bool Mode::enter()
     // disable taildrag takeoff on mode change
     plane.auto_state.fbwa_tdrag_takeoff_mode = false;
 
+    // wipe the takeoff rotation complete state
+    plane.auto_state.rotation_complete = false;
+
     // start with previous WP at current location
     plane.prev_WP_loc = plane.current_loc;
 
diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp
index c07d9f293f52f0..d7128c727345d7 100644
--- a/ArduPlane/takeoff.cpp
+++ b/ArduPlane/takeoff.cpp
@@ -190,7 +190,7 @@ void Plane::takeoff_calc_pitch(void)
 {
     // First see if TKOFF_ROTATE_SPD applies.
     // This will set the pitch for the first portion of the takeoff, up until cruise speed is reached.
-    if (g.takeoff_rotate_speed > 0) {
+    if (!auto_state.rotation_complete && g.takeoff_rotate_speed > 0) {
         // A non-zero rotate speed is recommended for ground takeoffs.
         if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
             // We have not reached rotate speed, use the specified takeoff target pitch angle.
@@ -209,6 +209,7 @@ void Plane::takeoff_calc_pitch(void)
             return;
         }
     }
+    auto_state.rotation_complete = true;
 
     // We are now past the rotation.
     // Initialize pitch limits for TECS.
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 511c90dad9a30b..c9bf2cc784e13f 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -5045,6 +5045,9 @@ def TakeoffGround(self):
 
         self.set_parameters({
             "TKOFF_ROTATE_SPD": 15.0,
+            "TKOFF_PLIM_SEC": 0,  # Ensure TKOFF_PLIM_SEC doesn't interfere with the test.
+            "TKOFF_DIST": 500,  # Ensure stall prevention doesn't interfere with the test.
+            "TKOFF_ALT": 100  # Ditto
         })
         self.change_mode("TAKEOFF")
 
@@ -5059,7 +5062,7 @@ def TakeoffGround(self):
             raise NotAchievedException(f"Did not achieve correct takeoff pitch ({nav_pitch}).")
 
         # Check whether we've achieved correct target pitch after rotation.
-        self.wait_groundspeed(23, 24)
+        self.wait_groundspeed(24, 25)
         m = self.assert_receive_message('NAV_CONTROLLER_OUTPUT', timeout=5)
         nav_pitch = m.nav_pitch
         if nav_pitch > 15.1 or nav_pitch < 14.9:
@@ -5135,6 +5138,39 @@ def TakeoffBadLevelOff(self):
 
         self.disarm_vehicle(force=True)
 
+    def TakeoffLevelOffWind(self):
+        '''Ensure the level-off functionality works.'''
+        '''
+        This is primarily targeted to test whether the level-off angle works
+        correctly even though the groundspeed eventually drops in face of wind.
+        '''
+        tkoff_alt = 100.
+        self.set_parameters({
+            "TKOFF_ROTATE_SPD": 15.0,
+            "TKOFF_ALT": tkoff_alt,
+            "TKOFF_DIST": 500,  # Ensure stall prevention doesn't interfere with the test.
+            "TKOFF_PLIM_SEC": 5  # Give some more time to detect the level-off.
+        })
+        self.change_mode("TAKEOFF")
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        # Wait until we're well past the rotation.
+        self.wait_groundspeed(24, 25)
+
+        self.set_parameters({
+            "SIM_WIND_DIR": 0.0,  # Set North wind.
+            "SIM_WIND_SPD": 10.0  # Enough to bring groundspeed below cruise speed.
+        })
+
+        self.wait_altitude(tkoff_alt-10, tkoff_alt, relative=True)
+        self.wait_level_flight(accuracy=10, timeout=1)  # Ensure we have roughly level-off.
+        self.delay_sim_time(5)
+
+        self.change_mode('AUTOLAND')
+        self.wait_disarmed(timeout=180)
+
     def DCMFallback(self):
         '''Really annoy the EKF and force fallback'''
         self.reboot_sitl()
@@ -5172,7 +5208,8 @@ def ForcedDCM(self):
         self.wait_statustext("DCM Active", check_context=True)
         self.context_stop_collecting('STATUSTEXT')
 
-        self.fly_home_land_and_disarm()
+        self.change_mode('AUTOLAND')
+        self.wait_disarmed(timeout=180)
 
     def EFITest(self, efi_type, name, sim_name, check_fuel_flow=True):
         '''method to be called by EFI tests'''
@@ -7301,6 +7338,7 @@ def tests1b(self):
             self.TakeoffGround,
             self.TakeoffIdleThrottle,
             self.TakeoffBadLevelOff,
+            self.TakeoffLevelOffWind,
             self.ForcedDCM,
             self.DCMFallback,
             self.MAVFTP,


Reviews:
user: login: peterbarker
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-23T08:57:55Z
user: login: tridge
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-24T02:04:42Z
user: login: tridge
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-04-24T02:05:36Z

Review comments:
diff_hunk: @@ -5135,6 +5138,38 @@ def TakeoffBadLevelOff(self):
 
         self.disarm_vehicle(force=True)
 
+    def TakeoffLevelOffWind(self):
+        '''Ensure the level-off functionality works.'''
+        '''
+        This is primarily targeted to test whether the level-off angle works
+        correctly even though the groundspeed eventually drops in face of wind.
+        '''
+        tkoff_alt = 100.
+        self.set_parameters({
+            "TKOFF_ROTATE_SPD": 15.0,
+            "TKOFF_ALT": tkoff_alt,
+            "TKOFF_DIST": 500,  # Ensure stall prevention doesn't interfere with the test.
+            "TKOFF_PLIM_SEC": 5  # Give some more time to detect the level-off.
+        })
+        self.change_mode("TAKEOFF")
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        # Wait until we're well past the rotation.
+        self.wait_groundspeed(24, 25)
+
+        self.set_parameters({
+            "SIM_WIND_DIR": 0.0,  # Set North wind.
+            "SIM_WIND_SPD": 10.0  # Enough to bring groundspeed below cruise speed.
+        })
+
+        self.wait_altitude(tkoff_alt-10, tkoff_alt, relative=True)
+        self.wait_level_flight(accuracy=10, timeout=1)  # Ensure we have roughly level-off.
+        self.delay_sim_time(5)
+
+        self.disarm_vehicle(force=True)  # No need to test further.
path: Tools/autotest/arduplane.py
user: login: peterbarker
body: maybe fly home and disarm?

created_at: 2025-04-23T08:57:18Z
author_association: CONTRIBUTOR
diff_hunk: @@ -5135,6 +5138,38 @@ def TakeoffBadLevelOff(self):
 
         self.disarm_vehicle(force=True)
 
+    def TakeoffLevelOffWind(self):
+        '''Ensure the level-off functionality works.'''
+        '''
+        This is primarily targeted to test whether the level-off angle works
+        correctly even though the groundspeed eventually drops in face of wind.
+        '''
+        tkoff_alt = 100.
+        self.set_parameters({
+            "TKOFF_ROTATE_SPD": 15.0,
+            "TKOFF_ALT": tkoff_alt,
+            "TKOFF_DIST": 500,  # Ensure stall prevention doesn't interfere with the test.
+            "TKOFF_PLIM_SEC": 5  # Give some more time to detect the level-off.
+        })
+        self.change_mode("TAKEOFF")
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        # Wait until we're well past the rotation.
+        self.wait_groundspeed(24, 25)
+
+        self.set_parameters({
+            "SIM_WIND_DIR": 0.0,  # Set North wind.
+            "SIM_WIND_SPD": 10.0  # Enough to bring groundspeed below cruise speed.
+        })
+
+        self.wait_altitude(tkoff_alt-10, tkoff_alt, relative=True)
+        self.wait_level_flight(accuracy=10, timeout=1)  # Ensure we have roughly level-off.
path: Tools/autotest/arduplane.py
user: login: peterbarker
body: I might rejig `wait_for_level_flight` to add a `minimum_duration` thing, because this looks like it really needs it

created_at: 2025-04-23T08:57:51Z
author_association: CONTRIBUTOR
diff_hunk: @@ -5135,6 +5138,38 @@ def TakeoffBadLevelOff(self):
 
         self.disarm_vehicle(force=True)
 
+    def TakeoffLevelOffWind(self):
+        '''Ensure the level-off functionality works.'''
+        '''
+        This is primarily targeted to test whether the level-off angle works
+        correctly even though the groundspeed eventually drops in face of wind.
+        '''
+        tkoff_alt = 100.
+        self.set_parameters({
+            "TKOFF_ROTATE_SPD": 15.0,
+            "TKOFF_ALT": tkoff_alt,
+            "TKOFF_DIST": 500,  # Ensure stall prevention doesn't interfere with the test.
+            "TKOFF_PLIM_SEC": 5  # Give some more time to detect the level-off.
+        })
+        self.change_mode("TAKEOFF")
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        # Wait until we're well past the rotation.
+        self.wait_groundspeed(24, 25)
+
+        self.set_parameters({
+            "SIM_WIND_DIR": 0.0,  # Set North wind.
+            "SIM_WIND_SPD": 10.0  # Enough to bring groundspeed below cruise speed.
+        })
+
+        self.wait_altitude(tkoff_alt-10, tkoff_alt, relative=True)
+        self.wait_level_flight(accuracy=10, timeout=1)  # Ensure we have roughly level-off.
+        self.delay_sim_time(5)
+
+        self.disarm_vehicle(force=True)  # No need to test further.
path: Tools/autotest/arduplane.py
user: login: tridge
body: we should start using AUTOLAND mode for that
created_at: 2025-04-24T02:04:42Z
author_association: CONTRIBUTOR

PR comments:
user: login: tridge
created_at: 2025-04-28T09:11:03Z
body: @Georacer @peterbarker I've pushed a change that I think will fix the failing tests
author_association: CONTRIBUTOR
user: login: Georacer
created_at: 2025-04-28T21:45:59Z
body: > @Georacer @peterbarker I've pushed a change that I think will fix the failing tests

Thanks, I appreciate it. I was about to come and fix it but you beat me to it. Sorry I didn't do it in time.
I see you prefer letting the plane fly home, instead of terminating after the test at hand.
I usually do it to save CI time.
author_association: CONTRIBUTOR