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 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)
|
||||||
|
|
Loading…
Reference in New Issue