rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-11 01:54:14 +01:00
Code Issues Releases Activity

Tweak the integral calculation so it is scaled in ms internally. This avoids a

Browse Source 
loss of precision on the accumulation.
This commit is contained in:
James Cotton 2011-06-22 18:51:25 -05:00
parent 373689207a
commit a09642675c
1 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/Modules/Stabilization/stabilization.c
View File

@ -296,15 +296,15 @@ float ApplyPid(pid_type * pid, const float err)
{ {
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
pid->lastErr = err; pid->lastErr = err;
pid->iAccumulator += err * pid->i * dT;
if(fabs(pid->iAccumulator) > pid->iLim) { // Scale up accumulator by 1000 while computing to avoid losing precision
if(pid->iAccumulator >0) { pid->iAccumulator += err * (pid->i * dT * 1000);
pid->iAccumulator = pid->iLim; if(pid->iAccumulator > (pid->iLim * 1000)) {
} else { pid->iAccumulator = pid->iLim * 1000;
pid->iAccumulator = -pid->iLim; } else if (pid->iAccumulator < -(pid->iLim * 1000)) {
} pid->iAccumulator = -pid->iLim * 1000;
} }
return ((err * pid->p) + pid->iAccumulator + (diff * pid->d / dT)); return ((err * pid->p) + pid->iAccumulator / 1000 + (diff * pid->d / dT));
} }