55 lines
1.4 KiB
C
55 lines
1.4 KiB
C
#include "pid.h"
|
||
|
||
void pid_init(pid_t *pid)
|
||
{
|
||
if (!pid) return;
|
||
pid->Kp = 0.0f;
|
||
pid->Ki = 0.0f;
|
||
pid->Kd = 0.0f;
|
||
pid->integral = 0.0f;
|
||
pid->prev_measurement = 0.0f;
|
||
pid->output_max = 0.0f;
|
||
pid->integral_max = 0.0f;
|
||
}
|
||
|
||
float pid_compute(pid_t *pid, float target, float actual)
|
||
{
|
||
if (!pid) return 0.0f;
|
||
float error = target - actual;
|
||
|
||
/* 比例项 */
|
||
float p_term = pid->Kp * error;
|
||
|
||
/* 微分项（测量值微分 — 避免目标突变微分冲击） */
|
||
float d_term = pid->Kd * (-(actual - pid->prev_measurement));
|
||
pid->prev_measurement = actual;
|
||
|
||
/* 积分项（带抗饱和限幅） */
|
||
pid->integral += error;
|
||
if (pid->integral_max > 0.0f) {
|
||
if (pid->integral > pid->integral_max)
|
||
pid->integral = pid->integral_max;
|
||
else if (pid->integral < -pid->integral_max)
|
||
pid->integral = -pid->integral_max;
|
||
}
|
||
float i_term = pid->Ki * pid->integral;
|
||
float output = p_term + i_term + d_term;
|
||
|
||
/* 输出限幅（0=不限幅） */
|
||
if (pid->output_max > 0.0f) {
|
||
if (output > pid->output_max)
|
||
output = pid->output_max;
|
||
else if (output < -pid->output_max)
|
||
output = -pid->output_max;
|
||
}
|
||
|
||
return output;
|
||
}
|
||
|
||
void pid_reset(pid_t *pid)
|
||
{
|
||
if (!pid) return;
|
||
pid->integral = 0.0f;
|
||
pid->prev_measurement = 0.0f;
|
||
}
|