diff --git a/PID.c b/PID.c index 877c32d..b1847e6 100644 --- a/PID.c +++ b/PID.c @@ -47,8 +47,8 @@ float PIDController_Update(PIDController *pid, float setpoint, float measurement /* * Derivative (band-limited differentiator) */ - - pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */ + /* Note: derivative on measurement, therefore minus sign in front of equation! */ + pid->differentiator = (-(2.0f * pid->Kd * (measurement - pid->prevMeasurement)) + (2.0f * pid->tau - pid->T) * pid->differentiator) / (2.0f * pid->tau + pid->T);