diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 281a9372b2..20b9132b67 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -567,9 +567,10 @@ const clivalue_t valueTable[] = { { "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) }, { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, - { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, + { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) }, { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) }, { "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) }, + { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b95cc16b04..63903dff81 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -115,7 +115,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default .horizon_tilt_effect = 75, .horizon_tilt_expert_mode = false, - .crash_limit_yaw = 200 + .crash_limit_yaw = 200, + .itermLimit = 100 ); } @@ -242,7 +243,7 @@ static float maxVelocity[3]; static float relaxFactor; static float dtermSetpointWeight; static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; -static float ITermWindupPoint, ITermWindupPointInv; +static float ITermWindupPointInv; static uint8_t horizonTiltExpertMode; static timeDelta_t crashTimeLimitUs; static timeDelta_t crashTimeDelayUs; @@ -252,6 +253,7 @@ static float crashDtermThreshold; static float crashGyroThreshold; static float crashSetpointThreshold; static float crashLimitYaw; +static float itermLimit; void pidInitConfig(const pidProfile_t *pidProfile) { @@ -270,7 +272,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; - ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; + const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); crashTimeLimitUs = pidProfile->crash_time * 1000; crashTimeDelayUs = pidProfile->crash_delay * 1000; @@ -280,6 +282,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) crashDtermThreshold = pidProfile->crash_dthreshold; crashSetpointThreshold = pidProfile->crash_setpoint_threshold; crashLimitYaw = pidProfile->crash_limit_yaw; + itermLimit = pidProfile->itermLimit; } void pidInit(const pidProfile_t *pidProfile) @@ -456,7 +459,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate I component const float ITerm = axisPID_I[axis]; - const float ITermNew = ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator; + const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { // Only increase ITerm if output is not saturated diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 850d9b2e8f..e487e9e8cb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -106,6 +106,7 @@ typedef struct pidProfile_s { uint8_t crash_recovery_rate; // degree/second pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase + uint16_t itermLimit; } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);