diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index ec10b69631..30ec5fc5b0 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -259,6 +259,21 @@ void Mode::output_rudder_and_steering(float val) SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val); } +// Output pilot throttle, this is used in stabilized modes without auto throttle control +// Direct mapping if THR_PASS_STAB is set +// Otherwise apply curve for trim correction if configured +void Mode::output_pilot_throttle() +{ + if (plane.g.throttle_passthru_stabilize) { + // THR_PASS_STAB set, direct mapping + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); + return; + } + + // get throttle, but adjust center to output TRIM_THROTTLE if flight option set + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true)); +} + // true if throttle min/max limits should be applied bool Mode::use_throttle_limits() const { diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 2c10d2a94e..b7f4732e92 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -155,6 +155,9 @@ protected: // Helper to output to both k_rudder and k_steering servo functions void output_rudder_and_steering(float val); + // Output pilot throttle, this is used in stabilized modes without auto throttle control + void output_pilot_throttle(); + #if HAL_QUADPLANE_ENABLED // References for convenience, used by QModes AC_PosControl*& pos_control; @@ -262,6 +265,8 @@ public: bool mode_allows_autotuning() const override { return true; } + void run() override; + protected: bool _enter() override; @@ -493,6 +498,8 @@ public: bool mode_allows_autotuning() const override { return true; } + void run() override; + }; class ModeFBWB : public Mode diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index c15970b9bd..7093fd96b8 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -26,6 +26,8 @@ void ModeAcro::update() void ModeAcro::run() { + output_pilot_throttle(); + if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) { // we can do 3D acro locking diff --git a/ArduPlane/mode_autotune.cpp b/ArduPlane/mode_autotune.cpp index 395a6d6ef5..cbbf5a22bc 100644 --- a/ArduPlane/mode_autotune.cpp +++ b/ArduPlane/mode_autotune.cpp @@ -14,3 +14,10 @@ void ModeAutoTune::update() plane.mode_fbwa.update(); } +void ModeAutoTune::run() +{ + // Run base class function and then output throttle + Mode::run(); + + output_pilot_throttle(); +} diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index 6fd5ea877f..ff53a835ee 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -35,3 +35,11 @@ void ModeFBWA::update() } } } + +void ModeFBWA::run() +{ + // Run base class function and then output throttle + Mode::run(); + + output_pilot_throttle(); +} diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp index b73dfb6fc2..ee0f7626de 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -13,4 +13,6 @@ void ModeStabilize::run() plane.stabilize_pitch(); stabilize_stick_mixing_direct(); plane.stabilize_yaw(); + + output_pilot_throttle(); } diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 1c86e6d9c9..eb7040ce6f 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -66,4 +66,6 @@ void ModeTraining::run() // Always manual rudder control output_rudder_and_steering(plane.rudder_in_expo(false)); + output_pilot_throttle(); + } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 8e91fd20de..c0cab9f9de 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -585,23 +585,8 @@ void Plane::set_throttle(void) #if AP_SCRIPTING_ENABLED if (nav_scripting_active()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); - } else -#endif - if (control_mode == &mode_stabilize || - control_mode == &mode_training || - control_mode == &mode_acro || - control_mode == &mode_fbwa || - control_mode == &mode_autotune) { - // a manual throttle mode - if (g.throttle_passthru_stabilize) { - // manual pass through of throttle while in FBWA or - // STABILIZE mode with THR_PASS_STAB set - 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, get_adjusted_throttle_input(true)); - } } +#endif if (control_mode->use_battery_compensation()) { // Apply voltage compensation to throttle output from flight mode