Remove setpoint-change from error when calculating derivative in pid controller (#4737)

Co-authored-by: Mathias Pihl <mapih@frode-laursen.com>
This commit is contained in:
Mathias Pihl 2023-12-13 00:50:55 +01:00 committed by GitHub
parent 8789925fe8
commit 69026f7599
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 10 additions and 4 deletions

View file

@ -16,7 +16,7 @@ float PIDController::update(float setpoint, float process_value) {
calculate_proportional_term_();
calculate_integral_term_();
calculate_derivative_term_();
calculate_derivative_term_(setpoint);
// u(t) := p(t) + i(t) + d(t)
float output = proportional_term_ + integral_term_ + derivative_term_;
@ -69,13 +69,18 @@ void PIDController::calculate_integral_term_() {
integral_term_ = accumulated_integral_;
}
void PIDController::calculate_derivative_term_() {
void PIDController::calculate_derivative_term_(float setpoint) {
// derivative_term_
// d(t) := K_d * de(t)/dt
float derivative = 0.0f;
if (dt_ != 0.0f)
if (dt_ != 0.0f) {
// remove changes to setpoint from error
if (!std::isnan(previous_setpoint_) && previous_setpoint_ != setpoint)
previous_error_ -= previous_setpoint_ - setpoint;
derivative = (error_ - previous_error_) / dt_;
}
previous_error_ = error_;
previous_setpoint_ = setpoint;
// smooth the derivative samples
derivative = weighted_average_(derivative_list_, derivative, derivative_samples_);

View file

@ -49,12 +49,13 @@ struct PIDController {
void calculate_proportional_term_();
void calculate_integral_term_();
void calculate_derivative_term_();
void calculate_derivative_term_(float setpoint);
float weighted_average_(std::deque<float> &list, float new_value, int samples);
float calculate_relative_time_();
/// Error from previous update used for derivative term
float previous_error_ = 0;
float previous_setpoint_ = NAN;
/// Accumulated integral value
float accumulated_integral_ = 0;
uint32_t last_time_ = 0;