Plane: Throttle limits refactor

ArduPlane/Plane.h

@@ -435,6 +435,10 @@ class Plane : public AP_Vehicle {
         uint32_t accel_event_ms;
         uint32_t start_time_ms;
         bool waiting_for_rudder_neutral;
+        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.
     } takeoff_state;
 
     // ground steering controller state
@@ -1115,7 +1119,7 @@ class Plane : public AP_Vehicle {
     bool auto_takeoff_check(void);
     void takeoff_calc_roll(void);
     void takeoff_calc_pitch(void);
-    void takeoff_calc_throttle(const bool use_max_throttle=false);
+    void takeoff_calc_throttle();
     int8_t takeoff_tail_hold(void);
     int16_t get_takeoff_pitch_min_cd(void);
     void landing_gear_update(void);

ArduPlane/mode.h

@@ -813,7 +813,6 @@ class ModeTakeoff: public Mode
     AP_Int8 level_pitch;
 
     bool takeoff_mode_setup;
-    bool climb_complete;
     Location start_loc;
 
     bool _enter() override;

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.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.
             }

ArduPlane/servos.cpp

@@ -519,24 +519,17 @@ float Plane::apply_throttle_limits(float throttle_in)
         min_throttle = 0;
     }
 
+    // Handle throttle limits for takeoff conditions.
     // Query the conditions where TKOFF_THR_MAX applies.
     const bool use_takeoff_throttle =
         (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
         (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
 
     // Handle throttle limits for takeoff conditions.
     if (use_takeoff_throttle) {
-        if (aparm.takeoff_throttle_max != 0) {
-            // Replace max throttle with the takeoff max throttle setting.
-            // This is typically done to protect against long intervals of large power draw.
-            // Or (in contrast) to give some extra throttle during the initial climb.
-            max_throttle = aparm.takeoff_throttle_max.get();
-        }
-        if (aparm.takeoff_throttle_min.get() != 0) {
-            // Replace min throttle with the takeoff min throttle setting.
-            // This is typically done to protect against premature stalls close to the ground.
-            min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
-        }
+        // Read from takeoff_state
+        max_throttle = takeoff_state.throttle_lim_max;
+        min_throttle = takeoff_state.throttle_lim_min;
     } else if (landing.is_flaring()) {
         // Allow throttle cutoff when flaring.
         // This is to allow the aircraft to bleed speed faster and land with a shut off thruster.
@@ -577,6 +570,7 @@ float Plane::apply_throttle_limits(float throttle_in)
     min_throttle = MIN(min_throttle, max_throttle);
 
     // Let TECS know about the updated throttle limits.
+    // These will be taken into account on the next iteration.
     TECS_controller.set_throttle_min(0.01f*min_throttle);
     TECS_controller.set_throttle_max(0.01f*max_throttle);
     return constrain_float(throttle_in, min_throttle, max_throttle);

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.throttle_max_timer_ms = now;
         steer_state.locked_course_err = 0; // use current heading without any error offset
         return true;
     }
@@ -242,28 +243,48 @@ void Plane::takeoff_calc_pitch(void)
 }
 
 /*
- * Set the throttle limits to run at during a takeoff.
+ * Calculate the throttle limits to run at during a takeoff.
+ * These limits are meant to be used exclusively by Plane::apply_throttle_limits().
  */
-void Plane::takeoff_calc_throttle(const bool use_max_throttle) {
-    // These limits will take effect at the next run of TECS::update_pitch_throttle().
-    // Set the maximum throttle limit.
+void Plane::takeoff_calc_throttle() {
+    // Initialize the maximum throttle limit.
     if (aparm.takeoff_throttle_max != 0) {
-        TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max);
+        takeoff_state.throttle_lim_max = aparm.takeoff_throttle_max;
+    } else {
+        takeoff_state.throttle_lim_max = aparm.throttle_max;
+    }
+
+    // Initialize the minimum throttle limit.
+    if (aparm.takeoff_throttle_min != 0) {
+        takeoff_state.throttle_lim_min = aparm.takeoff_throttle_min;
+    } else {
+        takeoff_state.throttle_lim_min = aparm.throttle_min;
     }
 
+    // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff.
+    // It only applies if the timer has been started externally.
+    if (takeoff_state.throttle_max_timer_ms != 0) {
+        const uint32_t dt = AP_HAL::millis() - takeoff_state.throttle_max_timer_ms;
+        if (dt*0.001 < aparm.takeoff_throttle_max_t) {
+            takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
+        } else {
+            // Reset the timer for future use.
+            takeoff_state.throttle_max_timer_ms = 0;
+        }
+    }
+
+    // Enact the TKOFF_OPTIONS logic.
     const float current_baro_alt = barometer.get_altitude();
     const bool below_lvl_alt = current_baro_alt < auto_state.baro_takeoff_alt + mode_takeoff.level_alt;
-
     // Set the minimum throttle limit.
     const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
-    if (!use_throttle_range || !ahrs.using_airspeed_sensor() || below_lvl_alt || use_max_throttle) { // Traditional takeoff throttle limit.
-        float min_throttle = (aparm.takeoff_throttle_max != 0) ? 0.01f*aparm.takeoff_throttle_max : 0.01f*aparm.throttle_max;
-        TECS_controller.set_throttle_min(min_throttle);
-    } else { // TKOFF_MODE == 1, allow for a throttle range.
-        if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN.
-            TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min);
-        }
+    if (!use_throttle_range // We don't want to employ a throttle range.
+        || !ahrs.using_airspeed_sensor() // We don't have an airspeed sensor.
+        || below_lvl_alt // We are below TKOFF_LVL_ALT.
+        ) { // Traditional takeoff throttle limit.
+        takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
     }
+
     calc_throttle();
 }