Hardlimit D-term, reduce kI when stick input is above certain limit (#357)

src/main/config/config.c

@@ -170,7 +170,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 119;
+static const uint8_t EEPROM_CONF_VERSION = 120;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
 {
@@ -219,6 +219,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->dterm_lpf_hz = 40;
     pidProfile->yaw_lpf_hz = 30;
 
+    pidProfile->rollPitchItermIgnoreRate = 200;     // dps
+    pidProfile->yawItermIgnoreRate = 50;            // dps
+
     pidProfile->yaw_p_limit = YAW_P_LIMIT_DEFAULT;
     pidProfile->mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT;
 

src/main/config/runtime_config.h

@@ -60,7 +60,6 @@ typedef enum {
     SMALL_ANGLE     = (1 << 3),
     FIXED_WING      = (1 << 4),                   // set when in flying_wing or airplane mode. currently used by althold selection code
     ANTI_WINDUP     = (1 << 5),
-    PID_ATTENUATE   = (1 << 6),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/flight/pid.c

@@ -307,15 +307,20 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
         if (pidProfile->dterm_lpf_hz) {
             newDTerm = pt1FilterApply4(&pidState->deltaLpfState, newDTerm, pidProfile->dterm_lpf_hz, dT);
         }
+
+        // Additionally constrain D
+        newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
     }
 
     // TODO: Get feedback from mixer on available correction range for each axis
-    const float pidAttenuationFactor = STATE(PID_ATTENUATE) ? 0.33f : 1.0f;
+    const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
-    const float newOutput = (newPTerm + newDTerm) * pidAttenuationFactor + pidState->errorGyroIf;
     const float newOutputLimited = constrainf(newOutput, -PID_MAX_OUTPUT, +PID_MAX_OUTPUT);
 
-    // Integrate only if we can do backtracking
+    // Prevent strong Iterm accumulation during stick inputs
-    pidState->errorGyroIf += (rateError * pidState->kI * dT) + ((newOutputLimited - newOutput) * pidState->kT * dT);
+    const float integratorThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
+    const float antiWindupScaler = constrainf(1.0f - (ABS(pidState->rateTarget) / integratorThreshold), 0.0f, 1.0f);
+
+    pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * dT);
 
     // Don't grow I-term if motors are at their limit
     if (STATE(ANTI_WINDUP) || motorLimitReached) {

src/main/flight/pid.h

@@ -60,10 +60,12 @@ typedef struct pidProfile_s {
     uint16_t yaw_p_limit;
     uint8_t yaw_lpf_hz;
 
+    uint16_t rollPitchItermIgnoreRate;      // Experimental threshold for ignoring iterm for pitch and roll on certain rates
+    uint16_t yawItermIgnoreRate;            // Experimental threshold for ignoring iterm for yaw on certain rates
+
     int16_t max_angle_inclination[ANGLE_INDEX_COUNT];       // Max possible inclination (roll and pitch axis separately
 
-    uint8_t mag_hold_rate_limit;    //Maximum rotation rate MAG_HOLD mode cas feed to yaw rate PID controller
+    uint8_t mag_hold_rate_limit;            //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
-
 } pidProfile_t;
 
 extern int16_t axisPID[];

src/main/io/serial_cli.c

@@ -769,7 +769,10 @@ const clivalue_t valueTable[] = {
     { "dterm_lpf_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } },
     { "yaw_lpf_hz",                 VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 200 } },
 
-    { "yaw_p_limit",                VAR_UINT16 | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN,  YAW_P_LIMIT_MAX }, 0 },
+    { "yaw_p_limit",                VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN,  YAW_P_LIMIT_MAX }, 0 },
+
+    { "iterm_ignore_threshold",     VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
+    { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
 
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, .config.minmax = { 1,  32 }, 0 },

src/main/mw.c

@@ -452,17 +452,8 @@ void processRx(void)
                 DISABLE_STATE(ANTI_WINDUP);
                 pidResetErrorAccumulators();
             }
-
-            // Enable low-throttle PID attenuation on multicopters
-            if (!STATE(FIXED_WING)) {
-                ENABLE_STATE(PID_ATTENUATE);
-            }
-            else {
-                DISABLE_STATE(PID_ATTENUATE);
-            }
         }
         else {
-            DISABLE_STATE(PID_ATTENUATE);
             DISABLE_STATE(ANTI_WINDUP);
         }
     }