diff --git a/Marlin/src/module/temperature.h b/Marlin/src/module/temperature.h index 088a17ec1b..16d4a38e08 100644 --- a/Marlin/src/module/temperature.h +++ b/Marlin/src/module/temperature.h @@ -204,31 +204,35 @@ typedef struct { float p, i, d, c, f; } raw_pidcf_t; float get_pid_output(const float target, const float current) { const float pid_error = target - current; + float output_pow; if (!target || pid_error < -(PID_FUNCTIONAL_RANGE)) { pid_reset = true; - return 0; + output_pow = 0; } else if (pid_error > PID_FUNCTIONAL_RANGE) { pid_reset = true; - return MAX_POW; + output_pow = MAX_POW; } + else { + if (pid_reset) { + pid_reset = false; + temp_iState = 0.0; + work_d = 0.0; + } - 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); + + output_pow = constrain(work_p + work_i + work_d + float(MIN_POW), 0, MAX_POW); } - 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); + return output_pow; } };