From c7c675852ecd1c01e4bb982b81523ac8c5161cf6 Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Fri, 28 Jun 2013 17:09:14 +0200 Subject: [PATCH] Adjust constants of PI servo according to sync interval. Instead of using fixed constants, set them by the following formula from the current sync to allow good performance of the servo even when the sync interval changes in runtime and to avoid instability. kp = min(kp_scale * sync^kp_exponent, kp_norm_max / sync) ki = min(ki_scale * sync^ki_exponent, ki_norm_max / sync) The scale, exponent and norm_max constants are configurable. The defaults are chosen so there is no change to the previous default constants of the servo with one second sync interval. The automatic adjustment can be disabled by setting the pi_proportional_const and pi_integral_const options to a non-zero value, but stability of the servo is always enforced. Signed-off-by: Miroslav Lichvar --- config.c | 36 ++++++++++++++++++++++++++++++++++ config.h | 6 ++++++ default.cfg | 6 ++++++ gPTP.cfg | 6 ++++++ pi.c | 56 +++++++++++++++++++++++++++++++++++++++++------------ pi.h | 44 +++++++++++++++++++++++++++++++++++++++++ ptp4l.8 | 46 +++++++++++++++++++++++++++++++++++++++---- ptp4l.c | 6 ++++++ 8 files changed, 190 insertions(+), 16 deletions(-) diff --git a/config.c b/config.c index 854a703..d914afa 100644 --- a/config.c +++ b/config.c @@ -307,6 +307,42 @@ static enum parser_result parse_global_setting(const char *option, return r; *cfg->pi_integral_const = df; + } else if (!strcmp(option, "pi_proportional_scale")) { + r = get_ranged_double(value, &df, 0.0, DBL_MAX); + if (r != PARSED_OK) + return r; + *cfg->pi_proportional_scale = df; + + } else if (!strcmp(option, "pi_proportional_exponent")) { + r = get_ranged_double(value, &df, -DBL_MAX, DBL_MAX); + if (r != PARSED_OK) + return r; + *cfg->pi_proportional_exponent = df; + + } else if (!strcmp(option, "pi_proportional_norm_max")) { + r = get_ranged_double(value, &df, DBL_MIN, 1.0); + if (r != PARSED_OK) + return r; + *cfg->pi_proportional_norm_max = df; + + } else if (!strcmp(option, "pi_integral_scale")) { + r = get_ranged_double(value, &df, 0.0, DBL_MAX); + if (r != PARSED_OK) + return r; + *cfg->pi_integral_scale = df; + + } else if (!strcmp(option, "pi_integral_exponent")) { + r = get_ranged_double(value, &df, -DBL_MAX, DBL_MAX); + if (r != PARSED_OK) + return r; + *cfg->pi_integral_exponent = df; + + } else if (!strcmp(option, "pi_integral_norm_max")) { + r = get_ranged_double(value, &df, DBL_MIN, 2.0); + if (r != PARSED_OK) + return r; + *cfg->pi_integral_norm_max = df; + } else if (!strcmp(option, "pi_offset_const")) { r = get_ranged_double(value, &df, 0.0, DBL_MAX); if (r != PARSED_OK) diff --git a/config.h b/config.h index c5de3ff..b014d7e 100644 --- a/config.h +++ b/config.h @@ -75,6 +75,12 @@ struct config { double *pi_proportional_const; double *pi_integral_const; + double *pi_proportional_scale; + double *pi_proportional_exponent; + double *pi_proportional_norm_max; + double *pi_integral_scale; + double *pi_integral_exponent; + double *pi_integral_norm_max; double *pi_offset_const; double *pi_f_offset_const; int *pi_max_frequency; diff --git a/default.cfg b/default.cfg index e10d257..c5493d6 100644 --- a/default.cfg +++ b/default.cfg @@ -40,6 +40,12 @@ kernel_leap 1 # pi_proportional_const 0.0 pi_integral_const 0.0 +pi_proportional_scale 0.0 +pi_proportional_exponent -0.3 +pi_proportional_norm_max 0.7 +pi_integral_scale 0.0 +pi_integral_exponent 0.4 +pi_integral_norm_max 0.3 pi_offset_const 0.0 pi_f_offset_const 0.0000001 pi_max_frequency 900000000 diff --git a/gPTP.cfg b/gPTP.cfg index c4e0fab..00215b4 100644 --- a/gPTP.cfg +++ b/gPTP.cfg @@ -39,6 +39,12 @@ kernel_leap 1 # pi_proportional_const 0.0 pi_integral_const 0.0 +pi_proportional_scale 0.0 +pi_proportional_exponent -0.3 +pi_proportional_norm_max 0.7 +pi_integral_scale 0.0 +pi_integral_exponent 0.4 +pi_integral_norm_max 0.3 pi_offset_const 0.0 pi_f_offset_const 0.0000001 pi_max_frequency 900000000 diff --git a/pi.c b/pi.c index 5f4930e..3c8a635 100644 --- a/pi.c +++ b/pi.c @@ -21,13 +21,16 @@ #include #include "pi.h" +#include "print.h" #include "servo_private.h" -#define HWTS_KP 0.7 -#define HWTS_KI 0.3 +#define HWTS_KP_SCALE 0.7 +#define HWTS_KI_SCALE 0.3 +#define SWTS_KP_SCALE 0.1 +#define SWTS_KI_SCALE 0.001 -#define SWTS_KP 0.1 -#define SWTS_KI 0.001 +#define MAX_KP_NORM_MAX 1.0 +#define MAX_KI_NORM_MAX 2.0 #define NSEC_PER_SEC 1000000000 #define FREQ_EST_MARGIN 0.001 @@ -35,6 +38,12 @@ /* These take their values from the configuration file. (see ptp4l.c) */ double configured_pi_kp = 0.0; double configured_pi_ki = 0.0; +double configured_pi_kp_scale = 0.0; +double configured_pi_kp_exponent = -0.3; +double configured_pi_kp_norm_max = 0.7; +double configured_pi_ki_scale = 0.0; +double configured_pi_ki_exponent = 0.4; +double configured_pi_ki_norm_max = 0.3; double configured_pi_offset = 0.0; double configured_pi_f_offset = 0.0000001; /* 100 nanoseconds */ int configured_pi_max_freq = 900000000; @@ -148,6 +157,18 @@ static double pi_sample(struct servo *servo, static void pi_sync_interval(struct servo *servo, double interval) { + struct pi_servo *s = container_of(servo, struct pi_servo, servo); + + s->kp = configured_pi_kp_scale * pow(interval, configured_pi_kp_exponent); + if (s->kp > configured_pi_kp_norm_max / interval) + s->kp = configured_pi_kp_norm_max / interval; + + s->ki = configured_pi_ki_scale * pow(interval, configured_pi_ki_exponent); + if (s->ki > configured_pi_ki_norm_max / interval) + s->ki = configured_pi_ki_norm_max / interval; + + pr_debug("PI servo: sync interval %.3f kp %.3f ki %.6f", + interval, s->kp, s->ki); } struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts) @@ -164,16 +185,27 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts) s->drift = fadj; s->maxppb = max_ppb; s->first_update = 1; + s->kp = 0.0; + s->ki = 0.0; if (configured_pi_kp && configured_pi_ki) { - s->kp = configured_pi_kp; - s->ki = configured_pi_ki; - } else if (sw_ts) { - s->kp = SWTS_KP; - s->ki = SWTS_KI; - } else { - s->kp = HWTS_KP; - s->ki = HWTS_KI; + /* Use the constants as configured by the user without + adjusting for sync interval unless they make the servo + unstable. */ + configured_pi_kp_scale = configured_pi_kp; + configured_pi_ki_scale = configured_pi_ki; + configured_pi_kp_exponent = 0.0; + configured_pi_ki_exponent = 0.0; + configured_pi_kp_norm_max = MAX_KP_NORM_MAX; + configured_pi_ki_norm_max = MAX_KI_NORM_MAX; + } else if (!configured_pi_kp_scale || !configured_pi_ki_scale) { + if (sw_ts) { + configured_pi_kp_scale = SWTS_KP_SCALE; + configured_pi_ki_scale = SWTS_KI_SCALE; + } else { + configured_pi_kp_scale = HWTS_KP_SCALE; + configured_pi_ki_scale = HWTS_KI_SCALE; + } } if (configured_pi_offset > 0.0) { diff --git a/pi.h b/pi.h index 2f31bce..0d8994b 100644 --- a/pi.h +++ b/pi.h @@ -33,6 +33,50 @@ extern double configured_pi_kp; */ extern double configured_pi_ki; +/** + * When set to a non-zero value, this variable determines the scale in the + * formula used to set the proportional constant of the PI controller from the + * sync interval. + * kp = min(kp_scale * sync^kp_exponent, kp_norm_max / sync) + */ +extern double configured_pi_kp_scale; + +/** + * This variable determines the exponent in the formula used to set the + * proportional constant of the PI controller from the sync interval. + * kp = min(kp_scale * sync^kp_exponent, kp_norm_max / sync) + */ +extern double configured_pi_kp_exponent; + +/** + * This variable determines the normalized maximum in the formula used to set + * the proportional constant of the PI controller from the sync interval. + * kp = min(kp_scale * sync^kp_exponent, kp_norm_max / sync) + */ +extern double configured_pi_kp_norm_max; + +/** + * When set to a non-zero value, this variable determines the scale in the + * formula used to set the integral constant of the PI controller from the + * sync interval. + * ki = min(ki_scale * sync^ki_exponent, ki_norm_max / sync) + */ +extern double configured_pi_ki_scale; + +/** + * This variable determines the exponent in the formula used to set the + * integral constant of the PI controller from the sync interval. + * ki = min(ki_scale * sync^ki_exponent, ki_norm_max / sync) + */ +extern double configured_pi_ki_exponent; + +/** + * This variable determines the normalized maximum in the formula used to set + * the integral constant of the PI controller from the sync interval. + * ki = min(ki_scale * sync^ki_exponent, ki_norm_max / sync) + */ +extern double configured_pi_ki_norm_max; + /** * When set to a non-zero value, this variable controls the maximum allowed * offset before a clock jump occurs instead of the default clock-slewing diff --git a/ptp4l.8 b/ptp4l.8 index 5a9e8b4..6f42c1c 100644 --- a/ptp4l.8 +++ b/ptp4l.8 @@ -254,17 +254,55 @@ servo is implemented, a PI controller. The default is pi. .TP .B pi_proportional_const -The proportional constant of the PI controller. When set to 0.0, the value will -be selected from 0.7 and 0.1 for the hardware and software time stamping +The proportional constant of the PI controller. When set to 0.0, the +proportional constant will be set by the following formula from the current +sync interval. +The default is 0.0. + +kp = min(kp_scale * sync^kp_exponent, kp_norm_max / sync)) +.TP +.B pi_integral_const +The integral constant of the PI controller. When set to 0.0, the +integral constant will be set by the following formula from the current +sync interval. +The default is 0.0. + +ki = min(ki_scale * sync^ki_exponent, ki_norm_max / sync) +.TP +.B pi_proportional_scale +The kp_scale constant in the formula used to set the proportional constant of +the PI controller from the sync interval. When set to 0.0, the value will be +selected from 0.7 and 0.1 for the hardware and software time stamping respectively. The default is 0.0. .TP -.B pi_integral_const -The integral constant of the PI controller. When set to 0.0, the value will be +.B pi_proportional_exponent +The kp_exponent constant in the formula used to set the proportional constant of +the PI controller from the sync interval. +The default is -0.3. +.TP +.B pi_proportional_norm_max +The kp_norm_max constant in the formula used to set the proportional constant of +the PI controller from the sync interval. +The default is 0.7 +.TP +.B pi_integral_scale +The ki_scale constant in the formula used to set the integral constant of +the PI controller from the sync interval. When set to 0.0, the value will be selected from 0.3 and 0.001 for the hardware and software time stamping respectively. The default is 0.0. .TP +.B pi_integral_exponent +The ki_exponent constant in the formula used to set the integral constant of +the PI controller from the sync interval. +The default is 0.4. +.TP +.B pi_integral_norm_max +The ki_norm_max constant in the formula used to set the integral constant of +the PI controller from the sync interval. +The default is 0.3. +.TP .B pi_offset_const The maximum offset the PI controller will correct by changing the clock frequency instead of stepping the clock. When set to 0.0, the controller will diff --git a/ptp4l.c b/ptp4l.c index 632166c..1d4a3e0 100644 --- a/ptp4l.c +++ b/ptp4l.c @@ -96,6 +96,12 @@ static struct config cfg_settings = { .pi_proportional_const = &configured_pi_kp, .pi_integral_const = &configured_pi_ki, + .pi_proportional_scale = &configured_pi_kp_scale, + .pi_proportional_exponent = &configured_pi_kp_exponent, + .pi_proportional_norm_max = &configured_pi_kp_norm_max, + .pi_integral_scale = &configured_pi_ki_scale, + .pi_integral_exponent = &configured_pi_ki_exponent, + .pi_integral_norm_max = &configured_pi_ki_norm_max, .pi_offset_const = &configured_pi_offset, .pi_f_offset_const = &configured_pi_f_offset, .pi_max_frequency = &configured_pi_max_freq,