diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 010e9a5e52..85462166d1 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -82,13 +82,21 @@ void ModeGuided::update() plane.calc_nav_pitch(); } - // Received an external msg that guides throttle in the last 3 seconds? - if (plane.aparm.throttle_cruise > 1 && + // Throttle output + if (plane.guided_throttle_passthru) { + // manual passthrough of throttle in fence breach + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); + + } else if (plane.aparm.throttle_cruise > 1 && plane.guided_state.last_forced_throttle_ms > 0 && millis() - plane.guided_state.last_forced_throttle_ms < 3000) { + // Received an external msg that guides throttle in the last 3 seconds? SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); + } else { + // TECS control plane.calc_throttle(); + } } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c4dc017244..8e91fd20de 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -601,10 +601,6 @@ void Plane::set_throttle(void) // 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)); } - } else if (control_mode->is_guided_mode() && - guided_throttle_passthru) { - // manual pass through of throttle while in GUIDED - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } if (control_mode->use_battery_compensation()) {