diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bd0b9da717..ad999852e8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -169,7 +169,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + 0.5f * (lastError[axis] + RateError) * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); if (allowITermShrinkOnly || motorLimitReached) { if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) { @@ -310,7 +310,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis]; + errorGyroI[axis] = errorGyroI[axis] + ((((lastError[axis] + RateError) / 2) * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings