From b3cb869b54831b64dcf12669628f8ae9dfb87c73 Mon Sep 17 00:00:00 2001 From: Michael Buesch Date: Sun, 29 Jul 2018 22:01:07 +0200 Subject: pid: Add dumping of controller results Signed-off-by: Michael Buesch --- firmware/controller_current.c | 6 +++++- firmware/controller_temp.c | 6 +++++- firmware/pid.c | 31 ++++++++++++++++++++++++++++++- firmware/pid.h | 12 ++++++++++++ 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 + * Copyright (c) 2015-2018 Michael Buesch * * 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 @@ -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); -- cgit v1.2.3