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
parent
3695137619
commit
6edbea4996
14
pi.c
14
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)
|
||||
|
|
Loading…
Reference in New Issue