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
parent
a41e9f8c8c
commit
5667ab5d30
6
pi.c
6
pi.c
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue