From 6edbea4996175b883c6ce7e66d5009ac04619ea5 Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Sun, 16 Jun 2013 21:31:20 +0200 Subject: [PATCH] Use a dynamic frequency estimation interval. A slow servo (with smaller constants and lower sync rate) needs a longer, better frequency estimation, but a higher sync rate hardly needs any estimation at all, since it learns the frequency right away in any case. Signed-off-by: Richard Cochran --- pi.c | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/pi.c b/pi.c index d42bf0b..5f4930e 100644 --- a/pi.c +++ b/pi.c @@ -30,6 +30,7 @@ #define SWTS_KI 0.001 #define NSEC_PER_SEC 1000000000 +#define FREQ_EST_MARGIN 0.001 /* These take their values from the configuration file. (see ptp4l.c) */ double configured_pi_kp = 0.0; @@ -64,6 +65,7 @@ static double pi_sample(struct servo *servo, enum servo_state *state) { double ki_term, ppb = 0.0; + double freq_est_interval, localdiff; struct pi_servo *s = container_of(servo, struct pi_servo, servo); switch (s->count) { @@ -84,6 +86,18 @@ static double pi_sample(struct servo *servo, break; } + /* Wait long enough before estimating the frequency offset. */ + localdiff = (s->local[1] - s->local[0]) / 1e9; + localdiff += localdiff * FREQ_EST_MARGIN; + freq_est_interval = 0.016 / s->ki; + if (freq_est_interval > 1000.0) { + freq_est_interval = 1000.0; + } + if (localdiff < freq_est_interval) { + *state = SERVO_UNLOCKED; + break; + } + s->drift += (s->offset[1] - s->offset[0]) * 1e9 / (s->local[1] - s->local[0]); if (s->drift < -s->maxppb)