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.

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

Fix PID derivative kickoff and reset #1403

Merged
merged 2 commits into from
Sep 9, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
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
6 changes: 3 additions & 3 deletions src/modules/interface/controller/attitude_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,17 +56,17 @@ void attitudeControllerCorrectRatePID(
/**
* Reset controller roll attitude PID
*/
void attitudeControllerResetRollAttitudePID(void);
void attitudeControllerResetRollAttitudePID(float rollActual);

/**
* Reset controller pitch attitude PID
*/
void attitudeControllerResetPitchAttitudePID(void);
void attitudeControllerResetPitchAttitudePID(float pitchActual);

/**
* Reset controller roll, pitch and yaw PID's.
*/
void attitudeControllerResetAllPID(void);
void attitudeControllerResetAllPID(float rollActual, float pitchActual, float yawActual);

/**
* Get the actuator output.
Expand Down
2 changes: 1 addition & 1 deletion src/modules/interface/controller/position_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
// A position controller calculate the thrust, roll, pitch to approach
// a 3D position setpoint
void positionControllerInit();
void positionControllerResetAllPID();
void positionControllerResetAllPID(float xActual, float yActual, float zActual);
void positionControllerResetAllfilters();
void positionController(float* thrust, attitude_t *attitude, const setpoint_t *setpoint,
const state_t *state);
Expand Down
42 changes: 18 additions & 24 deletions src/modules/src/controller/attitude_pid_controller.c
Original file line number Diff line number Diff line change
Expand Up @@ -143,14 +143,14 @@ void attitudeControllerCorrectRatePID(
float rollRateDesired, float pitchRateDesired, float yawRateDesired)
{
pidSetDesired(&pidRollRate, rollRateDesired);
rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, true));
rollOutput = saturateSignedInt16(pidUpdate(&pidRollRate, rollRateActual, false));

pidSetDesired(&pidPitchRate, pitchRateDesired);
pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, true));
pitchOutput = saturateSignedInt16(pidUpdate(&pidPitchRate, pitchRateActual, false));

pidSetDesired(&pidYawRate, yawRateDesired);

yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, true));
yawOutput = saturateSignedInt16(pidUpdate(&pidYawRate, yawRateActual, false));
}

void attitudeControllerCorrectAttitudePID(
Expand All @@ -159,41 +159,35 @@ void attitudeControllerCorrectAttitudePID(
float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired)
{
pidSetDesired(&pidRoll, eulerRollDesired);
*rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, true);
*rollRateDesired = pidUpdate(&pidRoll, eulerRollActual, false);

// Update PID for pitch axis
pidSetDesired(&pidPitch, eulerPitchDesired);
*pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, true);
*pitchRateDesired = pidUpdate(&pidPitch, eulerPitchActual, false);

// Update PID for yaw axis
float yawError;
yawError = eulerYawDesired - eulerYawActual;
if (yawError > 180.0f)
yawError -= 360.0f;
else if (yawError < -180.0f)
yawError += 360.0f;
pidSetError(&pidYaw, yawError);
*yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, false);
pidSetDesired(&pidYaw, eulerYawDesired);
*yawRateDesired = pidUpdate(&pidYaw, eulerYawActual, true);
}

void attitudeControllerResetRollAttitudePID(void)
void attitudeControllerResetRollAttitudePID(float rollActual)
{
pidReset(&pidRoll);
pidReset(&pidRoll, rollActual);
}

void attitudeControllerResetPitchAttitudePID(void)
void attitudeControllerResetPitchAttitudePID(float pitchActual)
{
pidReset(&pidPitch);
pidReset(&pidPitch, pitchActual);
}

void attitudeControllerResetAllPID(void)
void attitudeControllerResetAllPID(float rollActual, float pitchActual, float yawActual)
{
pidReset(&pidRoll);
pidReset(&pidPitch);
pidReset(&pidYaw);
pidReset(&pidRollRate);
pidReset(&pidPitchRate);
pidReset(&pidYawRate);
pidReset(&pidRoll, rollActual);
pidReset(&pidPitch, pitchActual);
pidReset(&pidYaw, yawActual);
pidReset(&pidRollRate, 0);
pidReset(&pidPitchRate, 0);
pidReset(&pidYawRate, 0);
}

void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw)
Expand Down
8 changes: 4 additions & 4 deletions src/modules/src/controller/controller_indi.c
Original file line number Diff line number Diff line change
Expand Up @@ -225,11 +225,11 @@ void controllerINDI(control_t *control, const setpoint_t *setpoint,
// behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = radians(setpoint->attitudeRate.roll);
attitudeControllerResetRollAttitudePID();
attitudeControllerResetRollAttitudePID(state->attitude.roll);
}
if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = radians(setpoint->attitudeRate.pitch);
attitudeControllerResetPitchAttitudePID();
attitudeControllerResetPitchAttitudePID(state->attitude.pitch);
}

/*
Expand Down Expand Up @@ -317,8 +317,8 @@ void controllerINDI(control_t *control, const setpoint_t *setpoint,
float_rates_zero(&indi.u_in);

if(indi.thrust == 0){
attitudeControllerResetAllPID();
positionControllerResetAllPID();
attitudeControllerResetAllPID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw);
positionControllerResetAllPID(state->position.x, state->position.y, state->position.z);

// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = -state->attitude.yaw;
Expand Down
8 changes: 4 additions & 4 deletions src/modules/src/controller/controller_pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -113,11 +113,11 @@ void controllerPid(control_t *control, const setpoint_t *setpoint,
// behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = setpoint->attitudeRate.roll;
attitudeControllerResetRollAttitudePID();
attitudeControllerResetRollAttitudePID(state->attitude.roll);
}
if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID();
attitudeControllerResetPitchAttitudePID(state->attitude.pitch);
}

// TODO: Investigate possibility to subtract gyro drift.
Expand Down Expand Up @@ -154,8 +154,8 @@ void controllerPid(control_t *control, const setpoint_t *setpoint,
cmd_pitch = control->pitch;
cmd_yaw = control->yaw;

attitudeControllerResetAllPID();
positionControllerResetAllPID();
attitudeControllerResetAllPID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw);
positionControllerResetAllPID(state->position.x, state->position.y, state->position.z);
matejkarasek marked this conversation as resolved.
Show resolved Hide resolved

// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
Expand Down
16 changes: 8 additions & 8 deletions src/modules/src/controller/position_controller_pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -184,7 +184,7 @@ static float runPid(float input, struct pidAxis_s *axis, float setpoint, float d
axis->setpoint = setpoint;

pidSetDesired(&axis->pid, axis->setpoint);
return pidUpdate(&axis->pid, input, true);
return pidUpdate(&axis->pid, input, false);
}


Expand Down Expand Up @@ -266,14 +266,14 @@ void velocityController(float* thrust, attitude_t *attitude, const Axis3f* setpo
*thrust = constrain(*thrust, 0, UINT16_MAX);
}

void positionControllerResetAllPID()
void positionControllerResetAllPID(float xActual, float yActual, float zActual)
{
pidReset(&this.pidX.pid);
pidReset(&this.pidY.pid);
pidReset(&this.pidZ.pid);
pidReset(&this.pidVX.pid);
pidReset(&this.pidVY.pid);
pidReset(&this.pidVZ.pid);
pidReset(&this.pidX.pid, xActual);
pidReset(&this.pidY.pid, yActual);
pidReset(&this.pidZ.pid, zActual);
pidReset(&this.pidVX.pid, 0);
pidReset(&this.pidVY.pid, 0);
pidReset(&this.pidVZ.pid, 0);
}

void positionControllerResetAllfilters() {
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/crtp_commander_rpyt.c
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk)
if (altHoldMode) {
if (!modeSet) { //Reset filter and PID values on first initiation of assist mode to prevent sudden reactions.
modeSet = true;
positionControllerResetAllPID();
positionControllerResetAllPID(0, 0, 0); // TODO: initialize with actual x, y, z position
positionControllerResetAllfilters();
}
setpoint->thrust = 0;
Expand Down
19 changes: 5 additions & 14 deletions src/utils/interface/pid.h
Original file line number Diff line number Diff line change
Expand Up @@ -86,21 +86,20 @@ void pidSetIntegralLimit(PidObject* pid, const float limit);
/**
* Reset the PID error values
*
* @param[in] pid A pointer to the pid object.
* @param[in] limit Pid integral swing limit.
* @param[in] pid A pointer to the pid object.
* @param[in] initial Initial value of the controlled state.
*/
void pidReset(PidObject* pid);
void pidReset(PidObject* pid, float initial);

/**
* Update the PID parameters.
*
* @param[in] pid A pointer to the pid object.
* @param[in] measured The measured value
* @param[in] updateError Set to TRUE if error should be calculated.
* Set to False if pidSetError() has been used.
* @param[in] isYawAngle Set to TRUE if it is a PID on yaw angle, set to false otherwise
* @return PID algorithm output
*/
float pidUpdate(PidObject* pid, const float measured, const bool updateError);
float pidUpdate(PidObject* pid, const float measured, const bool isYawAngle);

/**
* Set a new set point for the PID to track.
Expand All @@ -122,14 +121,6 @@ float pidGetDesired(PidObject* pid);
*/
bool pidIsActive(PidObject* pid);

/**
* Set a new error. Use if a special error calculation is needed.
*
* @param[in] pid A pointer to the pid object.
* @param[in] error The new error
*/
void pidSetError(PidObject* pid, const float error);

/**
* Set a new proportional gain for the PID.
*
Expand Down
42 changes: 25 additions & 17 deletions src/utils/src/pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -54,15 +54,20 @@ void pidInit(PidObject* pid, const float desired, const float kp,
}
}

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

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

if (isYawAngle){
if (pid->error > 180.0f){
pid->error -= 360.0f;
} else if (pid->error < -180.0f){
pid->error += 360.0f;
}
}

pid->outP = pid->kp * pid->error;
output += pid->outP;

Expand All @@ -73,15 +78,23 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
* in the setpoint. By using the process variable for the derivative calculation, we achieve
* smoother and more stable control during setpoint changes.
*/
float deriv = -(measured - pid->prevMeasured) / pid->dt;
float delta = -(measured - pid->prevMeasured);

if (isYawAngle){
matejkarasek marked this conversation as resolved.
Show resolved Hide resolved
if (delta > 180.0f){
delta -= 360.0f;
} else if (delta < -180.0f){
delta += 360.0f;
}
}

#if CONFIG_CONTROLLER_PID_FILTER_ALL
pid->deriv = deriv;
pid->deriv = delta / pid->dt;
#else
if (pid->enableDFilter){
pid->deriv = lpf2pApply(&pid->dFilter, deriv);
pid->deriv = lpf2pApply(&pid->dFilter, delta / pid->dt);
} else {
pid->deriv = deriv;
pid->deriv = delta / pid->dt;
}
#endif
if (isnan(pid->deriv)) {
Expand Down Expand Up @@ -116,7 +129,7 @@ float pidUpdate(PidObject* pid, const float measured, const bool updateError)
if (isnan(output)) {
output = 0;
}
#endif
#endif

// Constrain the total PID output (unless the outputLimit is zero)
if(pid->outputLimit != 0)
Expand All @@ -133,19 +146,14 @@ void pidSetIntegralLimit(PidObject* pid, const float limit) {
pid->iLimit = limit;
}

void pidReset(PidObject* pid)
void pidReset(PidObject* pid, const float actual)
{
pid->error = 0;
pid->prevMeasured = 0;
pid->prevMeasured = actual;
pid->integ = 0;
pid->deriv = 0;
}

void pidSetError(PidObject* pid, const float error)
{
pid->error = error;
}

void pidSetDesired(PidObject* pid, const float desired)
{
pid->desired = desired;
Expand Down