Plane: Fix inability to climb the last few meters of takeoff

ArduPlane/Plane.h

@@ -453,7 +453,7 @@ class Plane : public AP_Vehicle {
         float throttle_lim_max;
         float throttle_lim_min;
         uint32_t throttle_max_timer_ms;
-        // Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
+        uint32_t level_off_start_time_ms;
     } takeoff_state;
 
     // ground steering controller state
@@ -1147,6 +1147,7 @@ class Plane : public AP_Vehicle {
     int16_t get_takeoff_pitch_min_cd(void);
     void landing_gear_update(void);
     bool check_takeoff_timeout(void);
+    bool check_takeoff_timeout_level_off(void);
 
     // avoidance_adsb.cpp
     void avoidance_adsb_update(void);

ArduPlane/commands_logic.cpp

@@ -587,7 +587,10 @@ bool Plane::verify_takeoff()
 
     // see if we have reached takeoff altitude
     int32_t relative_alt_cm = adjusted_relative_altitude_cm();
-    if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
+    if (
+        relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached
+        plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out
+        ) {
         gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
                           (double)(relative_alt_cm*0.01f));
         steer_state.hold_course_cd = -1;

ArduPlane/mode_takeoff.cpp

@@ -119,6 +119,7 @@ void ModeTakeoff::update()
                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
                                 alt, dist, direction);
                 plane.takeoff_state.start_time_ms = millis();
+                plane.takeoff_state.level_off_start_time_ms = 0;
                 plane.takeoff_state.throttle_max_timer_ms = millis();
                 takeoff_mode_setup = true;
                 plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
@@ -157,6 +158,12 @@ void ModeTakeoff::update()
         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
     }
 
+    // If we have timed-out on the attempt to close the final few meters
+    // during pitch level-off, continue to NORMAL flight stage.
+    if (plane.check_takeoff_timeout_level_off()) {
+        plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
+    }
+
     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
         //below TKOFF_ALT
         plane.takeoff_calc_roll();

ArduPlane/takeoff.cpp

@@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void)
         takeoff_state.launchTimerStarted = false;
         takeoff_state.last_tkoff_arm_time = 0;
         takeoff_state.start_time_ms = now;
+        takeoff_state.level_off_start_time_ms = 0;
         takeoff_state.throttle_max_timer_ms = now;
         steer_state.locked_course_err = 0; // use current heading without any error offset
         return true;
@@ -316,6 +317,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void)
                 // make a note of that altitude to use it as a start height for scaling
                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100));
                 auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
+                takeoff_state.level_off_start_time_ms = AP_HAL::millis();
             }
         }
     }
@@ -376,9 +378,8 @@ void Plane::landing_gear_update(void)
 #endif
 
 /*
- check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout
+ check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred
 */
-
 bool Plane::check_takeoff_timeout(void)
 {
     if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
@@ -400,3 +401,17 @@ bool Plane::check_takeoff_timeout(void)
      return false;
 }
 
+/*
+ check if the pitch level-off time has expired; returns true if timeout has occurred
+*/
+bool Plane::check_takeoff_timeout_level_off(void)
+{
+    if (takeoff_state.level_off_start_time_ms > 0) {
+        // A takeoff is in progress.
+        uint32_t now = AP_HAL::millis();
+        if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) {
+            return true;
+        }
+    }
+    return false;
+}

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

Tools/autotest/arduplane.py

@@ -4860,6 +4860,46 @@ def TakeoffIdleThrottle(self):
 
         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 @@ def tests1b(self):
             self.TakeoffTakeoff4,
             self.TakeoffGround,
             self.TakeoffIdleThrottle,
+            self.TakeoffBadLevelOff,
             self.ForcedDCM,
             self.DCMFallback,
             self.MAVFTP,