From 798ab5d859a5b17d805d6af821a02645af694212 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 8 Jul 2022 09:53:29 +1000
Subject: [PATCH] AP_ICEngine: added option for throttle control while disarmed

this uses the passed in base throttle when disarmed
---
 libraries/AP_ICEngine/AP_ICEngine.cpp | 21 +++++++++++++++++----
 libraries/AP_ICEngine/AP_ICEngine.h   |  3 ++-
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp
index b87072f9d0..0016802cff 100644
--- a/libraries/AP_ICEngine/AP_ICEngine.cpp
+++ b/libraries/AP_ICEngine/AP_ICEngine.cpp
@@ -134,8 +134,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.
-    // @Bitmask: 0:DisableIgnitionRCFailsafe,1:DisableRedineGovernor
+    // @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
     AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0),
 
     // @Param: STARTCHN_MIN
@@ -297,7 +297,7 @@ void AP_ICEngine::update(void)
                 // reset initial height while disarmed
                 initial_height = -pos.z;
             }
-        } else if (idle_percent <= 0) { // check if we should idle
+        } else if (idle_percent <= 0 && !option_set(Options::THROTTLE_WHILE_DISARMED)) {
             // force ignition off when disarmed
             state = ICE_OFF;
         }
@@ -357,8 +357,11 @@ void AP_ICEngine::update(void)
 /*
   check for throttle override. This allows the ICE controller to force
   the correct starting throttle when starting the engine and maintain idle when disarmed
+
+  base_throttle is the throttle before the disarmed override
+  check. This allows for throttle control while disarmed
  */
-bool AP_ICEngine::throttle_override(float &percentage)
+bool AP_ICEngine::throttle_override(float &percentage, const float base_throttle)
 {
     if (!enable) {
         return false;
@@ -370,6 +373,9 @@ bool AP_ICEngine::throttle_override(float &percentage)
         idle_percent > percentage)
     {
         percentage = idle_percent;
+        if (option_set(Options::THROTTLE_WHILE_DISARMED) && !hal.util->get_soft_armed()) {
+            percentage = MAX(percentage, base_throttle);
+        }
         return true;
     }
 
@@ -405,6 +411,13 @@ bool AP_ICEngine::throttle_override(float &percentage)
         return true;
     }
 
+    // if THROTTLE_WHILE_DISARMED is set then we use the base_throttle, allowing the pilot to control throttle while disarmed
+    if (option_set(Options::THROTTLE_WHILE_DISARMED) && !hal.util->get_soft_armed() &&
+        base_throttle > percentage) {
+        percentage = base_throttle;
+        return true;
+    }
+
     return false;
 }
 
diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h
index 21a43213c2..dd6bb6123e 100644
--- a/libraries/AP_ICEngine/AP_ICEngine.h
+++ b/libraries/AP_ICEngine/AP_ICEngine.h
@@ -31,7 +31,7 @@ public:
     void update(void);
 
     // check for throttle override
-    bool throttle_override(float &percent);
+    bool throttle_override(float &percent, const float base_throttle);
 
     enum ICE_State {
         ICE_OFF=0,
@@ -126,6 +126,7 @@ private:
     enum class Options : uint16_t {
         DISABLE_IGNITION_RC_FAILSAFE=(1U<<0),
         DISABLE_REDLINE_GOVERNOR = (1U << 1),
+        THROTTLE_WHILE_DISARMED = (1U << 2),
     };
     AP_Int16 options;