From 19a814afbb7034b0f295a605879d0c8402e7d4fb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 03:16:04 +0200 Subject: [PATCH] Seperate float and int accumulation logic --- src/main/config/config.c | 4 ++-- src/main/flight/pid.c | 29 +++++++++-------------------- 2 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0caa5c62e7..a9723ecaa9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -220,8 +220,8 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 180; - pidProfile->yawItermIgnoreRate = 35; + pidProfile->rollPitchItermIgnoreRate = 200; + pidProfile->yawItermIgnoreRate = 50; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->dynamic_pid = 1; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f90f1f48b0..8de667012c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -76,20 +76,6 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) targetPidLooptime = gyro.targetLooptime * pidProcessDenom; } -uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) -{ - uint16_t dynamicKi; - uint16_t resetRate; - - resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - - uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8); - - dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - return dynamicKi; -} - void pidResetErrorGyroState(void) { int axis; @@ -195,9 +181,11 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate I component. - uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis]; + // Prevent strong Iterm accumulation during stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * antiWindupScaler, -250.0f, 250.0f); // 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 @@ -330,11 +318,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin // 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. - uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis]; + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS((int32_t)angleRate) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - int32_t rateErrorLimited = errorLimiter * RateError; - - errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI; + errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi; // 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