Plane: added TKOFF_THR_MIN for quadplane SLT

ArduPlane/Parameters.cpp

@@ -148,6 +148,15 @@ const AP_Param::Info Plane::var_info[] = {
     // @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_TDRAG_ELEV
     // @DisplayName: Takeoff tail dragger elevator

ArduPlane/Parameters.h

@@ -357,6 +357,7 @@ class Parameters {
         k_param_acro_yaw_rate,
         k_param_takeoff_throttle_max_t,
         k_param_autotune_options,
+        k_param_takeoff_throttle_min,
     };
 
     AP_Int16 format_version;

ArduPlane/quadplane.cpp

@@ -4064,6 +4064,21 @@ bool QuadPlane::in_transition(void) const
     return available() && transition->active();
 }
 
+/*
+  return true if we are in a fwd transition for a SLT quadplane
+  and in an auto-throttle mode
+ */
+bool QuadPlane::in_slt_fwd_transition(void) const
+{
+    if (!in_transition() || tiltrotor.enabled() || tailsitter.enabled()) {
+        return false;
+    }
+    if (!plane.control_mode->does_auto_throttle()) {
+        return false;
+    }
+    return transition->in_fwd_transition();
+}
+
 /*
   calculate current stopping distance for a quadplane in fixed wing flight
  */
@@ -4554,6 +4569,15 @@ void SLT_Transition::set_last_fw_pitch()
     last_fw_nav_pitch_cd = plane.nav_pitch_cd;
 }
 
+/*
+  return true if in a forward transition
+ */
+bool SLT_Transition::in_fwd_transition() const
+{
+    return transition_state == TRANSITION_AIRSPEED_WAIT ||
+        transition_state == TRANSITION_TIMER;
+}
+
 void SLT_Transition::force_transition_complete() {
     transition_state = TRANSITION_DONE; 
     in_forced_transition = false;

ArduPlane/quadplane.h

@@ -106,6 +106,12 @@ class QuadPlane
     */
     bool in_transition(void) const;
 
+    /*
+      return true if we are in a fwd transition for a SLT quadplane
+      and in an auto-throttle mode
+     */
+    bool in_slt_fwd_transition(void) const;
+
     bool handle_do_vtol_transition(enum MAV_VTOL_STATE state) const;
 
     bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);

ArduPlane/servos.cpp

@@ -517,7 +517,7 @@ float Plane::apply_throttle_limits(float throttle_in)
 
     const bool use_takeoff_throttle_max =
 #if HAL_QUADPLANE_ENABLED
-        quadplane.in_transition() ||
+        quadplane.in_slt_fwd_transition() ||
 #endif
         (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
         (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
@@ -538,7 +538,21 @@ float Plane::apply_throttle_limits(float throttle_in)
     throttle_watt_limiter(min_throttle, max_throttle);
 #endif
 
-    return constrain_float(throttle_in, min_throttle, max_throttle);
+    /*
+      apply TKOFF_THR_MIN if enabled and in an auto-throttle forward
+      transition
+     */
+    if (use_takeoff_throttle_max) {
+        const auto tmin_thr = aparm.takeoff_throttle_min;
+        if (tmin_thr > throttle_in) {
+            throttle_in = MAX(throttle_in, tmin_thr);
+            plane.TECS_controller.reset_throttle_I_cruise();
+        }
+    }
+
+    auto throttle = constrain_float(throttle_in, min_throttle, max_throttle);
+
+    return throttle;
 }
 
 /*

ArduPlane/tailsitter.cpp

@@ -1034,4 +1034,12 @@ bool Tailsitter_Transition::allow_weathervane()
     return !tailsitter.in_vtol_transition() && (vtol_limit_start_ms == 0);
 }
 
+/*
+  return true if in a forward transition
+ */
+bool Tailsitter_Transition::in_fwd_transition() const
+{
+    return transition_state == TRANSITION_ANGLE_WAIT_FW;
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/tailsitter.h

@@ -189,6 +189,8 @@ friend class Tailsitter;
 
     bool allow_weathervane() override;
 
+    bool in_fwd_transition() const override;
+
 private:
 
     enum {

ArduPlane/transition.h

@@ -58,6 +58,8 @@ class Transition
 
     virtual bool allow_stick_mixing() const { return true; }
 
+    virtual bool in_fwd_transition() const = 0;
+
 protected:
 
     // refences for convenience
@@ -101,6 +103,8 @@ class SLT_Transition : public Transition
 
     void set_last_fw_pitch(void) override;
 
+    bool in_fwd_transition() const override;
+
 protected:
 
     enum {

libraries/AP_TECS/AP_TECS.cpp

@@ -799,6 +799,8 @@ void AP_TECS::_update_throttle_with_airspeed(void)
             _last_throttle_dem = _throttle_dem;
         }
 
+        _throttle_dem_pre_integ = _throttle_dem;
+
         // Sum the components.
         _throttle_dem = _throttle_dem + _integTHR_state;
 
@@ -914,6 +916,10 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi
     float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
     float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f);
     _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
+
+    // integrator not used without airspeed, but store pre-integrator
+    // value anyway in case we enable airspeed
+    _throttle_dem_pre_integ = _throttle_dem;
 }
 
 void AP_TECS::_detect_bad_descent(void)
@@ -1197,6 +1203,16 @@ void AP_TECS::_update_STE_rate_lim(void)
     _STEdot_neg_max = - _maxSinkRate * GRAVITY_MSS;
 }
 
+// reset throttle integrator to give trim throttle
+// used when overriding throttle
+void AP_TECS::reset_throttle_I_cruise(void)
+{
+    // get the last throttle output without the integrator
+    const float nomThr = aparm.throttle_cruise * 0.01f;
+    // set integrator so that we would have gotten cruise throttle
+    _integTHR_state = nomThr - _throttle_dem_pre_integ;
+}
+
 
 void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
                                     int32_t EAS_dem_cm,

libraries/AP_TECS/AP_TECS.h

@@ -98,6 +98,9 @@ class AP_TECS {
         _integTHR_state = 0.0;
     }
 
+    // reset throttle integrator to give trim throttle
+    void reset_throttle_I_cruise(void);
+
     // return landing sink rate
     float get_land_sinkrate(void) const {
         return _land_sink;
@@ -215,6 +218,9 @@ class AP_TECS {
     // throttle demand in the range from -1.0 to 1.0, usually positive unless reverse thrust is enabled via _THRminf < 0
     float _throttle_dem;
 
+    // pre-integrator throttle demand
+    float _throttle_dem_pre_integ;
+
     // pitch angle demand in radians
     float _pitch_dem;
 

libraries/AP_Vehicle/AP_FixedWing.h

@@ -11,6 +11,7 @@ struct AP_FixedWing {
     AP_Int8 throttle_slewrate;
     AP_Int8 throttle_cruise;
     AP_Int8 takeoff_throttle_max;
+    AP_Int8 takeoff_throttle_min;
     AP_Int16 airspeed_min;
     AP_Int16 airspeed_max;
     AP_Float airspeed_cruise;