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 + * + * 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 +#include + +#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 + * + * 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 +#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, }; /**