From 2ca067dcace97e1b5c28f1da3755b9d33f02bf0b Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Wed, 8 Jan 2014 15:44:37 +0100 Subject: [PATCH] Fix drift calculation in PI servo with large values. When the drift value is adjusted by the newly measured frequency offset, multiply the frequencies instead of adding the measured offset to the old value to get accurate result even when updating a large drift. [ RC: use a simpler form of the frequency update calculation. ] Signed-off-by: Miroslav Lichvar Signed-off-by: Richard Cochran --- pi.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/pi.c b/pi.c index bd78e40..689b232 100644 --- a/pi.c +++ b/pi.c @@ -107,8 +107,9 @@ static double pi_sample(struct servo *servo, break; } - s->drift += (s->offset[1] - s->offset[0]) * 1e9 / - (s->local[1] - s->local[0]); + /* Adjust drift by the measured frequency offset. */ + s->drift += (1e9 - s->drift) * (s->offset[1] - s->offset[0]) / + (s->local[1] - s->local[0]); if (s->drift < -s->maxppb) s->drift = -s->maxppb; else if (s->drift > s->maxppb)