Miroslav Lichvar
2013-04-25 13:47:53 UTC
Make the interval between the two samples that are used to estimate the
drift inversely proportional to the integral constant to improve the
accuracy of the estimated drift and to make the initial convergence
faster.
The interval is now longer than 1 with integral constants below 0.01.
Signed-off-by: Miroslav Lichvar <***@redhat.com>
---
pi.c | 30 ++++++++++++++++++++++++------
1 file changed, 24 insertions(+), 6 deletions(-)
diff --git a/pi.c b/pi.c
index 427cb3e..dd54100 100644
--- a/pi.c
+++ b/pi.c
@@ -31,6 +31,9 @@
#define NSEC_PER_SEC 1000000000
+#define DRIFT_EST_KI 0.01
+#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,37 @@ 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;
+ if (drift_est_interval > DRIFT_EST_MAX)
+ drift_est_interval = DRIFT_EST_MAX;
+ if (s->count - 1 < drift_est_interval) {
+ *state = SERVO_UNLOCKED;
break;
}
@@ -89,7 +107,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 +119,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;
}
drift inversely proportional to the integral constant to improve the
accuracy of the estimated drift and to make the initial convergence
faster.
The interval is now longer than 1 with integral constants below 0.01.
Signed-off-by: Miroslav Lichvar <***@redhat.com>
---
pi.c | 30 ++++++++++++++++++++++++------
1 file changed, 24 insertions(+), 6 deletions(-)
diff --git a/pi.c b/pi.c
index 427cb3e..dd54100 100644
--- a/pi.c
+++ b/pi.c
@@ -31,6 +31,9 @@
#define NSEC_PER_SEC 1000000000
+#define DRIFT_EST_KI 0.01
+#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,37 @@ 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;
+ if (drift_est_interval > DRIFT_EST_MAX)
+ drift_est_interval = DRIFT_EST_MAX;
+ if (s->count - 1 < drift_est_interval) {
+ *state = SERVO_UNLOCKED;
break;
}
@@ -89,7 +107,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 +119,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;
}
--
1.8.1.4
1.8.1.4