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 <richardcochran@gmail.com>
master
Richard Cochran 2013-06-16 21:31:20 +02:00
parent 3695137619
commit 6edbea4996
1 changed files with 14 additions and 0 deletions

14
pi.c
View File

@ -30,6 +30,7 @@
#define SWTS_KI 0.001 #define SWTS_KI 0.001
#define NSEC_PER_SEC 1000000000 #define NSEC_PER_SEC 1000000000
#define FREQ_EST_MARGIN 0.001
/* These take their values from the configuration file. (see ptp4l.c) */ /* These take their values from the configuration file. (see ptp4l.c) */
double configured_pi_kp = 0.0; double configured_pi_kp = 0.0;
@ -64,6 +65,7 @@ static double pi_sample(struct servo *servo,
enum servo_state *state) enum servo_state *state)
{ {
double ki_term, ppb = 0.0; double ki_term, ppb = 0.0;
double freq_est_interval, localdiff;
struct pi_servo *s = container_of(servo, struct pi_servo, servo); struct pi_servo *s = container_of(servo, struct pi_servo, servo);
switch (s->count) { switch (s->count) {
@ -84,6 +86,18 @@ static double pi_sample(struct servo *servo,
break; 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->drift += (s->offset[1] - s->offset[0]) * 1e9 /
(s->local[1] - s->local[0]); (s->local[1] - s->local[0]);
if (s->drift < -s->maxppb) if (s->drift < -s->maxppb)