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

Make Anti-windup affect anti gravity coefficient

Browse source
This commit is contained in:
Tobias Bolin 2022-10-08 17:29:57 +02:00
parent 5ca47fed6d
commit c61efe1e2e
1 changed files with 4 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -884,13 +884,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
} }
DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf((1 + (pidRuntime.itermAccelerator / pidRuntime.pidCoefficient[FD_PITCH].Ki)) * 1000)); DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf((1 + (pidRuntime.itermAccelerator / pidRuntime.pidCoefficient[FD_PITCH].Ki)) * 1000));
// amount of antigravity added relative to user's pitch iTerm coefficient // amount of antigravity added relative to user's pitch iTerm coefficient
pidRuntime.itermAccelerator *= pidRuntime.dT;
// used later to increase iTerm // used later to increase iTerm
// iTerm windup (attenuation of iTerm if motorMix range is large) // iTerm windup (attenuation of iTerm if motorMix range is large)
float dynCi = pidRuntime.dT; float dynCi = 1.0;
if (pidRuntime.itermWindupPointInv > 1.0f) { if (pidRuntime.itermWindupPointInv > 1.0f) {
dynCi *= constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f); dynCi = constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
} }
// Precalculate gyro delta for D-term here, this allows loop unrolling // Precalculate gyro delta for D-term here, this allows loop unrolling
@ -1016,8 +1015,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
} }
} }
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
pidData[axis].I = constrainf(previousIterm + (Ki * dynCi + pidRuntime.itermAccelerator) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit); pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
// -----calculate pidSetpointDelta // -----calculate pidSetpointDelta
float pidSetpointDelta = 0; float pidSetpointDelta = 0;