Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

PID avoid derivative kick #1337

Merged
merged 1 commit into from
Jan 30, 2024
Merged

PID avoid derivative kick #1337

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/utils/interface/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ typedef struct
{
float desired; //< set point
float error; //< error
float prevError; //< previous error
float prevMeasured; //< previous measurement
float integ; //< integral
float deriv; //< derivative
float kp; //< proportional gain
Expand Down
118 changes: 57 additions & 61 deletions src/utils/src/pid.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ void pidInit(PidObject* pid, const float desired, const float kp,
bool enableDFilter)
{
pid->error = 0;
pid->prevError = 0;
pid->prevMeasured = 0;
pid->integ = 0;
pid->deriv = 0;
pid->desired = desired;
Expand All @@ -56,86 +56,82 @@ void pidInit(PidObject* pid, const float desired, const float kp,

float pidUpdate(PidObject* pid, const float measured, const bool updateError)
{
float output = 0.0f;
float output = 0.0f;

if (updateError)
{
pid->error = pid->desired - measured;
}

pid->outP = pid->kp * pid->error;
output += pid->outP;
if (updateError)
{
pid->error = pid->desired - measured;
}

float deriv = (pid->error - pid->prevError) / pid->dt;

#if CONFIG_CONTROLLER_PID_FILTER_ALL
pid->outP = pid->kp * pid->error;
output += pid->outP;

float deriv = -(measured - pid->prevMeasured) / pid->dt;

#if CONFIG_CONTROLLER_PID_FILTER_ALL
pid->deriv = deriv;
#else
if (pid->enableDFilter){
pid->deriv = lpf2pApply(&pid->dFilter, deriv);
} else {
pid->deriv = deriv;
#else
if (pid->enableDFilter){
pid->deriv = lpf2pApply(&pid->dFilter, deriv);
} else {
pid->deriv = deriv;
}
#endif
if (isnan(pid->deriv)) {
pid->deriv = 0;
}
pid->outD = pid->kd * pid->deriv;
output += pid->outD;
#endif
if (isnan(pid->deriv)) {
pid->deriv = 0;
}
pid->outD = pid->kd * pid->deriv;
output += pid->outD;

pid->integ += pid->error * pid->dt;
pid->integ += pid->error * pid->dt;

// Constrain the integral (unless the iLimit is zero)
if(pid->iLimit != 0)
{
pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
}
// Constrain the integral (unless the iLimit is zero)
if(pid->iLimit != 0)
{
pid->integ = constrain(pid->integ, -pid->iLimit, pid->iLimit);
}

pid->outI = pid->ki * pid->integ;
output += pid->outI;

pid->outI = pid->ki * pid->integ;
output += pid->outI;

pid->outFF = pid->kff * pid->desired;
output += pid->outFF;

#if CONFIG_CONTROLLER_PID_FILTER_ALL
//filter complete output instead of only D component to compensate for increased noise from increased barometer influence
if (pid->enableDFilter)
{
output = lpf2pApply(&pid->dFilter, output);
}
else {
output = output;
}
if (isnan(output)) {
output = 0;
}
#endif



// Constrain the total PID output (unless the outputLimit is zero)
if(pid->outputLimit != 0)
pid->outFF = pid->kff * pid->desired;
output += pid->outFF;

#if CONFIG_CONTROLLER_PID_FILTER_ALL
//filter complete output instead of only D component to compensate for increased noise from increased barometer influence
if (pid->enableDFilter)
{
output = constrain(output, -pid->outputLimit, pid->outputLimit);
output = lpf2pApply(&pid->dFilter, output);
}
else {
output = output;
}
if (isnan(output)) {
output = 0;
}
#endif

// Constrain the total PID output (unless the outputLimit is zero)
if(pid->outputLimit != 0)
{
output = constrain(output, -pid->outputLimit, pid->outputLimit);
}

pid->prevError = pid->error;
pid->prevMeasured = measured;

return output;
return output;
}

void pidSetIntegralLimit(PidObject* pid, const float limit) {
pid->iLimit = limit;
}


void pidReset(PidObject* pid)
{
pid->error = 0;
pid->prevError = 0;
pid->integ = 0;
pid->deriv = 0;
pid->error = 0;
pid->prevMeasured = 0;
pid->integ = 0;
pid->deriv = 0;
}

void pidSetError(PidObject* pid, const float error)
Expand Down