diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8dcb90e47d8dd..1b8e5ccea2546 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1095,6 +1095,7 @@ class Plane : public AP_Vehicle { // 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 6eef3fb0fab38..2d97c9cb1acd7 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 2f745074c6f27..1de286f8667e2 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -135,6 +135,8 @@ class Mode // 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/quadplane.cpp b/ArduPlane/quadplane.cpp index 25c7ff533ba2a..4a372fc15ffc6 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4742,4 +4742,10 @@ float QuadPlane::get_throttle_input() const return ret; } +// return true if forward throttle from forward_throttle_pct() should be used +bool QuadPlane::allow_forward_throttle_in_vtol_mode() const +{ + return in_vtol_mode() && motors->armed() && (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN); +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 2c5a624ff4a4e..140c47e585aa7 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -729,6 +729,11 @@ class QuadPlane */ void setup_rp_fw_angle_gains(void); + /* + return true if forward throttle from forward_throttle_pct() should be used + */ + bool allow_forward_throttle_in_vtol_mode() const; + public: void motor_test_output(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c8f03bb896848..c04cc8dbb3e8a 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 @@ -551,7 +559,10 @@ void Plane::set_throttle(void) SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); } - } else if (suppress_throttle()) { + return; + } + + if (suppress_throttle()) { if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); @@ -565,11 +576,18 @@ void Plane::set_throttle(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); } + return; + } + + // Update voltage scaling + g2.fwd_batt_cmp.update(); + #if AP_SCRIPTING_ENABLED - } else if (nav_scripting_active()) { + if (nav_scripting_active()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); + } else #endif - } else if (control_mode == &mode_stabilize || + if (control_mode == &mode_stabilize || control_mode == &mode_training || control_mode == &mode_acro || control_mode == &mode_fbwa || @@ -581,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) { @@ -591,21 +608,24 @@ void Plane::set_throttle(void) #if HAL_QUADPLANE_ENABLED } else if (quadplane.in_vtol_mode()) { float fwd_thr = 0; - // if armed and not spooled down ask quadplane code for forward throttle - if (quadplane.motors->armed() && - quadplane.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN) { - - fwd_thr = constrain_float(quadplane.forward_throttle_pct(), min_throttle, max_throttle); + // if enabled ask quadplane code for forward throttle + if (quadplane.allow_forward_throttle_in_vtol_mode()) { + 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); + } } /*