From f2aae280b3409d9dfdaf7e8784e71e7dccb78407 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Thu, 3 May 2012 11:17:34 +0200
Subject: [PATCH] Make the PI constants configurable.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 config.c    | 11 +++++++++++
 config.h    |  2 ++
 default.cfg |  2 ++
 pi.c        |  9 ++++++++-
 ptp4l.c     |  3 +++
 5 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/config.c b/config.c
index 9d6000c..6eb2186 100644
--- a/config.c
+++ b/config.c
@@ -22,6 +22,7 @@
 
 static void scan_line(char *s, struct config *cfg)
 {
+	double df;
 	int val;
 	Integer8 i8;
 	UInteger16 u16;
@@ -84,6 +85,16 @@ static void scan_line(char *s, struct config *cfg)
 
 		if (val > 0)
 			*cfg->tx_timestamp_retries = val;
+
+	} else if (1 == sscanf(s, " pi_proportional_const %lf", &df)) {
+
+		if (df > 0.0 && df < 1.0)
+			*cfg->pi_proportional_const = df;
+
+	} else if (1 == sscanf(s, " pi_integral_const %lf", &df)) {
+
+		if (df > 0.0 && df < 1.0)
+			*cfg->pi_integral_const = df;
 	}
 }
 
diff --git a/config.h b/config.h
index 923c324..0e69666 100644
--- a/config.h
+++ b/config.h
@@ -26,6 +26,8 @@ struct config {
 	struct defaultDS *dds;
 	struct port_defaults *pod;
 	int *tx_timestamp_retries;
+	double *pi_proportional_const;
+	double *pi_integral_const;
 };
 
 int config_read(char *name, struct config *cfg);
diff --git a/default.cfg b/default.cfg
index bc6748d..0e353bf 100644
--- a/default.cfg
+++ b/default.cfg
@@ -20,3 +20,5 @@ announceReceiptTimeout	3
 # Run time options
 #
 tx_timestamp_retries	2
+pi_proportional_const	0.0
+pi_integral_const	0.0
diff --git a/pi.c b/pi.c
index 5ef2337..33766b1 100644
--- a/pi.c
+++ b/pi.c
@@ -28,6 +28,10 @@
 #define SWTS_KP 0.1
 #define SWTS_KI 0.001
 
+/* These two take their values from the configuration file. (see ptp4l.c) */
+extern double configured_pi_kp;
+extern double configured_pi_ki;
+
 struct pi_servo {
 	struct servo servo;
 	double offset[2];
@@ -105,7 +109,10 @@ struct servo *pi_servo_create(int max_ppb, int sw_ts)
 	s->servo.sample  = pi_sample;
 	s->maxppb        = max_ppb;
 
-	if (sw_ts) {
+	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 {
diff --git a/ptp4l.c b/ptp4l.c
index bd463d7..cbf0d7e 100644
--- a/ptp4l.c
+++ b/ptp4l.c
@@ -31,6 +31,7 @@
 #define DEFAULT_PHC "/dev/ptp0"
 
 int sk_tx_retries = 2; /*see sk.c*/
+double configured_pi_kp, configured_pi_ki; /*see pi.c*/
 
 static int running = 1;
 static struct defaultDS ds;
@@ -199,6 +200,8 @@ int main(int argc, char *argv[])
 	cfg_settings.dds = &ds;
 	cfg_settings.pod = &pod;
 	cfg_settings.tx_timestamp_retries = &sk_tx_retries;
+	cfg_settings.pi_proportional_const = &configured_pi_kp;
+	cfg_settings.pi_integral_const = &configured_pi_ki;
 
 	if (config && config_read(config, &cfg_settings)) {
 		fprintf(stderr, "failed to read configuration file\n");