From ef473a43723fcee096b51d0385ff7961f58ff312 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 10 Jul 2022 16:07:52 +1000 Subject: [PATCH] AP_ICEngine: added option to force low throttle on engine off this fixes an issue with EFI engines that use low throttle demand to stop the engine, instead of using an ignition channel. This option needs to be set on these aircraft to prevent the idle governor or the fwd throttle integrator in quadplanes from keeping the engine on when the pilot asks for it to be off. --- libraries/AP_ICEngine/AP_ICEngine.cpp | 8 ++++++-- libraries/AP_ICEngine/AP_ICEngine.h | 4 ++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 7600fdb2b1..392fb1ef57 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -211,7 +211,7 @@ void AP_ICEngine::update(void) should_run = true; } - if ((options & uint16_t(Options::DISABLE_IGNITION_RC_FAILSAFE)) && AP_Notify::flags.failsafe_radio) { + if (option_set(Options::DISABLE_IGNITION_RC_FAILSAFE) && AP_Notify::flags.failsafe_radio) { // user has requested ignition kill on RC failsafe should_run = false; } @@ -340,9 +340,13 @@ bool AP_ICEngine::throttle_override(uint8_t &percentage) } if (state == ICE_STARTING || state == ICE_START_DELAY) { - percentage = (uint8_t)start_percent.get(); + percentage = start_percent.get(); + return true; + } else if (state != ICE_RUNNING && hal.util->get_soft_armed()) { + percentage = 0; return true; } + return false; } diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 9e894d7e7f..0a8cde302f 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -125,6 +125,10 @@ private: }; AP_Int16 options; + bool option_set(Options option) const { + return (options & uint16_t(option)) != 0; + } + // start_chan debounce uint16_t start_chan_last_value = 1500; uint32_t start_chan_last_ms;