From 70c7a79aee7cb676d15322847061f7418f75df7a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 1 Jun 2024 13:48:39 +1000 Subject: [PATCH 1/3] Plane: added TKOFF_THR_MIN for quadplane SLT this allows for a minimum throttle to be applied during a takeoff or a forward transition in auto throttle modes. This is useful on high drag aircraft where normal TECS throttle control results in a slow transition or takeoff --- ArduPlane/Parameters.cpp | 9 +++++++++ ArduPlane/Parameters.h | 1 + ArduPlane/quadplane.cpp | 24 ++++++++++++++++++++++++ ArduPlane/quadplane.h | 6 ++++++ ArduPlane/servos.cpp | 17 +++++++++++++++-- ArduPlane/tailsitter.cpp | 8 ++++++++ ArduPlane/tailsitter.h | 2 ++ ArduPlane/transition.h | 4 ++++ libraries/AP_Vehicle/AP_FixedWing.h | 1 + 9 files changed, 70 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index e341fc8259c405..a43fde33c7c83d 100644 --- a/ArduPlane/Parameters.cpp +++ b/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 diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index b464d33d67997f..25480124a47f84 100644 --- a/ArduPlane/Parameters.h +++ b/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; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 630b78850b1637..d3e1fbb6b670f3 100644 --- a/ArduPlane/quadplane.cpp +++ b/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; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5b2b35fa92dffe..a743a5fd25921f 100644 --- a/ArduPlane/quadplane.h +++ b/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); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 08b102acd9ae67..46b07deb45c1e8 100644 --- a/ArduPlane/servos.cpp +++ b/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,20 @@ 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); + } + } + + auto throttle = constrain_float(throttle_in, min_throttle, max_throttle); + + return throttle; } /* diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 4e28d534440a3e..c9b03b94f0613c 100644 --- a/ArduPlane/tailsitter.cpp +++ b/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 diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index fbf26702f8ca9f..b072bcb076ed16 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -189,6 +189,8 @@ friend class Tailsitter; bool allow_weathervane() override; + bool in_fwd_transition() const override; + private: enum { diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index b31e45ff040ece..156e63a4654873 100644 --- a/ArduPlane/transition.h +++ b/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 { diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 7aee38d5c5b35d..9896bf7c1006f7 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/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; From 23b50ce745753662ad5c9cf121f9c5a5b747848c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Aug 2024 08:07:33 +1000 Subject: [PATCH 2/3] AP_TECS: added reset_throttle_I_cruise() for resetting integrator during a fwd transition with min throttle override --- libraries/AP_TECS/AP_TECS.cpp | 16 ++++++++++++++++ libraries/AP_TECS/AP_TECS.h | 6 ++++++ 2 files changed, 22 insertions(+) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 604a21287f14db..07ae5c1deb3985 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/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, diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index fcd3fe97f9bfe7..63bd9fa8c79ebb 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/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; From 6b2edeaa053b78260af271228d9b1282db6d9d1e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Aug 2024 08:08:30 +1000 Subject: [PATCH 3/3] Plane: reset TECS integrator to cruise throttle on thr min override when raising the throttle in fwd transition with TKOFF_THR_MIN setup TECS to use cruise throttle when we finish the transition --- ArduPlane/servos.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 46b07deb45c1e8..ea93f7386c74ea 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -546,6 +546,7 @@ float Plane::apply_throttle_limits(float throttle_in) 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(); } }