diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 17cdfd794a..bddc4c6d5f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -707,8 +707,7 @@ const clivalue_t valueTable[] = { { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, - { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {50, 100 } }, - { "iterm_noise_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermNoiseThreshold, .config.minmax = {0, 20 } }, + { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {30, 100 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 4ce27a28b7..1de72a3162 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -169,8 +169,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->yaw_lpf_hz = 0; - pidProfile->itermWindupPointPercent = 75; - pidProfile->itermNoiseThreshold = 0; + pidProfile->itermWindupPointPercent = 50; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 260; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3ae40d0a0e..ddc2fc0e50 100644 --- a/src/main/flight/pid.c +++ b/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, ITermNoiseThresholdDps; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -164,7 +164,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) { maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; - ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f; } static float calcHorizonLevelStrength(void) { @@ -215,6 +214,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an static float previousRateError[2]; static float previousSetpoint[3]; + const float motorMixRange = getMotorMixRange(); + // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { float currentPidSetpoint = getSetpointRate(axis); @@ -243,14 +244,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate I component float ITerm = previousGyroIf[axis]; - const float motorMixRangeAbs = ABS(getMotorMixRange()); - if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) { + if (motorMixRange < 1.0f) { // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold - // Reduce strong Iterm accumulation during higher stick inputs float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator; - // gradually scale back integration when above windup point (default is 75%) - if (motorMixRangeAbs > ITermWindupPoint) { - ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint); + // gradually scale back integration when above windup point + if (motorMixRange > ITermWindupPoint) { + ITermDelta *= (1.0f - motorMixRange) * ITermWindupPoint; } ITerm += ITermDelta; // also limit maximum integrator value to prevent windup @@ -267,10 +266,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an dynC = c[axis]; if (currentPidSetpoint > 0) { if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis]) - dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + dynC *= (1.0f - rcDeflection) * relaxFactor[axis]; } else if (currentPidSetpoint < 0) { if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis]) - dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); + dynC *= (1.0f - rcDeflection) * relaxFactor[axis]; } } const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 87ac84f2ea..4cb7db3a9b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -67,7 +67,6 @@ typedef struct pidProfile_s { uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation - uint8_t itermNoiseThreshold; // Experimental ITerm noise threshold uint16_t yaw_p_limit; float pidSumLimit; uint8_t dterm_average_count; // Configurable delta count for dterm