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 <[email protected]>

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)

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;

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

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

pi.c

@@ -21,20 +21,29 @@
 #include <math.h>
 
 #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
 
 /* 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) {

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

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
-respectively.
+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 value will be
+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_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

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,