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 <mlichvar@redhat.com>
Signed-off-by: Richard Cochran <richardcochran@gmail.com>
master
Miroslav Lichvar 2014-01-08 15:44:37 +01:00 committed by Richard Cochran
parent e63a6ea89b
commit 2ca067dcac
1 changed files with 3 additions and 2 deletions

3
pi.c
View File

@ -107,7 +107,8 @@ static double pi_sample(struct servo *servo,
break;
}
s->drift += (s->offset[1] - s->offset[0]) * 1e9 /
/* 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;