From 5200b36ae44a814963bba0d68c4b76364dc98847 Mon Sep 17 00:00:00 2001
From: MarkMan0 <38912829+MarkMan0@users.noreply.github.com>
Date: Sun, 18 Dec 2022 06:48:33 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix,=20Refactor=20PID=20scaling?=
=?UTF-8?q?=20(#25096)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Marlin/src/module/settings.cpp | 4 +-
Marlin/src/module/temperature.cpp | 59 +++----
Marlin/src/module/temperature.h | 251 ++++++++++++++++++++++--------
buildroot/tests/LPC1768 | 7 +
4 files changed, 211 insertions(+), 110 deletions(-)
diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp
index 4ae4c19922d..aa0cfe52f76 100644
--- a/Marlin/src/module/settings.cpp
+++ b/Marlin/src/module/settings.cpp
@@ -1095,7 +1095,7 @@ void MarlinSettings::postprocess() {
{
_FIELD_TEST(bedPID);
#if ENABLED(PIDTEMPBED)
- const PID_t &pid = thermalManager.temp_bed.pid;
+ const auto &pid = thermalManager.temp_bed.pid;
const raw_pid_t bed_pid = { pid.p(), pid.i(), pid.d() };
#else
const raw_pid_t bed_pid = { NAN, NAN, NAN };
@@ -1109,7 +1109,7 @@ void MarlinSettings::postprocess() {
{
_FIELD_TEST(chamberPID);
#if ENABLED(PIDTEMPCHAMBER)
- const PID_t &pid = thermalManager.temp_chamber.pid;
+ const auto &pid = thermalManager.temp_chamber.pid;
const raw_pid_t chamber_pid = { pid.p(), pid.i(), pid.d() };
#else
const raw_pid_t chamber_pid = { NAN, NAN, NAN };
diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp
index fb3953841b7..b88f19a86bc 100644
--- a/Marlin/src/module/temperature.cpp
+++ b/Marlin/src/module/temperature.cpp
@@ -534,10 +534,6 @@ PGMSTR(str_t_heating_failed, STR_T_HEATING_FAILED);
volatile bool Temperature::raw_temps_ready = false;
-#if ENABLED(PID_EXTRUSION_SCALING)
- int32_t Temperature::pes_e_position, Temperature::lpq[LPQ_MAX_LEN];
- lpq_ptr_t Temperature::lpq_ptr = 0;
-#endif
#if ENABLED(MPCTEMP)
int32_t Temperature::mpc_e_position; // = 0
@@ -1338,50 +1334,33 @@ void Temperature::min_temp_error(const heater_id_t heater_id) {
#if HAS_PID_HEATING
- template<typename TT, int MIN_POW, int MAX_POW>
+ template<typename TT>
class PIDRunner {
public:
TT &tempinfo;
- __typeof__(TT::pid) work_pid{0};
- float temp_iState = 0, temp_dState = 0;
- bool pid_reset = true;
PIDRunner(TT &t) : tempinfo(t) { }
- float get_pid_output() {
-
+ float get_pid_output(const uint8_t extr=0) {
#if ENABLED(PID_OPENLOOP)
return constrain(tempinfo.target, 0, MAX_POW);
#else // !PID_OPENLOOP
- const float pid_error = tempinfo.target - tempinfo.celsius;
- if (!tempinfo.target || pid_error < -(PID_FUNCTIONAL_RANGE)) {
- pid_reset = true;
- return 0;
- }
- else if (pid_error > PID_FUNCTIONAL_RANGE) {
- pid_reset = true;
- return MAX_POW;
- }
+ float out = tempinfo.pid.get_pid_output(tempinfo.target, tempinfo.celsius);
- if (pid_reset) {
- pid_reset = false;
- temp_iState = 0.0;
- work_pid.Kd = 0.0;
- }
+ #if ENABLED(PID_FAN_SCALING)
+ out += tempinfo.pid.get_fan_scale_output(thermalManager.fan_speed[extr]);
+ #endif
- const float max_power_over_i_gain = float(MAX_POW) / tempinfo.pid.Ki - float(MIN_POW);
- temp_iState = constrain(temp_iState + pid_error, 0, max_power_over_i_gain);
+ #if ENABLED(PID_EXTRUSION_SCALING)
+ out += tempinfo.pid.get_extrusion_scale_output(
+ extr == active_extruder, stepper.position(E_AXIS), planner.mm_per_step[E_AXIS], thermalManager.lpq_len
+ );
+ #endif
- work_pid.Kp = tempinfo.pid.Kp * pid_error;
- work_pid.Ki = tempinfo.pid.Ki * temp_iState;
- work_pid.Kd = work_pid.Kd + PID_K2 * (tempinfo.pid.Kd * (temp_dState - tempinfo.celsius) - work_pid.Kd);
-
- temp_dState = tempinfo.celsius;
-
- return constrain(work_pid.Kp + work_pid.Ki + work_pid.Kd + float(MIN_POW), 0, MAX_POW);
+ return constrain(out, tempinfo.pid.low(), tempinfo.pid.high());
#endif // !PID_OPENLOOP
}
@@ -1395,7 +1374,8 @@ void Temperature::min_temp_error(const heater_id_t heater_id) {
STR_PID_DEBUG_INPUT, c,
STR_PID_DEBUG_OUTPUT, pid_out
#if DISABLED(PID_OPENLOOP)
- , " pTerm ", work_pid.Kp, " iTerm ", work_pid.Ki, " dTerm ", work_pid.Kd
+ , " pTerm ", tempinfo.pid.pTerm(), " iTerm ", tempinfo.pid.iTerm(), " dTerm ", tempinfo.pid.dTerm()
+ , " cTerm ", tempinfo.pid.cTerm(), " fTerm ", tempinfo.pid.fTerm()
#endif
);
}
@@ -1413,14 +1393,14 @@ void Temperature::min_temp_error(const heater_id_t heater_id) {
#if ENABLED(PIDTEMP)
- typedef PIDRunner<hotend_info_t, 0, PID_MAX> PIDRunnerHotend;
+ typedef PIDRunner<hotend_info_t> PIDRunnerHotend;
static PIDRunnerHotend hotend_pid[HOTENDS] = {
#define _HOTENDPID(E) temp_hotend[E],
REPEAT(HOTENDS, _HOTENDPID)
};
- const float pid_output = is_idling ? 0 : hotend_pid[ee].get_pid_output();
+ const float pid_output = is_idling ? 0 : hotend_pid[ee].get_pid_output(ee);
#if ENABLED(PID_DEBUG)
if (ee == active_extruder)
@@ -1521,7 +1501,7 @@ void Temperature::min_temp_error(const heater_id_t heater_id) {
#if ENABLED(PIDTEMPBED)
float Temperature::get_pid_output_bed() {
- static PIDRunner<bed_info_t, MIN_BED_POWER, MAX_BED_POWER> bed_pid(temp_bed);
+ static PIDRunner<bed_info_t> bed_pid(temp_bed);
const float pid_output = bed_pid.get_pid_output();
TERN_(PID_BED_DEBUG, bed_pid.debug(temp_bed.celsius, pid_output, F("(Bed)")));
return pid_output;
@@ -1532,7 +1512,7 @@ void Temperature::min_temp_error(const heater_id_t heater_id) {
#if ENABLED(PIDTEMPCHAMBER)
float Temperature::get_pid_output_chamber() {
- static PIDRunner<chamber_info_t, MIN_CHAMBER_POWER, MAX_CHAMBER_POWER> chamber_pid(temp_chamber);
+ static PIDRunner<chamber_info_t> chamber_pid(temp_chamber);
const float pid_output = chamber_pid.get_pid_output();
TERN_(PID_CHAMBER_DEBUG, chamber_pid.debug(temp_chamber.celsius, pid_output, F("(Chamber)")));
return pid_output;
@@ -2471,9 +2451,6 @@ void Temperature::init() {
TERN_(PROBING_HEATERS_OFF, paused_for_probing = false);
- #if BOTH(PIDTEMP, PID_EXTRUSION_SCALING)
- pes_e_position = 0;
- #endif
// Init (and disable) SPI thermocouples
#if TEMP_SENSOR_IS_ANY_MAX_TC(0) && PIN_EXISTS(TEMP_0_CS)
diff --git a/Marlin/src/module/temperature.h b/Marlin/src/module/temperature.h
index d0f7d2dda77..c40a418428b 100644
--- a/Marlin/src/module/temperature.h
+++ b/Marlin/src/module/temperature.h
@@ -159,94 +159,216 @@ typedef struct { float p, i, d, c, f; } raw_pidcf_t;
#define scalePID_d(d) ( float(d) / PID_dT )
#define unscalePID_d(d) ( float(d) * PID_dT )
- typedef struct {
- float Kp, Ki, Kd;
+ /// @brief The default PID class, only has Kp, Ki, Kd, other classes extend this one
+ /// @tparam MIN_POW output when current is above target by functional_range
+ /// @tparam MAX_POW output when current is below target by functional_range
+ /// @details This class has methods for Kc and Kf terms, but returns constant default values
+ /// PID classes that implement these features are expected to override these methods
+ /// Since the finally used PID class is typedef-d, there is no need to use virtual functions
+ template<int MIN_POW, int MAX_POW>
+ struct PID_t{
+ protected:
+ bool pid_reset = true;
+ float temp_iState = 0.0f, temp_dState = 0.0f;
+ float work_p = 0, work_i = 0, work_d = 0;
+
+ public:
+ float Kp = 0, Ki = 0, Kd = 0;
float p() const { return Kp; }
float i() const { return unscalePID_i(Ki); }
float d() const { return unscalePID_d(Kd); }
float c() const { return 1; }
float f() const { return 0; }
+ float pTerm() const { return work_p; }
+ float iTerm() const { return work_i; }
+ float dTerm() const { return work_d; }
+ float cTerm() const { return 0; }
+ float fTerm() const { return 0; }
void set_Kp(float p) { Kp = p; }
void set_Ki(float i) { Ki = scalePID_i(i); }
void set_Kd(float d) { Kd = scalePID_d(d); }
void set_Kc(float) {}
void set_Kf(float) {}
- void set(float p, float i, float d, float c=1, float f=0) { set_Kp(p); set_Ki(i); set_Kd(d); UNUSED(c); UNUSED(f); }
+ int low() const { return MIN_POW; }
+ int high() const { return MAX_POW; }
+ void reset() { pid_reset = true; }
+ void set(float p, float i, float d, float c=1, float f=0) { set_Kp(p); set_Ki(i); set_Kd(d); set_Kc(c); set_Kf(f); }
void set(const raw_pid_t &raw) { set(raw.p, raw.i, raw.d); }
- void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d); }
- } PID_t;
+ void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d, raw.c, raw.f); }
+
+ float get_fan_scale_output(const uint8_t) { return 0; }
+
+ float get_extrusion_scale_output(const bool, const int32_t, const float, const int16_t) { return 0; }
+
+ float get_pid_output(const float target, const float current) {
+ const float pid_error = target - current;
+ if (!target || pid_error < -(PID_FUNCTIONAL_RANGE)) {
+ pid_reset = true;
+ return 0;
+ }
+ else if (pid_error > PID_FUNCTIONAL_RANGE) {
+ pid_reset = true;
+ return MAX_POW;
+ }
+
+ if (pid_reset) {
+ pid_reset = false;
+ temp_iState = 0.0;
+ work_d = 0.0;
+ }
+
+ const float max_power_over_i_gain = float(MAX_POW) / Ki - float(MIN_POW);
+ temp_iState = constrain(temp_iState + pid_error, 0, max_power_over_i_gain);
+
+ work_p = Kp * pid_error;
+ work_i = Ki * temp_iState;
+ work_d = work_d + PID_K2 * (Kd * (temp_dState - current) - work_d);
+
+ temp_dState = current;
+
+ return constrain(work_p + work_i + work_d + float(MIN_POW), 0, MAX_POW);
+ }
+
+ };
#endif
#if ENABLED(PIDTEMP)
- typedef struct {
- float Kp, Ki, Kd, Kc;
- float p() const { return Kp; }
- float i() const { return unscalePID_i(Ki); }
- float d() const { return unscalePID_d(Kd); }
+ /// @brief Extrusion scaled PID class
+ template<int MIN_POW, int MAX_POW, int LPQ_ARR_SZ>
+ struct PIDC_t : public PID_t<MIN_POW, MAX_POW> {
+ private:
+ using base = PID_t<MIN_POW, MAX_POW>;
+ float work_c = 0;
+ float prev_e_pos = 0;
+ int32_t lpq[LPQ_ARR_SZ] = {};
+ int16_t lpq_ptr = 0;
+ public:
+ float Kc = 0;
float c() const { return Kc; }
- float f() const { return 0; }
- void set_Kp(float p) { Kp = p; }
- void set_Ki(float i) { Ki = scalePID_i(i); }
- void set_Kd(float d) { Kd = scalePID_d(d); }
void set_Kc(float c) { Kc = c; }
- void set_Kf(float) {}
- void set(float p, float i, float d, float c=1, float f=0) { set_Kp(p); set_Ki(i); set_Kd(d); set_Kc(c); set_Kf(f); }
- void set(const raw_pid_t &raw) { set(raw.p, raw.i, raw.d); }
- void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d, raw.c); }
- } PIDC_t;
-
- typedef struct {
- float Kp, Ki, Kd, Kf;
- float p() const { return Kp; }
- float i() const { return unscalePID_i(Ki); }
- float d() const { return unscalePID_d(Kd); }
- float c() const { return 1; }
- float f() const { return Kf; }
- void set_Kp(float p) { Kp = p; }
- void set_Ki(float i) { Ki = scalePID_i(i); }
- void set_Kd(float d) { Kd = scalePID_d(d); }
- void set_Kc(float) {}
- void set_Kf(float f) { Kf = f; }
- void set(float p, float i, float d, float c=1, float f=0) { set_Kp(p); set_Ki(i); set_Kd(d); set_Kf(f); }
- void set(const raw_pid_t &raw) { set(raw.p, raw.i, raw.d); }
- void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d, raw.f); }
- } PIDF_t;
-
- typedef struct {
- float Kp, Ki, Kd, Kc, Kf;
- float p() const { return Kp; }
- float i() const { return unscalePID_i(Ki); }
- float d() const { return unscalePID_d(Kd); }
- float c() const { return Kc; }
- float f() const { return Kf; }
- void set_Kp(float p) { Kp = p; }
- void set_Ki(float i) { Ki = scalePID_i(i); }
- void set_Kd(float d) { Kd = scalePID_d(d); }
- void set_Kc(float c) { Kc = c; }
- void set_Kf(float f) { Kf = f; }
- void set(float p, float i, float d, float c=1, float f=0) { set_Kp(p); set_Ki(i); set_Kd(d); set_Kc(c); set_Kf(f); }
+ float cTerm() const { return work_c; }
+ void set(float p, float i, float d, float c=1, float f=0) {
+ base::set_Kp(p);
+ base::set_Ki(i);
+ base::set_Kd(d);
+ set_Kc(c);
+ base::set_Kf(f);
+ }
void set(const raw_pid_t &raw) { set(raw.p, raw.i, raw.d); }
void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d, raw.c, raw.f); }
- } PIDCF_t;
+ void reset() {
+ base::reset();
+ prev_e_pos = 0;
+ lpq_ptr = 0;
+ LOOP_L_N(i, LPQ_ARR_SZ) lpq[i] = 0;
+ }
+
+ float get_extrusion_scale_output(const bool is_active, const int32_t e_position, const float e_mm_per_step, const int16_t lpq_len) {
+ work_c = 0;
+ if (!is_active) return work_c;
+
+ if (e_position > prev_e_pos) {
+ lpq[lpq_ptr] = e_position - prev_e_pos;
+ prev_e_pos = e_position;
+ }
+ else
+ lpq[lpq_ptr] = 0;
+
+ ++lpq_ptr;
+
+ if (lpq_ptr >= LPQ_ARR_SZ || lpq_ptr >= lpq_len)
+ lpq_ptr = 0;
+
+ work_c = (lpq[lpq_ptr] * e_mm_per_step) * Kc;
+
+ return work_c;
+ }
+ };
+
+ /// @brief Fan scaled PID, this class implements the get_fan_scale_output() method
+ /// @tparam MIN_POW @see PID_t
+ /// @tparam MAX_POW @see PID_t
+ /// @tparam SCALE_MIN_SPEED parameter from Configuration_adv.h
+ /// @tparam SCALE_LIN_FACTOR parameter from Configuration_adv.h
+ template<int MIN_POW, int MAX_POW, int SCALE_MIN_SPEED, int SCALE_LIN_FACTOR>
+ struct PIDF_t : public PID_t<MIN_POW, MAX_POW> {
+ private:
+ using base = PID_t<MIN_POW, MAX_POW>;
+ float work_f = 0;
+ public:
+ float Kf = 0;
+ float f() const { return Kf; }
+ void set_Kf(float f) { Kf = f; }
+ float fTerm() const { return work_f; }
+ void set(float p, float i, float d, float c=1, float f=0) {
+ base::set_Kp(p);
+ base::set_Ki(i);
+ base::set_Kd(d);
+ base::set_Kc(c);
+ set_Kf(f);
+ }
+ void set(const raw_pid_t &raw) { set(raw.p, raw.i, raw.d); }
+ void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d, raw.c, raw.f); }
+
+ float get_fan_scale_output(const uint8_t fan_speed) {
+ work_f = 0;
+ if (fan_speed > SCALE_MIN_SPEED)
+ work_f = Kf + (SCALE_LIN_FACTOR) * fan_speed;
+
+ return work_f;
+ }
+ };
+
+ /// @brief Inherits PID and PIDC - can't use proper diamond inheritance w/o virtual
+ template<int MIN_POW, int MAX_POW, int LPQ_ARR_SZ, int SCALE_MIN_SPEED, int SCALE_LIN_FACTOR>
+ struct PIDCF_t : public PIDC_t<MIN_POW, MAX_POW, LPQ_ARR_SZ> {
+ private:
+ using base = PID_t<MIN_POW, MAX_POW>;
+ using cPID = PIDC_t<MIN_POW, MAX_POW, LPQ_ARR_SZ>;
+ float work_f = 0;
+ public:
+ float Kf = 0;
+ float c() const { return cPID::c(); }
+ float f() const { return Kf; }
+ void set_Kc(float c) { cPID::set_Kc(c); }
+ void set_Kf(float f) { Kf = f; }
+ float cTerm() const { return cPID::cTerm(); }
+ float fTerm() const { return work_f; }
+ void set(float p, float i, float d, float c=1, float f=0) {
+ base::set_Kp(p);
+ base::set_Ki(i);
+ base::set_Kd(d);
+ cPID::set_Kc(c);
+ set_Kf(f);
+ }
+ void set(const raw_pid_t &raw) { set(raw.p, raw.i, raw.d); }
+ void set(const raw_pidcf_t &raw) { set(raw.p, raw.i, raw.d, raw.c, raw.f); }
+
+ void reset() { cPID::reset(); }
+
+ float get_fan_scale_output(const uint8_t fan_speed) {
+ work_f = fan_speed > (SCALE_MIN_SPEED) ? Kf + (SCALE_LIN_FACTOR) * fan_speed : 0;
+ return work_f;
+ }
+ float get_extrusion_scale_output(const bool is_active, const int32_t e_position, const float e_mm_per_step, const int16_t lpq_len) {
+ return cPID::get_extrusion_scale_output(is_active, e_position, e_mm_per_step, lpq_len);
+ }
+ };
typedef
#if BOTH(PID_EXTRUSION_SCALING, PID_FAN_SCALING)
- PIDCF_t
+ PIDCF_t<0, PID_MAX, LPQ_MAX_LEN, PID_FAN_SCALING_MIN_SPEED, PID_FAN_SCALING_LIN_FACTOR>
#elif ENABLED(PID_EXTRUSION_SCALING)
- PIDC_t
+ PIDC_t<0, PID_MAX, LPQ_MAX_LEN>
#elif ENABLED(PID_FAN_SCALING)
- PIDF_t
+ PIDF_t<0, PID_MAX, PID_FAN_SCALING_MIN_SPEED, PID_FAN_SCALING_LIN_FACTOR>
#else
- PID_t
+ PID_t<0, PID_MAX>
#endif
hotend_pid_t;
- #if ENABLED(PID_EXTRUSION_SCALING)
- typedef IF<(LPQ_MAX_LEN > 255), uint16_t, uint8_t>::type lpq_ptr_t;
- #endif
-
#if ENABLED(PID_PARAMS_PER_HOTEND)
#define SET_HOTEND_PID(F,H,V) thermalManager.temp_hotend[H].pid.set_##F(V)
#else
@@ -326,14 +448,14 @@ struct PIDHeaterInfo : public HeaterInfo {
#endif
#if HAS_HEATED_BED
#if ENABLED(PIDTEMPBED)
- typedef struct PIDHeaterInfo<PID_t> bed_info_t;
+ typedef struct PIDHeaterInfo<PID_t<MIN_BED_POWER, MAX_BED_POWER>> bed_info_t;
#else
typedef heater_info_t bed_info_t;
#endif
#endif
#if HAS_HEATED_CHAMBER
#if ENABLED(PIDTEMPCHAMBER)
- typedef struct PIDHeaterInfo<PID_t> chamber_info_t;
+ typedef struct PIDHeaterInfo<PID_t<MIN_CHAMBER_POWER, MAX_CHAMBER_POWER>> chamber_info_t;
#else
typedef heater_info_t chamber_info_t;
#endif
@@ -585,11 +707,6 @@ class Temperature {
static hotend_watch_t watch_hotend[HOTENDS];
#endif
- #if ENABLED(PID_EXTRUSION_SCALING)
- static int32_t pes_e_position, lpq[LPQ_MAX_LEN];
- static lpq_ptr_t lpq_ptr;
- #endif
-
#if ENABLED(MPCTEMP)
static int32_t mpc_e_position;
#endif
@@ -1035,7 +1152,7 @@ class Temperature {
// Update the temp manager when PID values change
#if ENABLED(PIDTEMP)
- static void updatePID() { TERN_(PID_EXTRUSION_SCALING, pes_e_position = 0); }
+ static void updatePID() { HOTEND_LOOP() temp_hotend[e].pid.reset(); }
static void setPID(const uint8_t hotend, const_float_t p, const_float_t i, const_float_t d) {
#if ENABLED(PID_PARAMS_PER_HOTEND)
temp_hotend[hotend].pid.set(p, i, d);
diff --git a/buildroot/tests/LPC1768 b/buildroot/tests/LPC1768
index 9fb7479d992..535e776a389 100755
--- a/buildroot/tests/LPC1768
+++ b/buildroot/tests/LPC1768
@@ -52,5 +52,12 @@ opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER ADAPTIVE_FAN_SLOWING NO
SDSUPPORT SDCARD_SORT_ALPHA AUTO_REPORT_SD_STATUS EMERGENCY_PARSER SOFT_RESET_ON_KILL SOFT_RESET_VIA_SERIAL
exec_test $1 $2 "Re-ARM with NOZZLE_AS_PROBE and many features." "$3"
+restore_configs
+opt_set MOTHERBOARD BOARD_BTT_SKR_V1_3 EXTRUDERS 2 \
+ TEMP_SENSOR_0 1 TEMP_SENSOR_1 1 TEMP_SENSOR_BED 1 TEMP_SENSOR_CHAMBER 1 \
+ TEMP_CHAMBER_PIN P1_30 HEATER_CHAMBER_PIN P0_28
+opt_enable PIDTEMPBED PIDTEMPCHAMBER PID_EXTRUSION_SCALING PID_FAN_SCALING
+exec_test $1 $2 "SKR v1.3 with 2*Extr, bed, chamber all PID." "$3"
+
# clean up
restore_configs