From 7efdfba8d6d5431c9039a353fcfaae28ae0c325d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gauthier=20=C3=96stervall?= Date: Wed, 5 Nov 2025 13:39:17 +0100 Subject: [PATCH] Fix D component The value from the previous update, used for the bilinear filter, was getting negated to produce the new value. "Memory" got oscillating between positive and negative. This change moves the negation to the input of the filter (derivative on measurement) but keeps the filter out of that negation. --- PID.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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);