Check sample time stamps in drift calculation.

Before calculating the clock drift in the PI servo, make sure
the first sample is older than the second sample to avoid getting
invalid drift or NaN.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
master
Miroslav Lichvar 2013-02-05 17:36:10 +01:00 committed by Richard Cochran
parent a41e9f8c8c
commit 5667ab5d30
1 changed files with 6 additions and 0 deletions

6
pi.c
View File

@ -73,6 +73,12 @@ static double pi_sample(struct servo *servo,
s->offset[1] = offset;
s->local[1] = local_ts;
/* Make sure the first sample is older than the second. */
if (s->local[0] >= s->local[1]) {
s->count = 0;
break;
}
s->drift += (s->offset[1] - s->offset[0]) * 1e9 /
(s->local[1] - s->local[0]);
if (s->drift < -s->maxppb)