From 42f1a94bea3e0bb613a67f871be79302f12fe4c8 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Fri, 23 Aug 2024 17:11:41 +0200 Subject: [PATCH 1/2] Fix PID derivative kickoff and reset --- .../controller/attitude_controller.h | 6 +-- .../controller/position_controller.h | 2 +- .../src/controller/attitude_pid_controller.c | 42 ++++++++----------- src/modules/src/controller/controller_indi.c | 8 ++-- src/modules/src/controller/controller_pid.c | 8 ++-- .../src/controller/position_controller_pid.c | 16 +++---- src/modules/src/crtp_commander_rpyt.c | 2 +- src/utils/interface/pid.h | 19 +++------ src/utils/src/pid.c | 42 +++++++++++-------- 9 files changed, 69 insertions(+), 76 deletions(-) diff --git a/src/modules/interface/controller/attitude_controller.h b/src/modules/interface/controller/attitude_controller.h index 96cb5da2aa..14d1d62134 100644 --- a/src/modules/interface/controller/attitude_controller.h +++ b/src/modules/interface/controller/attitude_controller.h @@ -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. diff --git a/src/modules/interface/controller/position_controller.h b/src/modules/interface/controller/position_controller.h index 33d692d03e..87a1545231 100644 --- a/src/modules/interface/controller/position_controller.h +++ b/src/modules/interface/controller/position_controller.h @@ -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); diff --git a/src/modules/src/controller/attitude_pid_controller.c b/src/modules/src/controller/attitude_pid_controller.c index 92aa365c35..5b81dccaaf 100644 --- a/src/modules/src/controller/attitude_pid_controller.c +++ b/src/modules/src/controller/attitude_pid_controller.c @@ -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( @@ -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) diff --git a/src/modules/src/controller/controller_indi.c b/src/modules/src/controller/controller_indi.c index 784102cf51..3a62fe6c50 100644 --- a/src/modules/src/controller/controller_indi.c +++ b/src/modules/src/controller/controller_indi.c @@ -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); } /* @@ -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; diff --git a/src/modules/src/controller/controller_pid.c b/src/modules/src/controller/controller_pid.c index 6b481aff8f..b91543ce58 100644 --- a/src/modules/src/controller/controller_pid.c +++ b/src/modules/src/controller/controller_pid.c @@ -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. @@ -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); // Reset the calculated YAW angle for rate control attitudeDesired.yaw = state->attitude.yaw; diff --git a/src/modules/src/controller/position_controller_pid.c b/src/modules/src/controller/position_controller_pid.c index 9dfb66148d..ab87fdf310 100644 --- a/src/modules/src/controller/position_controller_pid.c +++ b/src/modules/src/controller/position_controller_pid.c @@ -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); } @@ -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() { diff --git a/src/modules/src/crtp_commander_rpyt.c b/src/modules/src/crtp_commander_rpyt.c index a281636362..ef8a295bf8 100644 --- a/src/modules/src/crtp_commander_rpyt.c +++ b/src/modules/src/crtp_commander_rpyt.c @@ -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; diff --git a/src/utils/interface/pid.h b/src/utils/interface/pid.h index cbdfb828d4..16811fd58d 100644 --- a/src/utils/interface/pid.h +++ b/src/utils/interface/pid.h @@ -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. @@ -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. * diff --git a/src/utils/src/pid.c b/src/utils/src/pid.c index b65bb02b28..f97b8b0174 100644 --- a/src/utils/src/pid.c +++ b/src/utils/src/pid.c @@ -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; @@ -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){ + 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)) { @@ -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) @@ -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; From 64d8bc57b2a326769b2b0aa3d6c75d69cbbb8460 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Mon, 9 Sep 2024 10:57:17 +0200 Subject: [PATCH 2/2] fix format and add comment --- src/modules/src/controller/controller_pid.c | 2 +- src/utils/src/pid.c | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/src/controller/controller_pid.c b/src/modules/src/controller/controller_pid.c index b91543ce58..cd2d0ab108 100644 --- a/src/modules/src/controller/controller_pid.c +++ b/src/modules/src/controller/controller_pid.c @@ -155,7 +155,7 @@ void controllerPid(control_t *control, const setpoint_t *setpoint, cmd_yaw = control->yaw; attitudeControllerResetAllPID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw); - positionControllerResetAllPID(state->position.x, state->position.y, state->position.z); + positionControllerResetAllPID(state->position.x, state->position.y, state->position.z); // Reset the calculated YAW angle for rate control attitudeDesired.yaw = state->attitude.yaw; diff --git a/src/utils/src/pid.c b/src/utils/src/pid.c index f97b8b0174..b1b5cbff81 100644 --- a/src/utils/src/pid.c +++ b/src/utils/src/pid.c @@ -80,6 +80,7 @@ float pidUpdate(PidObject* pid, const float measured, const bool isYawAngle) */ float delta = -(measured - pid->prevMeasured); + // For yaw measurements, take care of spikes when crossing 180deg <-> -180deg if (isYawAngle){ if (delta > 180.0f){ delta -= 360.0f;