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 a pid_apply_setpoint which takes in the setpoint and feedback term

Browse Source 
This version allows performing setpoint weighting, currently on the derivative
component.
This commit is contained in:
James Cotton 2012-09-11 13:21:19 -05:00
parent 11b099b1ef
commit e4a167dca1
2 changed files with 46 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,17 +39,58 @@ static float bound(float val, float range);
//! Store the shared time constant for the derivative cutoff.
static float deriv_tau = 7.9577e-3f;
//! Store the setpoint weight to apply for the derivative term
static float deriv_gamma = 1.0;
/**
* Update the PID computation
* @param[in] pid The PID struture which stores temporary information
* @param[in] err The error term
* @param[in] dT The time step
* @returns Output the computed controller value
*/
float pid_apply(struct pid *pid, const float err, float dT)
{
{
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
// Calculate DT1 term
float diff = (err - pid->lastErr);
float dterm = 0;
pid->lastErr = err;
if(pid->d && dT)
{
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
return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
}
/**
* Update the PID computation with setpoint weighting on the derivative
* @param[in] pid The PID struture which stores temporary information
* @param[in] setpoint The setpoint to use
* @param[in] measured The measured value of output
* @param[in] dT The time step
* @returns Output the computed controller value
*
* This version of apply uses setpoint weighting for the derivative component so the gain
* on the gyro derivative can be different than the gain on the setpoint derivative
*/
float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, float dT)
{
float err = setpoint - measured;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
// Calculate DT1 term, fixed T1 timeconstant
// Calculate DT1 term,
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
float dterm = 0;
pid->lastErr = err;
if(pid->d && dT)
{
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
@ -78,9 +119,10 @@ void pid_zero(struct pid *pid)
* @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)
void pid_configure_derivative(float cutoff, float g)
{
deriv_tau = 1.0f / (2 * F_PI * cutoff);
deriv_gamma = g;
}
/**

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

@ -44,6 +44,7 @@ struct pid {
//! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT);
float pid_apply_setpoint(struct pid *pid, const float setpoint, const float measured, 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);