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;