Prevent ItermAccel and windup to clash with each other

src/main/fc/cli.c

@@ -702,6 +702,7 @@ const clivalue_t valueTable[] = {
     { "pid_at_min_throttle",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } },
     { "anti_gravity_threshold",     VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } },
     { "anti_gravity_gain",          VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorGain, .config.minmax = {1, 30 } },
+    { "anti_gravity_rate_max",      VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermAcceleratorRateLimit, .config.minmax = {0, 2000 } },
     { "setpoint_relax_ratio",       VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } },
     { "dterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
     { "yaw_accel_limit",            VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } },

src/main/fc/config.c

@@ -184,6 +184,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->rateAccelLimit = 0.0f;
     pidProfile->itermThrottleThreshold = 300;
     pidProfile->itermAcceleratorGain = 3.0f;
+    pidProfile->itermAcceleratorRateLimit = 80;
 }
 
 void resetProfile(profile_t *profile)

src/main/flight/pid.c

@@ -148,7 +148,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 }
 
 static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
-static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv;
+static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit;
 
 void pidInitConfig(const pidProfile_t *pidProfile) {
     for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -165,6 +165,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
     ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
     ITermWindupPointInv = 1.0f - ITermWindupPoint;
+    itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit;
 }
 
 static float calcHorizonLevelStrength(void) {
@@ -216,6 +217,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
     static float previousSetpoint[3];
 
     const float motorMixRange = getMotorMixRange();
+    // Dynamic ki component to gradually scale back integration when above windup point
+    float dynKi = MIN((1.0f - motorMixRange) / ITermWindupPointInv, 1.0f);
 
     // ----------PID controller----------
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -247,13 +250,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         float ITerm = previousGyroIf[axis];
         if (motorMixRange < 1.0f) {
             // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
-            float ITermDelta = Ki[axis] * errorRate * dT;
+            // Iterm will only be accelerated below steady rate threshold
-            // gradually scale back integration when above windup point
+            if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit)
-            if (motorMixRange > ITermWindupPoint) {
+                dynKi *= itermAccelerator;
-                ITermDelta *= (1.0f - motorMixRange) / ITermWindupPointInv;
+            float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
-            } else {
-                ITermDelta *= itermAccelerator;
-            }
             ITerm += ITermDelta;
             // also limit maximum integrator value to prevent windup
             ITerm = constrainf(ITerm, -250.0f, 250.0f);

src/main/flight/pid.h

@@ -76,8 +76,9 @@ typedef struct pidProfile_s {
     uint8_t levelSensitivity;               // Angle mode sensitivity reflected in degrees assuming user using full stick
 
     // Betaflight PID controller parameters
-    uint16_t itermThrottleThreshold;        // max allowed throttle delta before errorGyroReset in ms
+    uint16_t itermThrottleThreshold;        // max allowed throttle delta before iterm accelerated in ms
     float itermAcceleratorGain;             // Iterm Accelerator Gain when itermThrottlethreshold is hit
+    uint16_t itermAcceleratorRateLimit;     // Setpointrate limit for iterm accelerator to operate within
     uint8_t setpointRelaxRatio;             // Setpoint weight relaxation effect
     uint8_t dtermSetpointWeight;            // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
     float yawRateAccelLimit;                // yaw accel limiter for deg/sec/ms