diff --git a/pi.c b/pi.c
index 427cb3e..252e231 100644
--- a/pi.c
+++ b/pi.c
@@ -31,6 +31,9 @@
 
 #define NSEC_PER_SEC 1000000000
 
+#define DRIFT_EST_KI 0.02
+#define DRIFT_EST_MAX 1000.0
+
 /* These take their values from the configuration file. (see ptp4l.c) */
 double configured_pi_kp = 0.0;
 double configured_pi_ki = 0.0;
@@ -46,7 +49,8 @@ struct pi_servo {
 	double kp;
 	double ki;
 	double max_offset;
-	int count;
+	int state;
+	unsigned int count;
 };
 
 static void pi_destroy(struct servo *servo)
@@ -60,23 +64,41 @@ static double pi_sample(struct servo *servo,
 			uint64_t local_ts,
 			enum servo_state *state)
 {
-	double ki_term, ppb = 0.0;
+	double drift_est_interval, ki_term, ppb = 0.0;
 	struct pi_servo *s = container_of(servo, struct pi_servo, servo);
 
-	switch (s->count) {
+	switch (s->state) {
 	case 0:
 		s->offset[0] = offset;
 		s->local[0] = local_ts;
 		*state = SERVO_UNLOCKED;
+		s->state = 1;
 		s->count = 1;
 		break;
 	case 1:
 		s->offset[1] = offset;
 		s->local[1] = local_ts;
+		s->count++;
 
 		/* Make sure the first sample is older than the second. */
 		if (s->local[0] >= s->local[1]) {
-			s->count = 0;
+			*state = SERVO_UNLOCKED;
+			s->state = 0;
+			break;
+		}
+
+		/* The interval between the two samples used to estimate the
+		   drift is inversely proportional to the integral constant to
+		   make the initial convergence faster. */
+		drift_est_interval = DRIFT_EST_KI / s->ki /
+			((s->local[1] - s->local[0]) / (s->count - 1));
+		if (drift_est_interval > DRIFT_EST_MAX)
+			drift_est_interval = DRIFT_EST_MAX;
+		char *x = getenv("TEST_EST");
+		if (x)
+			drift_est_interval = atoi(x);
+		if (s->count - 1 < drift_est_interval) {
+			*state = SERVO_UNLOCKED;
 			break;
 		}
 
@@ -89,7 +111,7 @@ static double pi_sample(struct servo *servo,
 
 		*state = SERVO_JUMP;
 		ppb = s->drift;
-		s->count = 2;
+		s->state = 2;
 		break;
 	case 2:
 		/*
@@ -101,7 +123,7 @@ static double pi_sample(struct servo *servo,
 		 */
 		if (s->max_offset && (s->max_offset < fabs(offset))) {
 			*state = SERVO_UNLOCKED;
-			s->count = 0;
+			s->state = 0;
 			break;
 		}