diff --git a/src/modules/src/controller/controller_pid.c b/src/modules/src/controller/controller_pid.c index b91543ce5..cd2d0ab10 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 f97b8b017..b1b5cbff8 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;