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

OP-1511 added comment to filter code for better readability

Browse Source
This commit is contained in:
Corvus Corax 2014-09-27 23:18:21 +02:00
parent 316f7fcf02
commit 2586dafa8c
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/libraries/math/pid.c
View File

@ -89,9 +89,12 @@ float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if (pid->d > 0.0f && dT > 0.0f) {
// low pass filter derivative term. below formula is the same as
// dterm = (1-alpha)*pid->lastDer + alpha * (...)/dT
// with alpha = dT/(deriv_tau+dT)
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * 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
pid->lastDer = dterm;
}
return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}