Throttle boost (#5508)

* throttle boost which temporarily boosts throttle in both directions to improve response

* fix comment and use pt1FilterGain

* incorporate review suggestions

* incorporate review suggestions

* use float constant to avoid double promotion

* formatting changes

* formatting change

* hopefully last style changes

src/main/flight/mixer.c

@@ -765,6 +765,10 @@ void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
     }
 
     motorMixRange = motorMixMax - motorMixMin;
+    if (throttleBoost > 0.0f) {
+        float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle);
+        throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f);
+    }
 
     if (motorMixRange > 1.0f) {
         for (int i = 0; i < motorCount; i++) {

src/main/flight/pid.c

@@ -130,7 +130,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .horizon_tilt_effect = 75,
         .horizon_tilt_expert_mode = false,
         .crash_limit_yaw = 200,
-        .itermLimit = 150
+        .itermLimit = 150,
+        .throttle_boost = 0,
+        .throttle_boost_cutoff = 15
     );
 }
 
@@ -266,6 +268,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
         ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
         pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT));
     }
+
+    pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
 }
 
 static FAST_RAM float Kp[3], Ki[3], Kd[3];
@@ -284,6 +288,8 @@ static FAST_RAM float crashGyroThreshold;
 static FAST_RAM float crashSetpointThreshold;
 static FAST_RAM float crashLimitYaw;
 static FAST_RAM float itermLimit;
+FAST_RAM float throttleBoost;
+pt1Filter_t throttleLpf;
 
 void pidInitConfig(const pidProfile_t *pidProfile)
 {
@@ -317,6 +323,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
     crashLimitYaw = pidProfile->crash_limit_yaw;
     itermLimit = pidProfile->itermLimit;
+    throttleBoost = pidProfile->throttle_boost * 0.1f;
 }
 
 void pidInit(const pidProfile_t *pidProfile)

src/main/flight/pid.h

@@ -19,6 +19,7 @@
 
 #include <stdbool.h>
 #include "common/time.h"
+#include "common/filter.h"
 #include "pg/pg.h"
 
 #define MAX_PID_PROCESS_DENOM       16
@@ -106,6 +107,9 @@ typedef struct pidProfile_s {
     uint16_t crash_limit_yaw;               // limits yaw errorRate, so crashes don't cause huge throttle increase
     uint16_t itermLimit;
     uint16_t dterm_lowpass2_hz;                // Extra PT1 Filter on D in hz
+    uint8_t throttle_boost;                 // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
+    uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
+    
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE
@@ -140,3 +144,6 @@ void pidInitConfig(const pidProfile_t *pidProfile);
 void pidInit(const pidProfile_t *pidProfile);
 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 bool crashRecoveryModeActive(void);
+
+FAST_RAM float throttleBoost;
+pt1Filter_t throttleLpf;

src/main/interface/settings.c

@@ -714,6 +714,9 @@ const clivalue_t valueTable[] = {
     { "pidsum_limit_yaw",           VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
     { "yaw_lowpass_hz",             VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lowpass_hz) },
 
+    { "throttle_boost",             VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost) },
+    { "throttle_boost_cutoff",      VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 5, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost_cutoff) },
+    
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
     { "d_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) },