From d8cb9be46a225a159168581531d401fa2eff8c68 Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Mon, 14 Jan 2013 17:09:11 +0100 Subject: [PATCH] Remove unnecessary states in PI servo. Step the clock as soon as the second sample is collected. Signed-off-by: Miroslav Lichvar --- pi.c | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/pi.c b/pi.c index 934fff6..825ec22 100644 --- a/pi.c +++ b/pi.c @@ -72,25 +72,19 @@ static double pi_sample(struct servo *servo, case 1: s->offset[1] = offset; s->local[1] = local_ts; - *state = SERVO_UNLOCKED; - s->count = 2; - break; - case 2: + s->drift += (s->offset[1] - s->offset[0]) * 1e9 / (s->local[1] - s->local[0]); if (s->drift < -s->maxppb) s->drift = -s->maxppb; else if (s->drift > s->maxppb) s->drift = s->maxppb; - *state = SERVO_UNLOCKED; - s->count = 3; - break; - case 3: + *state = SERVO_JUMP; ppb = s->drift; - s->count = 4; + s->count = 2; break; - case 4: + case 2: /* * reset the clock servo when offset is greater than the max * offset value. Note that the clock jump will be performed in