Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Cleanup code // add unittestst for iterm windup

Browse source
This commit is contained in:
borisbstyle 2018-09-10 12:27:53 +02:00
parent 261f8d679a
commit 00dbaf9fa7
5 changed files with 32 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/flight/pid.c
View file

@ -445,7 +445,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;
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
const float ITermWindupPoint = ((float)pidProfile->itermWindupPointPercent - 0.001f) / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000;
@ -985,12 +985,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate I component
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated
pidData[axis].I = ITermNew;
}
pidData[axis].I = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
// -----calculate D component
if (pidCoefficient[axis].Kd > 0) {