mirror of
https://github.com/esphome/esphome.git
synced 2024-12-02 11:44:13 +01:00
124 lines
3.4 KiB
C++
124 lines
3.4 KiB
C++
#include "pid_controller.h"
|
|
|
|
namespace esphome {
|
|
namespace pid {
|
|
|
|
float PIDController::update(float setpoint, float process_value) {
|
|
// e(t) ... error at timestamp t
|
|
// r(t) ... setpoint
|
|
// y(t) ... process value (sensor reading)
|
|
// u(t) ... output value
|
|
|
|
dt_ = calculate_relative_time_();
|
|
|
|
// e(t) := r(t) - y(t)
|
|
error_ = setpoint - process_value;
|
|
|
|
calculate_proportional_term_();
|
|
calculate_integral_term_();
|
|
calculate_derivative_term_();
|
|
|
|
// u(t) := p(t) + i(t) + d(t)
|
|
float output = proportional_term_ + integral_term_ + derivative_term_;
|
|
|
|
// smooth/sample the output
|
|
int samples = in_deadband() ? deadband_output_samples_ : output_samples_;
|
|
return weighted_average_(output_list_, output, samples);
|
|
}
|
|
|
|
bool PIDController::in_deadband() {
|
|
// return (fabs(error) < deadband_threshold);
|
|
float err = -error_;
|
|
return ((err > 0 && err < threshold_high_) || (err < 0 && err > threshold_low_));
|
|
}
|
|
|
|
void PIDController::calculate_proportional_term_() {
|
|
// p(t) := K_p * e(t)
|
|
proportional_term_ = kp_ * error_;
|
|
|
|
// set dead-zone to -X to +X
|
|
if (in_deadband()) {
|
|
// shallow the proportional_term in the deadband by the pdm
|
|
proportional_term_ *= kp_multiplier_;
|
|
|
|
} else {
|
|
// pdm_offset prevents a jump when leaving the deadband
|
|
float threshold = (error_ < 0) ? threshold_high_ : threshold_low_;
|
|
float pdm_offset = (threshold - (kp_multiplier_ * threshold)) * kp_;
|
|
proportional_term_ += pdm_offset;
|
|
}
|
|
}
|
|
|
|
void PIDController::calculate_integral_term_() {
|
|
// i(t) := K_i * \int_{0}^{t} e(t) dt
|
|
float new_integral = error_ * dt_ * ki_;
|
|
|
|
if (in_deadband()) {
|
|
// shallow the integral when in the deadband
|
|
accumulated_integral_ += new_integral * ki_multiplier_;
|
|
} else {
|
|
accumulated_integral_ += new_integral;
|
|
}
|
|
|
|
// constrain accumulated integral value
|
|
if (!std::isnan(min_integral_) && accumulated_integral_ < min_integral_)
|
|
accumulated_integral_ = min_integral_;
|
|
if (!std::isnan(max_integral_) && accumulated_integral_ > max_integral_)
|
|
accumulated_integral_ = max_integral_;
|
|
|
|
integral_term_ = accumulated_integral_;
|
|
}
|
|
|
|
void PIDController::calculate_derivative_term_() {
|
|
// derivative_term_
|
|
// d(t) := K_d * de(t)/dt
|
|
float derivative = 0.0f;
|
|
if (dt_ != 0.0f)
|
|
derivative = (error_ - previous_error_) / dt_;
|
|
previous_error_ = error_;
|
|
|
|
// smooth the derivative samples
|
|
derivative = weighted_average_(derivative_list_, derivative, derivative_samples_);
|
|
|
|
derivative_term_ = kd_ * derivative;
|
|
|
|
if (in_deadband()) {
|
|
// shallow the derivative when in the deadband
|
|
derivative_term_ *= kd_multiplier_;
|
|
}
|
|
}
|
|
|
|
float PIDController::weighted_average_(std::deque<float> &list, float new_value, int samples) {
|
|
// if only 1 sample needed, clear the list and return
|
|
if (samples == 1) {
|
|
list.clear();
|
|
return new_value;
|
|
}
|
|
|
|
// add the new item to the list
|
|
list.push_front(new_value);
|
|
|
|
// keep only 'samples' readings, by popping off the back of the list
|
|
while (list.size() > samples)
|
|
list.pop_back();
|
|
|
|
// calculate and return the average of all values in the list
|
|
float sum = 0;
|
|
for (auto &elem : list)
|
|
sum += elem;
|
|
return sum / list.size();
|
|
}
|
|
|
|
float PIDController::calculate_relative_time_() {
|
|
uint32_t now = millis();
|
|
uint32_t dt = now - this->last_time_;
|
|
if (last_time_ == 0) {
|
|
last_time_ = now;
|
|
return 0.0f;
|
|
}
|
|
last_time_ = now;
|
|
return dt / 1000.0f;
|
|
}
|
|
|
|
} // namespace pid
|
|
} // namespace esphome
|