This patch makes the PI controller adapt its PI constants whenever the
sync interval changes. The constants are calculated using the formula
C = A * sync_interval ^ B
where the scaling factor A and exponent B will be user configurable (in
a subsequent patch). Credit for this idea goes to Miroslav Lichvar.
Signed-off-by: Richard Cochran <***@gmail.com>
---
pi.c | 58 ++++++++++++++++++++++++++++++++++++++++++++++++----------
1 file changed, 48 insertions(+), 10 deletions(-)
diff --git a/pi.c b/pi.c
index d5e6359..2048f12 100644
--- a/pi.c
+++ b/pi.c
@@ -23,11 +23,11 @@
#include "pi.h"
#include "servo_private.h"
-#define HWTS_KP 0.7
-#define HWTS_KI 0.3
-
-#define SWTS_KP 0.1
-#define SWTS_KI 0.001
+#define SWTS_KP_SCALE 0.1
+#define SWTS_KI_SCALE 0.001
+#define SWTS_KP_EXPONENT (-0.5)
+#define SWTS_KI_EXPONENT (+0.5)
+#define TT 1.0
#define NSEC_PER_SEC 1000000000
#define FREQ_EST_MARGIN 0.001
@@ -39,6 +39,13 @@ double configured_pi_offset = 0.0;
double configured_pi_f_offset = 0.0000001; /* 100 nanoseconds */
int configured_pi_max_freq = 900000000;
+double kp_scale = .8;
+double ki_scale = .2;
+double kp_exponent = (-0.5);
+double ki_exponent = (+0.5);
+static double kp_slow_const = .7;
+static double ki_slow_const = .38;
+
struct pi_servo {
struct servo servo;
int64_t offset[2];
@@ -53,6 +60,22 @@ struct pi_servo {
int first_update;
};
+static double kp(double t)
+{
+ if (t > TT) {
+ return kp_slow_const / t;
+ }
+ return kp_scale * pow(t, kp_exponent);
+}
+
+static double ki(double t)
+{
+ if (t > TT) {
+ return ki_slow_const / t;
+ }
+ return ki_scale * pow(t, ki_exponent);
+}
+
static void pi_destroy(struct servo *servo)
{
struct pi_servo *s = container_of(servo, struct pi_servo, servo);
@@ -148,6 +171,16 @@ static double pi_sample(struct servo *servo,
static void pi_sync_interval(struct servo *servo, int n)
{
+ double t;
+ struct pi_servo *s = container_of(servo, struct pi_servo, servo);
+
+ if (n < 0) {
+ t = 1.0 / (1 << -n);
+ } else {
+ t = (1 << n);
+ }
+ s->kp = kp(t);
+ s->ki = ki(t);
}
struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
@@ -165,15 +198,20 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
s->maxppb = max_ppb;
s->first_update = 1;
+ if (sw_ts) {
+ kp_scale = SWTS_KP_SCALE;
+ ki_scale = SWTS_KI_SCALE;
+ kp_exponent = SWTS_KP_EXPONENT;
+ ki_exponent = SWTS_KI_EXPONENT;
+ kp_slow_const = SWTS_KP_SCALE;
+ ki_slow_const = SWTS_KI_SCALE;
+ }
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;
+ s->kp = kp(1.0);
+ s->ki = ki(1.0);
}
if (configured_pi_offset > 0.0) {
--
1.7.10.4