rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

PID: Add the 20 Hz low pass filter to the derivative term

Browse Source
This commit is contained in:
James Cotton 2012-09-10 03:10:41 -05:00
parent 9763a70364
commit fa9a616b4c
2 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/Libraries/math/pid.c
View File

@ -37,12 +37,21 @@ static float bound(float val, float range);
float pid_apply(struct pid *pid, const float err, float dT) float pid_apply(struct pid *pid, const float err, float dT)
{ {
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
float dterm = 0;
pid->lastErr = err; pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision // Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f); pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f); pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
// Calculate DT1 term, fixed T1 timeconstant
if(pid->d && dT)
{
dterm = pid->lastDer + dT / ( dT + 7.9577e-3f) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
} }
/** /**
@ -56,6 +65,7 @@ void pid_zero(struct pid *pid)
pid->iAccumulator = 0; pid->iAccumulator = 0;
pid->lastErr = 0; pid->lastErr = 0;
pid->lastDer = 0;
} }
/** /**

1
flight/Libraries/math/pid.h
View File

@ -39,6 +39,7 @@ struct pid {
float iLim; float iLim;
float iAccumulator; float iAccumulator;
float lastErr; float lastErr;
float lastDer;
}; };
//! Methods to use the pid structures //! Methods to use the pid structures