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

PID: Add a function to set the cutoff for the derivative term

Browse Source 
Also contains a term for setting the deriative setpoint in the future
This commit is contained in:
James Cotton 2012-09-11 12:25:01 -05:00
parent 4d87af38b6
commit 42bbd52d68
2 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,9 +31,14 @@
#include "openpilot.h"
#include "pid.h"
#define F_PI ((float) M_PI)
//! Private method
static float bound(float val, float range);
//! Store the shared time constant for the derivative cutoff.
static float deriv_tau = 7.9577e-3f;
float pid_apply(struct pid *pid, const float err, float dT)
{
float diff = (err - pid->lastErr);
@ -47,7 +52,7 @@ float pid_apply(struct pid *pid, const float err, float 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);
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((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
@ -68,6 +73,16 @@ void pid_zero(struct pid *pid)
pid->lastDer = 0;
}
/**
* @brief Configure the common terms that alter ther derivative
* @param[in] cutoff The cutoff frequency (in Hz)
* @param[in] gamma The gamma term for setpoint shaping (unsused now)
*/
void pid_configure_derivative(float cutoff, float gamma)
{
deriv_tau = 1.0f / (2 * F_PI * cutoff);
}
/**
* Configure the settings for a pid structure
* @param[out] pid The PID structure to configure

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

@ -46,5 +46,6 @@ struct pid {
float pid_apply(struct pid *pid, const float err, float dT);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);
#endif /* PID_H */