From 5667ab5d304319ce41e7e4ddf8b4cdc616cf6221 Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Tue, 5 Feb 2013 17:36:10 +0100 Subject: [PATCH] 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 --- pi.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/pi.c b/pi.c index 825ec22..c3a4035 100644 --- a/pi.c +++ b/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)