aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorMichael Buesch <m@bues.ch>2018-07-29 22:01:07 +0200
committerMichael Buesch <m@bues.ch>2018-07-29 22:01:07 +0200
commitb3cb869b54831b64dcf12669628f8ae9dfb87c73 (patch)
tree43730319b77d001b9547e0c938c40ed6a42947f8
parentaa1d0464905cfd0ade606c9846b980be034338c5 (diff)
downloadxytronic-lf-b3cb869b54831b64dcf12669628f8ae9dfb87c73.zip
xytronic-lf-b3cb869b54831b64dcf12669628f8ae9dfb87c73.tar.xz
pid: Add dumping of controller results
Signed-off-by: Michael Buesch <m@bues.ch>
-rw-r--r--firmware/controller_current.c6
-rw-r--r--firmware/controller_temp.c6
-rw-r--r--firmware/pid.c31
-rw-r--r--firmware/pid.h12
4 files changed, 52 insertions, 3 deletions
diff --git a/firmware/controller_current.c b/firmware/controller_current.c
index 8b84bdf..d8f0476 100644
--- a/firmware/controller_current.c
+++ b/firmware/controller_current.c
@@ -229,7 +229,11 @@ void contrcurr_init(void)
struct pid_k_set k_set;
k_set = contrcurr_factors;
- pid_init(&contrcurr.pid, &k_set,
+ pid_init(&contrcurr.pid,
+#if CONF_DEBUG
+ PSTR("pid-c"),
+#endif
+ &k_set,
float_to_fixpt(CONTRCURR_NEGLIM_I),
float_to_fixpt(CONTRCURR_POSLIM_I),
float_to_fixpt(CONTRCURR_NEGLIM),
diff --git a/firmware/controller_temp.c b/firmware/controller_temp.c
index 9ba0b28..f18abcc 100644
--- a/firmware/controller_temp.c
+++ b/firmware/controller_temp.c
@@ -323,7 +323,11 @@ void contrtemp_init(void)
settings = get_settings();
k_set = &settings->temp_k[TEMPBOOST_NORMAL];
- pid_init(&contrtemp.pid, k_set,
+ pid_init(&contrtemp.pid,
+#if CONF_DEBUG
+ PSTR("pid-t"),
+#endif
+ k_set,
float_to_fixpt(CONTRTEMP_NEGLIM_I),
float_to_fixpt(CONTRTEMP_POSLIM_I),
float_to_fixpt(CONTRTEMP_NEGLIM),
diff --git a/firmware/pid.c b/firmware/pid.c
index 448ca0c..9aa8e9a 100644
--- a/firmware/pid.c
+++ b/firmware/pid.c
@@ -1,7 +1,7 @@
/*
* PID controller
*
- * Copyright (c) 2015-2017 Michael Buesch <m@bues.ch>
+ * Copyright (c) 2015-2018 Michael Buesch <m@bues.ch>
*
* 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
@@ -20,6 +20,7 @@
#include "pid.h"
#include "util.h"
+#include "debug_uart.h"
#include <string.h>
@@ -46,11 +47,21 @@ fixpt_t pid_run(struct pid *pid, fixpt_t dt, fixpt_t r)
/* Calculate the deviation. */
e = fixpt_sub(pid->setpoint, r);
+#if CONF_DEBUG
+ debug_report_fixpt(DEBUG_PREFIX2(pid->name, PSTR("e")),
+ &pid->debug_old_e, e);
+#endif
+
/* P term */
kp = pid->k.kp;
p = fixpt_mul(kp, e);
pid_result = p;
+#if CONF_DEBUG
+ debug_report_fixpt(DEBUG_PREFIX2(pid->name, PSTR("p")),
+ &pid->debug_old_p, p);
+#endif
+
/* I term */
ki = pid->k.ki;
i = fixpt_add(pid->integr, fixpt_mul(fixpt_mul(ki, e), dt));
@@ -58,6 +69,11 @@ fixpt_t pid_run(struct pid *pid, fixpt_t dt, fixpt_t r)
pid->integr = i;
pid_result = fixpt_add(pid_result, i);
+#if CONF_DEBUG
+ debug_report_fixpt(DEBUG_PREFIX2(pid->name, PSTR("i")),
+ &pid->debug_old_i, i);
+#endif
+
/* D term */
kd = pid->k.kd;
de = fixpt_sub(e, pid->prev_e);
@@ -65,16 +81,29 @@ fixpt_t pid_run(struct pid *pid, fixpt_t dt, fixpt_t r)
pid->prev_e = fixpt_div(e, pid->k.d_decay_div);
pid_result = fixpt_add(pid_result, d);
+#if CONF_DEBUG
+ debug_report_fixpt(DEBUG_PREFIX2(pid->name, PSTR("d")),
+ &pid->debug_old_d, d);
+ debug_report_fixpt(DEBUG_PREFIX2(pid->name, PSTR("pe")),
+ &pid->debug_old_pe, pid->prev_e);
+#endif
+
pid_result = clamp(pid_result, pid->y_neglim, pid->y_poslim);
return pid_result;
}
void pid_init(struct pid *pid,
+#if CONF_DEBUG
+ const char __flash *name,
+#endif
const struct pid_k_set *k,
fixpt_t i_neglim, fixpt_t i_poslim,
fixpt_t y_neglim, fixpt_t y_poslim)
{
+#if CONF_DEBUG
+ pid->name = name;
+#endif
pid->i_neglim = i_neglim;
pid->i_poslim = i_poslim;
pid->y_neglim = y_neglim;
diff --git a/firmware/pid.h b/firmware/pid.h
index 907d244..80e7a5b 100644
--- a/firmware/pid.h
+++ b/firmware/pid.h
@@ -24,6 +24,15 @@ struct pid {
fixpt_t prev_e;
fixpt_t integr;
+
+#if CONF_DEBUG
+ const char __flash *name;
+ fixpt_t debug_old_e;
+ fixpt_t debug_old_p;
+ fixpt_t debug_old_i;
+ fixpt_t debug_old_d;
+ fixpt_t debug_old_pe;
+#endif
};
void pid_reset(struct pid *pid);
@@ -31,6 +40,9 @@ void pid_reset(struct pid *pid);
void pid_set_factors(struct pid *pid, const struct pid_k_set *k);
void pid_init(struct pid *pid,
+#if CONF_DEBUG
+ const char __flash *name,
+#endif
const struct pid_k_set *k,
fixpt_t i_neglim, fixpt_t i_poslim,
fixpt_t y_neglim, fixpt_t y_poslim);
bues.ch cgit interface