From 17fb9455a9ce091b15ce00f5e58f1123ab5a0623 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 22 Dec 2023 16:09:42 +0000 Subject: [PATCH] Plane: `set_throttle` add early return for disarmed and throttle suppression --- ArduPlane/servos.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index d77ed1a172..65b5260079 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -551,7 +551,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 +568,15 @@ void Plane::set_throttle(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); } + return; + } + #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 ||