Add low pass for Derviative part in PID controller
This commit is contained in:
parent
91d9db6a4e
commit
cd35f9e694
@ -21,6 +21,8 @@
|
||||
#include <reflow-controller/pid-controller.h>
|
||||
#include <string.h>
|
||||
|
||||
const float kd_tau = 2.0f;
|
||||
|
||||
void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min,
|
||||
float output_sat_max, float integral_max, float sample_period)
|
||||
{
|
||||
@ -32,7 +34,8 @@ void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p,
|
||||
pid->k_int = k_int;
|
||||
pid->k_deriv = k_deriv;
|
||||
pid->k_int_t = pid->k_int * pid->sample_period * 0.5f;
|
||||
pid->k_deriv_t = pid->k_deriv * 2.0f / pid->sample_period;
|
||||
pid->k_deriv_t = pid->k_deriv * 2.0f / (pid->sample_period + 2 * kd_tau);
|
||||
pid->k_inv_deriv_t = (2 * kd_tau - sample_period) / (2 * kd_tau + sample_period);
|
||||
pid->output_sat_max = output_sat_max;
|
||||
pid->output_sat_min = output_sat_min;
|
||||
pid->integral_max = integral_max;
|
||||
@ -61,7 +64,7 @@ static void calculate_integral(struct pid_controller *pid, float deviation)
|
||||
{
|
||||
pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in);
|
||||
|
||||
/* Saturate integral term to spoecified maximum */
|
||||
/* Saturate integral term to specified maximum */
|
||||
if (pid->integral > pid->integral_max)
|
||||
pid->integral = pid->integral_max;
|
||||
else if (pid->integral < -pid->integral_max)
|
||||
@ -78,13 +81,13 @@ float pid_sample(struct pid_controller *pid, float deviation)
|
||||
output = deviation * pid->k_p;
|
||||
|
||||
/* PID runaway compensation */
|
||||
if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5f) &&
|
||||
!(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5f)) {
|
||||
if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5f && pid->integral > 0) &&
|
||||
!(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5f && pid->integral < 0)) {
|
||||
calculate_integral(pid, deviation);
|
||||
}
|
||||
|
||||
/* Calculate derivative part */
|
||||
pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->derivate;
|
||||
pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->k_inv_deriv_t * pid->derivate;
|
||||
|
||||
output += pid->derivate;
|
||||
output += pid->integral;
|
||||
|
Loading…
Reference in New Issue
Block a user