From 01c8717308009bedf2ce3e72a49444b775a5417c Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 22 Dec 2023 16:18:29 +0000 Subject: [PATCH] Plane: ask flight mode if min/max throttle limits should be applied --- ArduPlane/Plane.h | 1 + ArduPlane/mode.cpp | 32 ++++++++++++++++++++++++++++++++ ArduPlane/mode.h | 2 ++ ArduPlane/servos.cpp | 33 ++++++++++++++++++++++++--------- 4 files changed, 59 insertions(+), 9 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8dcb90e47d..1b8e5ccea2 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1095,6 +1095,7 @@ private: // servos.cpp void set_servos_idle(void); void set_servos(); + float apply_throttle_limits(float throttle_in); void set_throttle(void); void set_takeoff_expected(void); void set_servos_old_elevons(void); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 6eef3fb0fa..2d97c9cb1a 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -258,3 +258,35 @@ void Mode::output_rudder_and_steering(float val) SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val); SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val); } + +// true if throttle min/max limits should be applied +bool Mode::use_throttle_limits() const +{ +#if AP_SCRIPTING_ENABLED + if (plane.nav_scripting_active()) { + return false; + } +#endif + + if (this == &plane.mode_stabilize || + this == &plane.mode_training || + this == &plane.mode_acro || + this == &plane.mode_fbwa || + this == &plane.mode_autotune) { + // a manual throttle mode + return !plane.g.throttle_passthru_stabilize; + } + + if (is_guided_mode() && plane.guided_throttle_passthru) { + // manual pass through of throttle while in GUIDED + return false; + } + +#if HAL_QUADPLANE_ENABLED + if (quadplane.in_vtol_mode()) { + return quadplane.allow_forward_throttle_in_vtol_mode(); + } +#endif + + return true; +} diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 2f745074c6..1de286f866 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -135,6 +135,8 @@ public: // true if is taking virtual bool is_taking_off() const; + // true if throttle min/max limits should be applied + bool use_throttle_limits() const; protected: diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 65b5260079..c04cc8dbb3 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -495,11 +495,11 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) } } #endif // #if AP_BATTERY_WATT_MAX_ENABLED - + /* - setup output channels all non-manual modes + Apply min/max limits to throttle */ -void Plane::set_throttle(void) +float Plane::apply_throttle_limits(float throttle_in) { // convert 0 to 100% (or -100 to +100) into PWM int8_t min_throttle = aparm.throttle_min.get(); @@ -531,7 +531,6 @@ void Plane::set_throttle(void) } // compensate for battery voltage drop - g2.fwd_batt_cmp.update(); g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle); #if AP_BATTERY_WATT_MAX_ENABLED @@ -539,6 +538,15 @@ void Plane::set_throttle(void) throttle_watt_limiter(min_throttle, max_throttle); #endif + return constrain_float(throttle_in, min_throttle, max_throttle); +} + +/* + setup output channels all non-manual modes + */ +void Plane::set_throttle(void) +{ + if (!arming.is_armed_and_safety_off()) { // Always set 0 scaled even if overriding to zero pwm. // This ensures slew limits and other functions using the scaled value pick up in the correct place @@ -571,6 +579,9 @@ void Plane::set_throttle(void) return; } + // Update voltage scaling + g2.fwd_batt_cmp.update(); + #if AP_SCRIPTING_ENABLED if (nav_scripting_active()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); @@ -588,8 +599,7 @@ void Plane::set_throttle(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } else { // get throttle, but adjust center to output TRIM_THROTTLE if flight option set - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_int16(get_adjusted_throttle_input(true), min_throttle, max_throttle)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_adjusted_throttle_input(true)); } } else if (control_mode->is_guided_mode() && guided_throttle_passthru) { @@ -600,17 +610,22 @@ void Plane::set_throttle(void) float fwd_thr = 0; // if enabled ask quadplane code for forward throttle if (quadplane.allow_forward_throttle_in_vtol_mode()) { - fwd_thr = constrain_float(quadplane.forward_throttle_pct(), min_throttle, max_throttle); + fwd_thr = quadplane.forward_throttle_pct(); } SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr); #endif // HAL_QUADPLANE_ENABLED } else { - // Apply min/max limits and voltage compensation to throttle output from flight mode + // Apply voltage compensation to throttle output from flight mode const float throttle = g2.fwd_batt_cmp.apply_throttle(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(throttle, min_throttle, max_throttle)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); } + if (control_mode->use_throttle_limits()) { + // Apply min/max throttle limits + const float limited_throttle = apply_throttle_limits(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, limited_throttle); + } } /*