From 8a23bf1d661a399409cc9546798a5d8140da6285 Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Mon, 14 Jan 2013 17:09:08 +0100
Subject: [PATCH] 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>
---
 pi.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/pi.c b/pi.c
index b2d1470..9b3b09e 100644
--- a/pi.c
+++ b/pi.c
@@ -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;