diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 8d0010c302..8571ffb1e9 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -102,7 +102,14 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Range: 0 100 AP_GROUPINFO("START_PCT", 10, AP_ICEngine, start_percent, 5), - AP_GROUPEND + // @Param: IDLE_PCT + // @DisplayName: Throttle percentage for engine idle + // @Description: This is the minimum percentage throttle output while running, this includes being disarmed, but not safe + // @User: Standard + // @Range: 0 100 + AP_GROUPINFO("IDLE_PCT", 11, AP_ICEngine, idle_percent, 0), + + AP_GROUPEND }; @@ -210,7 +217,7 @@ void AP_ICEngine::update(void) // reset initial height while disarmed initial_height = -pos.z; } - } else { + } else if (idle_percent == 0) { // check if we should idle // force ignition off when disarmed state = ICE_OFF; } @@ -250,13 +257,20 @@ void AP_ICEngine::update(void) /* check for throttle override. This allows the ICE controller to force - the correct starting throttle when starting the engine + the correct starting throttle when starting the engine and maintain idle when disarmed */ bool AP_ICEngine::throttle_override(uint8_t &percentage) { if (!enable) { return false; } + + uint8_t current_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + if (idle_percent > current_throttle && state == ICE_RUNNING) { + percentage = (uint8_t)idle_percent; + return true; + } + if (state == ICE_STARTING || state == ICE_START_DELAY) { percentage = (uint8_t)start_percent.get(); return true; diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index b0a58261bc..6d660d86dd 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -89,6 +89,9 @@ private: // throttle percentage for engine start AP_Int8 start_percent; + // throttle percentage for engine idle + AP_Int8 idle_percent; + // height when we enter ICE_START_HEIGHT_DELAY float initial_height;