diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index acdea0c98f..c9490bebf4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -179,13 +179,13 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pid.offsetLimit[i] = pidProfile->offset_limit[i]; // Exponential error decay rates - pid.errorDecayRateGround = (pidProfile->error_decay_time_ground) ? (10 * pid.dT / pidProfile->error_decay_time_ground) : 0; - pid.errorDecayRateCyclic = (pidProfile->error_decay_time_cyclic) ? (10 * pid.dT / pidProfile->error_decay_time_cyclic) : 0; - pid.errorDecayRateYaw = (pidProfile->error_decay_time_yaw) ? (10 * pid.dT / pidProfile->error_decay_time_yaw) : 0; + pid.errorDecayRateGround = (pidProfile->error_decay_time_ground) ? (10.0f / pidProfile->error_decay_time_ground) : 0; + pid.errorDecayRateCyclic = (pidProfile->error_decay_time_cyclic) ? (10.0f / pidProfile->error_decay_time_cyclic) : 0; + pid.errorDecayRateYaw = (pidProfile->error_decay_time_yaw) ? (10.0f / pidProfile->error_decay_time_yaw) : 0; // Max decay speeds in degs/s (linear decay) - pid.errorDecayLimitCyclic = (pidProfile->error_decay_limit_cyclic) ? (pid.dT * pidProfile->error_decay_limit_cyclic) : 1e6; - pid.errorDecayLimitYaw = (pidProfile->error_decay_limit_yaw) ? (pid.dT * pidProfile->error_decay_limit_yaw) : 1e6; + pid.errorDecayLimitCyclic = (pidProfile->error_decay_limit_cyclic) ? (pidProfile->error_decay_limit_cyclic) : 250; + pid.errorDecayLimitYaw = (pidProfile->error_decay_limit_yaw) ? (pidProfile->error_decay_limit_yaw) : 250; // Error Rotation enable pid.errorRotation = pidProfile->error_rotation; @@ -769,9 +769,9 @@ static void pidApplyCyclicMode2(uint8_t axis) pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError; // Apply I-term error decay - pid.data[axis].axisError -= isAirborne() ? - limitf(pid.data[axis].axisError * pid.errorDecayRateCyclic, pid.errorDecayLimitCyclic): - pid.data[axis].axisError * pid.errorDecayRateGround; + pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError * + (isAirborne() ? pid.errorDecayRateCyclic : pid.errorDecayRateGround), + pid.errorDecayLimitCyclic); //// Feedforward @@ -847,9 +847,9 @@ static void pidApplyYawMode2(void) pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError; // Apply I-term error decay - pid.data[axis].axisError -= isSpooledUp() ? - limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw): - pid.data[axis].axisError * pid.errorDecayRateGround; + pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError * + (isSpooledUp() ? pid.errorDecayRateYaw : pid.errorDecayRateGround), + pid.errorDecayLimitYaw); //// Feedforward @@ -945,16 +945,16 @@ static void pidApplyCyclicMode3(uint8_t axis, const pidProfile_t * pidProfile) float decayRate, decayLimit, errorDecay; if (isAirborne()) { - decayRate = pidTableLookup(curve, pidProfile->error_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.04f; - decayLimit = pidTableLookup(curve, pidProfile->error_decay_limit_curve, LOOKUP_CURVE_POINTS); - errorDecay = limitf(pid.data[axis].axisError * decayRate, decayLimit); + decayRate = pid.errorDecayRateCyclic * pidTableLookup(curve, pidProfile->error_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.08f; + decayLimit = pid.errorDecayLimitCyclic * pidTableLookup(curve, pidProfile->error_decay_limit_curve, LOOKUP_CURVE_POINTS) * 0.08f; } else { - decayRate = pid.errorDecayRateGround / pid.dT; - decayLimit = 0; - errorDecay = pid.data[axis].axisError * decayRate; + decayRate = pid.errorDecayRateGround; + decayLimit = pid.errorDecayLimitCyclic; } + errorDecay = limitf(pid.data[axis].axisError * decayRate, decayLimit); + pid.data[axis].axisError -= errorDecay * pid.dT; DEBUG_AXIS(ERROR_DECAY, axis, 0, decayRate * 100); @@ -1079,9 +1079,9 @@ static void pidApplyYawMode3(void) pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError; // Apply error decay - pid.data[axis].axisError -= isSpooledUp() ? - limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw): - pid.data[axis].axisError * pid.errorDecayRateGround; + pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError * + (isSpooledUp() ? pid.errorDecayRateYaw : pid.errorDecayRateGround), + pid.errorDecayLimitYaw); //// Feedforward diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index 9d75589f34..53229cebc3 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -53,9 +53,9 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_mode = 0, .dterm_mode_yaw = 0, .error_decay_time_ground = 25, - .error_decay_time_cyclic = 18, + .error_decay_time_cyclic = 20, .error_decay_time_yaw = 0, - .error_decay_limit_cyclic = 20, + .error_decay_limit_cyclic = 12, .error_decay_limit_yaw = 0, .error_decay_rate_curve = { 12,13,14,15,17,20,23,28,36,49,78,187,250,250,250,250 }, .error_decay_limit_curve = { 12,12,12,12,12,12,12,12,12,12,12,12,13,14,15,16 },