This servo uses linear regression to estimate current time and
frequency error. The number of points used in the regression is
variable (from 4 to 64 in powers of 2) and is selected by a long-term
statistic of the prediction error.
Future improvements could include tracking of sudden frequency changes
(e.g. due to temperature variations), better stability of the error
statistic when a large offset is corrected, options to set the speed of
the adaptation, minimum and maximum number of points, or an option to
prefer frequency accuracy over time accuracy.
Signed-off-by: Miroslav Lichvar <***@redhat.com>
---
config.c | 2 +
linreg.c | 312 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
linreg.h | 26 ++++++
makefile | 8 +-
phc2sys.8 | 7 ++
phc2sys.c | 17 +++-
ptp4l.8 | 4 +-
servo.c | 4 +
servo.h | 1 +
9 files changed, 373 insertions(+), 8 deletions(-)
create mode 100644 linreg.c
create mode 100644 linreg.h
diff --git a/config.c b/config.c
index 7fdc1d8..567256c 100644
--- a/config.c
+++ b/config.c
@@ -497,6 +497,8 @@ static enum parser_result parse_global_setting(const char *option,
} else if (!strcmp(option, "clock_servo")) {
if (!strcasecmp("pi", value))
cfg->clock_servo = CLOCK_SERVO_PI;
+ else if (!strcasecmp("linreg", value))
+ cfg->clock_servo = CLOCK_SERVO_LINREG;
else
return BAD_VALUE;
diff --git a/linreg.c b/linreg.c
new file mode 100644
index 0000000..add81e4
--- /dev/null
+++ b/linreg.c
@@ -0,0 +1,312 @@
+/**
+ * @file linreg.c
+ * @brief Implements an adaptive servo based on linear regression.
+ * @note Copyright (C) 2014 Miroslav Lichvar <***@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include <stdlib.h>
+#include <math.h>
+
+#include "linreg.h"
+#include "print.h"
+#include "servo_private.h"
+
+/* Maximum and minimum number of points used in regression,
+ defined as a power of 2 */
+#define MAX_SIZE 6
+#define MIN_SIZE 2
+
+#define MAX_POINTS (1 << MAX_SIZE)
+
+/* Smoothing factor used for long-term prediction error */
+#define ERR_SMOOTH 0.02
+/* Number of updates used for initialization */
+#define ERR_INITIAL_UPDATES 10
+/* Maximum ratio of two err values to be considered equal */
+#define ERR_EQUALS 1.05
+
+/* Uncorrected local time vs remote time */
+struct point {
+ uint64_t x;
+ uint64_t y;
+};
+
+struct result {
+ /* Slope and intercept from latest regression */
+ double slope;
+ double intercept;
+ /* Exponential moving average of prediction error */
+ double err;
+ /* Number of initial err updates */
+ int err_updates;
+};
+
+struct linreg_servo {
+ struct servo servo;
+ /* Circular buffer of points */
+ struct point points[MAX_POINTS];
+ /* Current time in x, y */
+ struct point reference;
+ /* Number of stored points */
+ unsigned int num_points;
+ /* Index of the newest point */
+ unsigned int last_point;
+ /* Remainder from last update of reference.x */
+ double x_remainder;
+ /* Local time stamp of last update */
+ uint64_t last_update;
+ /* Regression results for all sizes */
+ struct result results[MAX_SIZE - MIN_SIZE + 1];
+ /* Current frequency offset of the clock */
+ double clock_freq;
+ /* Expected interval between updates */
+ double update_interval;
+};
+
+static void linreg_destroy(struct servo *servo)
+{
+ struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+ free(s);
+}
+
+static void move_reference(struct linreg_servo *s, int64_t x, int64_t y)
+{
+ struct result *res;
+ unsigned int i;
+
+ s->reference.x += x;
+ s->reference.y += y;
+
+ /* Update intercepts for new reference */
+ for (i = MIN_SIZE; i <= MAX_SIZE; i++) {
+ res = &s->results[i - MIN_SIZE];
+ res->intercept += x * res->slope - y;
+ }
+}
+
+static void update_reference(struct linreg_servo *s, uint64_t local_ts)
+{
+ double x_interval;
+ int64_t y_interval;
+
+ if (s->last_update) {
+ y_interval = local_ts - s->last_update;
+
+ /* Remove current frequency correction from the interval */
+ x_interval = y_interval / (1.0 + s->clock_freq / 1e9);
+ x_interval += s->x_remainder;
+ s->x_remainder = x_interval - (int64_t)x_interval;
+
+ move_reference(s, (int64_t)x_interval, y_interval);
+ }
+
+ s->last_update = local_ts;
+}
+
+static void add_sample(struct linreg_servo *s, int64_t offset)
+{
+ s->last_point = (s->last_point + 1) % MAX_POINTS;
+
+ s->points[s->last_point].x = s->reference.x;
+ s->points[s->last_point].y = s->reference.y - offset;
+
+ if (s->num_points < MAX_POINTS)
+ s->num_points++;
+}
+
+static void regress(struct linreg_servo *s)
+{
+ double x, y, y0, e, x_sum, y_sum, xy_sum, x2_sum;
+ unsigned int i, l, n, size;
+ struct result *res;
+
+ x_sum = 0.0, y_sum = 0.0, xy_sum = 0.0, x2_sum = 0.0;
+ i = 0;
+
+ y0 = (int64_t)(s->points[s->last_point].y - s->reference.y);
+
+ for (size = MIN_SIZE; size <= MAX_SIZE; size++) {
+ n = 1 << size;
+ if (n > s->num_points)
+ /* Not enough points for this size */
+ break;
+
+ res = &s->results[size - MIN_SIZE];
+
+ /* Update moving average of the prediction error */
+ if (res->slope) {
+ e = fabs(res->intercept - y0);
+ if (res->err_updates < ERR_INITIAL_UPDATES) {
+ res->err *= res->err_updates;
+ res->err += e;
+ res->err_updates++;
+ res->err /= res->err_updates;
+ } else {
+ res->err += ERR_SMOOTH * (e - res->err);
+ }
+ }
+
+ for (; i < n; i++) {
+ /* Iterate points from newest to oldest */
+ l = (MAX_POINTS + s->last_point - i) % MAX_POINTS;
+
+ x = (int64_t)(s->points[l].x - s->reference.x);
+ y = (int64_t)(s->points[l].y - s->reference.y);
+
+ x_sum += x;
+ y_sum += y;
+ xy_sum += x * y;
+ x2_sum += x * x;
+ }
+
+ /* Get new intercept and slope */
+ res->slope = (xy_sum - x_sum * y_sum / n) /
+ (x2_sum - x_sum * x_sum / n);
+ res->intercept = (y_sum - res->slope * x_sum) / n;
+ }
+}
+
+/* Return largest size with smallest prediction error */
+static int get_best_size(struct linreg_servo *s)
+{
+ struct result *res;
+ double best_err;
+ int size, best_size;
+
+ best_size = 0;
+ best_err = 0.0;
+
+ for (size = MIN_SIZE; size <= MAX_SIZE; size++) {
+ res = &s->results[size - MIN_SIZE];
+ if ((!best_size && res->slope) ||
+ (best_err * ERR_EQUALS > res->err &&
+ res->err_updates >= ERR_INITIAL_UPDATES)) {
+ best_size = size;
+ best_err = res->err;
+ }
+ }
+
+ return best_size;
+}
+
+static double linreg_sample(struct servo *servo,
+ int64_t offset,
+ uint64_t local_ts,
+ enum servo_state *state)
+{
+ struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+ struct result *res;
+ int size, corr_interval;
+
+ /*
+ * The current time and the time when will be the frequency of the
+ * clock actually updated is assumed here to be equal to local_ts
+ * (which is the time stamp of the received sync message). As long as
+ * the differences are smaller than the update interval, the loop
+ * should be robust enough to handle this simplification.
+ */
+
+ update_reference(s, local_ts);
+ add_sample(s, offset);
+ regress(s);
+
+ size = get_best_size(s);
+
+ if (size < MIN_SIZE) {
+ /* Not enough points, wait for more */
+ *state = SERVO_UNLOCKED;
+ return -s->clock_freq;
+ }
+
+ res = &s->results[size - MIN_SIZE];
+
+ pr_debug("linreg: points %d slope %.9f intercept %.0f err %.0f",
+ 1 << size, res->slope, res->intercept, res->err);
+
+ if ((servo->first_update &&
+ servo->first_step_threshold &&
+ servo->first_step_threshold < fabs(res->intercept)) ||
+ (servo->step_threshold &&
+ servo->step_threshold < fabs(res->intercept))) {
+ /* The clock will be stepped by offset */
+ move_reference(s, 0, -offset);
+ s->last_update -= offset;
+ *state = SERVO_JUMP;
+ } else {
+ *state = SERVO_LOCKED;
+ }
+
+ /* Set clock frequency to the slope */
+ s->clock_freq = 1e9 * (res->slope - 1.0);
+
+ /*
+ * Adjust the frequency to correct the time offset. Use longer
+ * correction interval with larger sizes to reduce the frequency error.
+ * The update interval is assumed to be not affected by the frequency
+ * adjustment. If it is (e.g. phc2sys controlling the system clock), a
+ * correction slowing down the clock will result in an overshoot. With
+ * the system clock's maximum adjustment of 10% that's acceptable.
+ */
+ corr_interval = size <= 4 ? 1 : size / 2;
+ s->clock_freq += res->intercept / s->update_interval / corr_interval;
+
+ /* Clamp the frequency to the allowed maximum */
+ if (s->clock_freq > servo->max_frequency)
+ s->clock_freq = servo->max_frequency;
+ else if (s->clock_freq < -servo->max_frequency)
+ s->clock_freq = -servo->max_frequency;
+
+ return -s->clock_freq;
+}
+
+static void linreg_sync_interval(struct servo *servo, double interval)
+{
+ struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+
+ s->update_interval = interval;
+}
+
+static void linreg_reset(struct servo *servo)
+{
+ struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+ unsigned int i;
+
+ s->num_points = 0;
+ s->last_update = 0;
+
+ for (i = MIN_SIZE; i < MAX_SIZE; i++) {
+ s->results[i - MIN_SIZE].slope = 0.0;
+ s->results[i - MIN_SIZE].err_updates = 0;
+ }
+}
+
+struct servo *linreg_servo_create(int fadj)
+{
+ struct linreg_servo *s;
+
+ s = calloc(1, sizeof(*s));
+ if (!s)
+ return NULL;
+
+ s->servo.destroy = linreg_destroy;
+ s->servo.sample = linreg_sample;
+ s->servo.sync_interval = linreg_sync_interval;
+ s->servo.reset = linreg_reset;
+
+ s->clock_freq = -fadj;
+
+ return &s->servo;
+}
diff --git a/linreg.h b/linreg.h
new file mode 100644
index 0000000..5c86ea7
--- /dev/null
+++ b/linreg.h
@@ -0,0 +1,26 @@
+/**
+ * @file linreg.h
+ * @note Copyright (C) 2014 Miroslav Lichvar <***@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#ifndef HAVE_LINREG_H
+#define HAVE_LINREG_H
+
+#include "servo.h"
+
+struct servo *linreg_servo_create(int fadj);
+
+#endif
diff --git a/makefile b/makefile
index 22e7d0d..eff5109 100644
--- a/makefile
+++ b/makefile
@@ -24,7 +24,7 @@ CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS)
LDLIBS = -lm -lrt $(EXTRA_LDFLAGS)
PRG = ptp4l pmc phc2sys hwstamp_ctl
OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \
- filter.o fsm.o mave.o mmedian.o msg.o phc.o pi.o port.o print.o ptp4l.o raw.o \
+ filter.o fsm.o linreg.o mave.o mmedian.o msg.o phc.o pi.o port.o print.o ptp4l.o raw.o \
servo.o sk.o stats.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o
OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o pmc.o pmc_common.o sysoff.o
@@ -47,9 +47,9 @@ ptp4l: $(OBJ)
pmc: msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o transport.o udp.o \
udp6.o uds.o util.o version.o
-phc2sys: clockadj.o clockcheck.o msg.o phc.o phc2sys.o pi.o pmc_common.o \
- print.o raw.o servo.o sk.o stats.o sysoff.o tlv.o transport.o udp.o udp6.o \
- uds.o util.o version.o
+phc2sys: clockadj.o clockcheck.o linreg.o msg.o phc.o phc2sys.o pi.o \
+ pmc_common.o print.o raw.o servo.o sk.o stats.o sysoff.o tlv.o \
+ transport.o udp.o udp6.o uds.o util.o version.o
hwstamp_ctl: hwstamp_ctl.o version.o
diff --git a/phc2sys.8 b/phc2sys.8
index 812b69e..8688e48 100644
--- a/phc2sys.8
+++ b/phc2sys.8
@@ -15,6 +15,8 @@ phc2sys \- synchronize two clocks
] [
.BI \-O " offset"
] [
+.BI \-E " servo"
+] [
.BI \-P " kp"
] [
.BI \-I " ki"
@@ -89,6 +91,11 @@ should no longer be used.
Specify the slave clock by device (e.g. /dev/ptp1) or interface (e.g. eth1) or
by name. The default is CLOCK_REALTIME (the system clock).
.TP
+.BI \-E " servo"
+Specify which clock servo should be used. Valid values are pi for a PI
+controller and linreg for an adaptive controller using linear regression.
+The default is pi.
+.TP
.BI \-P " kp"
Specify the proportional constant of the PI controller. The default is 0.7.
.TP
diff --git a/phc2sys.c b/phc2sys.c
index d1bfd2e..6221515 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -561,6 +561,7 @@ static void usage(char *progname)
" -c [dev|name] slave clock (CLOCK_REALTIME)\n"
" -d [dev] master PPS device\n"
" -s [dev|name] master clock\n"
+ " -E [pi|linreg] clock servo (pi)\n"
" -P [kp] proportional constant (0.7)\n"
" -I [ki] integration constant (0.3)\n"
" -S [step] step threshold (disabled)\n"
@@ -590,6 +591,7 @@ int main(int argc, char *argv[])
int max_ppb, r, wait_sync = 0, forced_sync_offset = 0;
int print_level = LOG_INFO, use_syslog = 1, verbose = 0;
int sanity_freq_limit = 200000000;
+ enum servo_type servo = CLOCK_SERVO_PI;
double ppb, phc_interval = 1.0, phc_rate;
struct timespec phc_interval_tp;
struct clock dst_clock = {
@@ -605,7 +607,7 @@ int main(int argc, char *argv[])
progname = strrchr(argv[0], '/');
progname = progname ? 1+progname : argv[0];
while (EOF != (c = getopt(argc, argv,
- "c:d:s:P:I:S:F:R:N:O:L:i:u:wn:xl:mqvh"))) {
+ "c:d:s:E:P:I:S:F:R:N:O:L:i:u:wn:xl:mqvh"))) {
switch (c) {
case 'c':
dst_clock.clkid = clock_open(optarg);
@@ -624,6 +626,17 @@ int main(int argc, char *argv[])
case 's':
src = clock_open(optarg);
break;
+ case 'E':
+ if (!strcasecmp(optarg, "pi")) {
+ servo = CLOCK_SERVO_PI;
+ } else if (!strcasecmp(optarg, "linreg")) {
+ servo = CLOCK_SERVO_LINREG;
+ } else {
+ fprintf(stderr,
+ "invalid servo name %s\n", optarg);
+ return -1;
+ }
+ break;
case 'P':
if (get_arg_val_d(c, optarg, &configured_pi_kp,
0.0, DBL_MAX))
@@ -795,7 +808,7 @@ int main(int argc, char *argv[])
}
}
- dst_clock.servo = servo_create(CLOCK_SERVO_PI, -ppb, max_ppb, 0);
+ dst_clock.servo = servo_create(servo, -ppb, max_ppb, 0);
if (pps_fd >= 0) {
servo_sync_interval(dst_clock.servo, 1.0);
diff --git a/ptp4l.8 b/ptp4l.8
index 226123f..edc7045 100644
--- a/ptp4l.8
+++ b/ptp4l.8
@@ -288,8 +288,8 @@ generated by the master.
The default is 0 (disabled).
.TP
.B clock_servo
-The servo which is used to synchronize the local clock. Currently only one
-servo is implemented, a PI controller.
+The servo which is used to synchronize the local clock. Valid values are pi for
+a PI controller and linreg for an adaptive controller using linear regression.
The default is pi.
.TP
.B pi_proportional_const
diff --git a/servo.c b/servo.c
index dd99d30..5b8b0ed 100644
--- a/servo.c
+++ b/servo.c
@@ -18,6 +18,7 @@
*/
#include <string.h>
+#include "linreg.h"
#include "pi.h"
#include "servo_private.h"
@@ -35,6 +36,9 @@ struct servo *servo_create(enum servo_type type, int fadj, int max_ppb, int sw_t
case CLOCK_SERVO_PI:
servo = pi_servo_create(fadj, sw_ts);
break;
+ case CLOCK_SERVO_LINREG:
+ servo = linreg_servo_create(fadj);
+ break;
default:
return NULL;
}
diff --git a/servo.h b/servo.h
index 2439966..7346167 100644
--- a/servo.h
+++ b/servo.h
@@ -54,6 +54,7 @@ struct servo;
*/
enum servo_type {
CLOCK_SERVO_PI,
+ CLOCK_SERVO_LINREG,
};
/**
--
1.8.4.2
