Fix initial drift calculation in PI servo.

Convert the calculated drift to ppb and also clamp it.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
master
Miroslav Lichvar 2013-01-14 17:09:08 +01:00 committed by Richard Cochran
parent d4db76d64c
commit 8a23bf1d66
1 changed files with 5 additions and 1 deletions

6
pi.c
View File

@ -76,8 +76,12 @@ static double pi_sample(struct servo *servo,
s->count = 2;
break;
case 2:
s->drift += (s->offset[1] - s->offset[0]) /
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;