TECS: Fix airspeed control during takeoff

ArduPlane/Attitude.cpp

@@ -448,6 +448,10 @@ void Plane::stabilize()
 }
 
 
+/*
+ * Set the throttle output.
+ * This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc.
+*/
 void Plane::calc_throttle()
 {
     if (aparm.throttle_cruise <= 1) {
@@ -458,6 +462,7 @@ void Plane::calc_throttle()
         return;
     }
 
+    // Read the TECS throttle output and set it to the throttle channel.
     float commanded_throttle = TECS_controller.get_throttle_demand();
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
 }

ArduPlane/Parameters.cpp

@@ -142,12 +142,28 @@ const AP_Param::Info Plane::var_info[] = {
 
     // @Param: TKOFF_THR_MAX_T
     // @DisplayName: Takeoff throttle maximum time
-    // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff without an airspeed sensor. If an airspeed sensor is being used then the throttle is set to maximum until the takeoff airspeed is reached.
+    // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff.
     // @Units: s
     // @Range: 0 10
     // @Increment: 0.5
     // @User: Standard
     ASCALAR(takeoff_throttle_max_t,     "TKOFF_THR_MAX_T",  4),
+
+    // @Param: TKOFF_THR_MIN
+    // @DisplayName: Takeoff minimum throttle
+    // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_OPTIONS bit 0 is set. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead.
+    // @Units: %
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+    ASCALAR(takeoff_throttle_min,       "TKOFF_THR_MIN",    60),
+
+    // @Param: TKOFF_OPTIONS
+    // @DisplayName: Takeoff options
+    // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 
+    // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor.
+    // @User: Advanced
+    ASCALAR(takeoff_options,               "TKOFF_OPTIONS",       0),
 
     // @Param: TKOFF_TDRAG_ELEV
     // @DisplayName: Takeoff tail dragger elevator

ArduPlane/Parameters.h

@@ -356,6 +356,8 @@ class Parameters {
         k_param_acro_yaw_rate,
         k_param_takeoff_throttle_max_t,
         k_param_autotune_options,
+        k_param_takeoff_throttle_min,
+        k_param_takeoff_options,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -1114,6 +1114,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);
     int8_t takeoff_tail_hold(void);
     int16_t get_takeoff_pitch_min_cd(void);
     void landing_gear_update(void);

ArduPlane/altitude.cpp

@@ -513,9 +513,9 @@ int32_t Plane::adjusted_altitude_cm(void)
 }
 
 /*
-  return home-relative altitude adjusted for ALT_OFFSET This is useful
+  return home-relative altitude adjusted for ALT_OFFSET. This is useful
   during long flights to account for barometer changes from the GCS,
-  or to adjust the flying height of a long mission
+  or to adjust the flying height of a long mission.
  */
 int32_t Plane::adjusted_relative_altitude_cm(void)
 {

ArduPlane/commands_logic.cpp

@@ -391,7 +391,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_speed_time_ms = 0;
-    auto_state.takeoff_complete = false;                            // set flag to use gps ground course during TO.  IMU will be doing yaw drift correction
+    auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction.
     auto_state.height_below_takeoff_to_level_off_cm = 0;
     // Flag also used to override "on the ground" throttle disable
 

ArduPlane/mode_takeoff.cpp

@@ -22,11 +22,11 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = {
     // @Increment: 1
     // @Units: m
     // @User: Standard
-    AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 5),
+    AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 10),
 
     // @Param: LVL_PITCH
     // @DisplayName: Takeoff mode altitude initial pitch
-    // @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT
+    // @Description: This is the target pitch during the takeoff.
     // @Range: 0 30
     // @Increment: 1
     // @Units: deg
@@ -148,19 +148,19 @@ void ModeTakeoff::update()
 
     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
         //below TAKOFF_LVL_ALT
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0);
         plane.takeoff_calc_roll();
         plane.takeoff_calc_pitch();
+        plane.takeoff_calc_throttle(true);
     } else {
-        if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering
+        if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering
 #if AP_FENCE_ENABLED
             plane.fence.auto_enable_fence_after_takeoff();
 #endif
             plane.calc_nav_roll();
             plane.calc_nav_pitch();
             plane.calc_throttle();
         } else { // still climbing to TAKEOFF_ALT; may be loitering
-            plane.calc_throttle();
+            plane.takeoff_calc_throttle();
             plane.takeoff_calc_roll();
             plane.takeoff_calc_pitch();
         }

ArduPlane/servos.cpp

@@ -499,47 +499,73 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
 #endif // #if AP_BATTERY_WATT_MAX_ENABLED
 
 /*
-  Apply min/max limits to throttle
+  Apply min/max safety limits to throttle.
  */
 float Plane::apply_throttle_limits(float throttle_in)
 {
-    // convert 0 to 100% (or -100 to +100) into PWM
+    // Pull the base throttle limits.
+    // These are usually set to map the ESC operating range.
     int8_t min_throttle = aparm.throttle_min.get();
     int8_t max_throttle = aparm.throttle_max.get();
 
 #if AP_ICENGINE_ENABLED
-    // apply idle governor
+    // Apply idle governor.
     g2.ice_control.update_idle_governor(min_throttle);
 #endif
 
+    // If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0.
     if (min_throttle < 0 && !allow_reverse_thrust()) {
         // reverse thrust is available but inhibited.
         min_throttle = 0;
     }
 
-    const bool use_takeoff_throttle_max =
+    // Query the conditions where TKOFF_THR_MAX applies.
+    const bool use_takeoff_throttle =
 #if HAL_QUADPLANE_ENABLED
         quadplane.in_transition() ||
 #endif
         (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
         (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
 
-    if (use_takeoff_throttle_max) {
+    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();
         }
+        // Do not allow min throttle to go below a lower threshold.
+        // This is typically done to protect against premature stalls close to the ground.
+        const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
+        if (!use_throttle_range || !ahrs.using_airspeed_sensor()) {
+            // Use a constant max throttle throughout the takeoff or when airspeed readings are not available.
+            min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get());
+        } else if (use_throttle_range) { // Use a throttle range through the takeoff.
+            if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1.
+                min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
+            }
+        }
     } 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.
         min_throttle = 0;
     }
 
-    // compensate for battery voltage drop
+    // Compensate the limits for battery voltage drop.
+    // This relaxes the limits when the battery is getting depleted.
     g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle);
 
 #if AP_BATTERY_WATT_MAX_ENABLED
-    // apply watt limiter
+    // Ensure that the power draw limits are not exceeded.
     throttle_watt_limiter(min_throttle, max_throttle);
 #endif
 
+    // Do a sanity check on them. Constrain down if necessary.
+    min_throttle = MIN(min_throttle, max_throttle);
+
+    // Let TECS know about the updated throttle limits.
+    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

@@ -219,7 +219,29 @@ void Plane::takeoff_calc_pitch(void)
 }
 
 /*
- * get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
+ * Set the throttle limits to run at during a takeoff.
+ */
+void Plane::takeoff_calc_throttle(const bool use_max_throttle) {
+    // This setting will take effect at the next run of TECS::update_pitch_throttle().
+
+    // Set the maximum throttle limit.
+    if (aparm.takeoff_throttle_max != 0) {
+        TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max);
+    }
+
+    // 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() || use_max_throttle) { // Traditional takeoff throttle limit.
+        TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max);
+    } 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);
+        }
+    }
+    calc_throttle();
+}
+
+/* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
  */
 int16_t Plane::get_takeoff_pitch_min_cd(void)
 {

Tools/autotest/ArduPlane_Tests/TakeoffAuto1/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

Tools/autotest/ArduPlane_Tests/TakeoffAuto2/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

Tools/autotest/ArduPlane_Tests/TakeoffAuto3/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

Tools/autotest/ArduPlane_Tests/TakeoffAuto4/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