From e8deb491decd8a72e7694f09872ea41d27cd99a2 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 20 Sep 2023 10:08:24 -0700 Subject: [PATCH] AP_ICEngine: Add an option to disable starting while disarmed --- libraries/AP_ICEngine/AP_ICEngine.cpp | 11 ++++++++--- libraries/AP_ICEngine/AP_ICEngine.h | 7 ++++--- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 1b91734824..8c4669dd25 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -142,8 +142,8 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Param: OPTIONS // @DisplayName: ICE options - // @Description: Options for ICE control. The DisableIgnitionRCFailsafe option will cause the ignition to be set off on any R/C failsafe. If ThrottleWhileDisarmed is set then throttle control will be allowed while disarmed for planes when in MANUAL mode. - // @Bitmask: 0:DisableIgnitionRCFailsafe,1:DisableRedineGovernor,2:ThrottleWhileDisarmed + // @Description: Options for ICE control. The Disable ignition in RC failsafe option will cause the ignition to be set off on any R/C failsafe. If Throttle while disarmed is set then throttle control will be allowed while disarmed for planes when in MANUAL mode. If disable while disarmed is set the engine will not start while the vehicle is disarmed. + // @Bitmask: 0:Disable ignition in RC failsafe,1:Disable redine governor,2:ThrottleWhileDisarmed,3:Disable while disarmed AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0), // @Param: STARTCHN_MIN @@ -247,6 +247,11 @@ void AP_ICEngine::update(void) should_run = false; } + if (option_set(Options::NO_RUNNING_WHILE_DISARMED) && !hal.util->get_soft_armed()) { + // disable the engine if disarmed + should_run = false; + } + #if HAL_PARACHUTE_ENABLED // Stop on parachute deployment AP_Parachute *parachute = AP::parachute(); @@ -558,7 +563,7 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) // Calculate the change per loop to achieve the desired slew rate of 1 percent per second static const float idle_setpoint_step = idle_slew * AP::scheduler().get_loop_period_s(); - // Update Integrator + // Update Integrator if (underspeed) { idle_governor_integrator += idle_setpoint_step; } else { diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index b9cd3d44de..1c243dd88f 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -142,9 +142,10 @@ private: float idle_governor_integrator; enum class Options : uint16_t { - DISABLE_IGNITION_RC_FAILSAFE=(1U<<0), - DISABLE_REDLINE_GOVERNOR = (1U << 1), - THROTTLE_WHILE_DISARMED = (1U << 2), + DISABLE_IGNITION_RC_FAILSAFE = (1U << 0), + DISABLE_REDLINE_GOVERNOR = (1U << 1), + THROTTLE_WHILE_DISARMED = (1U << 2), + NO_RUNNING_WHILE_DISARMED = (1U << 3), }; AP_Int16 options;