From 33737f7570002813e84102091629ec95d8d91707 Mon Sep 17 00:00:00 2001
From: Alexander Wigen <alex@wigen.net>
Date: Tue, 29 Oct 2024 21:58:58 +0100
Subject: [PATCH] Add gov_spoolup_min_throttle

The default 5% minimum throttle during spoolup doesn't work well for
nitro models. This commits adds a new configurable option to adjust the
minimum throttle used during spoolup.
---
 src/main/cli/settings.c    | 1 +
 src/main/flight/governor.c | 7 ++++---
 src/main/msp/msp.c         | 2 ++
 src/main/pg/governor.c     | 1 +
 src/main/pg/governor.h     | 1 +
 5 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 561485e7d9..3f18913002 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -935,6 +935,7 @@ const clivalue_t valueTable[] = {
     { "gov_rpm_filter",             VAR_UINT8  |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 250 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_rpm_filter) },
     { "gov_tta_filter",             VAR_UINT8  |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 250 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_tta_filter) },
     { "gov_ff_filter",              VAR_UINT8  |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 250 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_ff_filter) },
+    { "gov_spoolup_min_throttle",   VAR_UINT8  |  MASTER_VALUE,  .config.minmaxUnsigned = { 0, 100 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_spoolup_min_throttle) },
 
 // PG_CONTROLRATE_PROFILES
 #ifdef USE_PROFILE_NAMES
diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c
index 8fee7d87d3..ee2bdba8af 100644
--- a/src/main/flight/governor.c
+++ b/src/main/flight/governor.c
@@ -51,7 +51,6 @@
 #define GOV_THROTTLE_OFF_LIMIT          0.05f
 
 // Throttle limits for spoolup
-#define GOV_MIN_SPOOLUP_THROTTLE        0.05f
 #define GOV_MAX_SPOOLUP_THROTTLE        0.95f
 
 // Headspeed quality levels
@@ -108,6 +107,7 @@ typedef struct {
 
     // Throttle handover level
     float           maxIdleThrottle;
+    float           minSpoolupThrottle;
 
     // Current headspeed
     float           actualHeadSpeed;
@@ -849,11 +849,11 @@ static float govSpoolUpControl(void)
     output = gov.pidSum;
 
     // Apply gov.C if output not saturated
-    if (!((output > GOV_MAX_SPOOLUP_THROTTLE && gov.C > 0) || (output < GOV_MIN_SPOOLUP_THROTTLE && gov.C < 0)))
+    if (!((output > GOV_MAX_SPOOLUP_THROTTLE && gov.C > 0) || (output < gov.minSpoolupThrottle && gov.C < 0)))
         gov.I += gov.C;
 
     // Limit output
-    output = constrainf(output, GOV_MIN_SPOOLUP_THROTTLE, GOV_MAX_SPOOLUP_THROTTLE);
+    output = constrainf(output, gov.minSpoolupThrottle, GOV_MAX_SPOOLUP_THROTTLE);
 
     return output;
 }
@@ -1150,6 +1150,7 @@ void governorInit(const pidProfile_t *pidProfile)
         gov.lostHeadspeedTimeout = governorConfig()->gov_lost_headspeed_timeout * 100;
 
         gov.maxIdleThrottle = constrain(governorConfig()->gov_handover_throttle, 10, 50) / 100.0f;
+        gov.minSpoolupThrottle = constrain(governorConfig()->gov_spoolup_min_throttle, 0, 100) / 100.0f;
 
         const float diff_cutoff = governorConfig()->gov_rpm_filter ?
             constrainf(governorConfig()->gov_rpm_filter, 1, 50) : 20;
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index c076b41de8..6dae9506e5 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1982,6 +1982,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, governorConfig()->gov_rpm_filter);
         sbufWriteU8(dst, governorConfig()->gov_tta_filter);
         sbufWriteU8(dst, governorConfig()->gov_ff_filter);
+        sbufWriteU8(dst, governorConfig()->gov_spoolup_min_throttle);
         break;
 
     default:
@@ -3337,6 +3338,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         governorConfigMutable()->gov_rpm_filter = sbufReadU8(src);
         governorConfigMutable()->gov_tta_filter = sbufReadU8(src);
         governorConfigMutable()->gov_ff_filter = sbufReadU8(src);
+        governorConfigMutable()->gov_spoolup_min_throttle = sbufReadU8(src);
         break;
 
     default:
diff --git a/src/main/pg/governor.c b/src/main/pg/governor.c
index d8903931bc..bc61d76906 100644
--- a/src/main/pg/governor.c
+++ b/src/main/pg/governor.c
@@ -43,5 +43,6 @@ PG_RESET_TEMPLATE(governorConfig_t, governorConfig,
     .gov_rpm_filter = 10,
     .gov_tta_filter = 0,
     .gov_ff_filter = 10,
+    .gov_spoolup_min_throttle = 5,
 );
 
diff --git a/src/main/pg/governor.h b/src/main/pg/governor.h
index 802e81b342..521c50cc8a 100644
--- a/src/main/pg/governor.h
+++ b/src/main/pg/governor.h
@@ -38,6 +38,7 @@ typedef struct governorConfig_s {
     uint8_t  gov_rpm_filter;
     uint8_t  gov_tta_filter;
     uint8_t  gov_ff_filter;
+    uint8_t  gov_spoolup_min_throttle;
 } governorConfig_t;
 
 PG_DECLARE(governorConfig_t, governorConfig);