131 lines
3.3 KiB
C
131 lines
3.3 KiB
C
#include "app_config.h"
|
||
|
||
#include "app_pid.h"
|
||
|
||
|
||
|
||
|
||
// PID控制器初始化
|
||
void initializePid(PID_t *pid, pid_mode_t mode, float dtMin)
|
||
{
|
||
pid->mode = mode;
|
||
pid->dt_min = dtMin > SIGMA ? dtMin : SIGMA;
|
||
pid->kp = 0.0f;
|
||
pid->ki = 0.0f;
|
||
pid->kd = 0.0f;
|
||
pid->integral = 0.0f;
|
||
pid->integral_limit = 0.0f;
|
||
pid->output_limit = 0.0f;
|
||
pid->error_previous = 0.0f;
|
||
pid->last_output = 0.0f;
|
||
}
|
||
|
||
// 设置PID参数
|
||
int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit)
|
||
{
|
||
int32_t ret = 0;
|
||
|
||
if (isfinite(kp)) {
|
||
pid->kp = kp;
|
||
} else {
|
||
ret = -1;
|
||
}
|
||
|
||
if (isfinite(ki)) {
|
||
pid->ki = ki;
|
||
} else {
|
||
ret = -2;
|
||
}
|
||
|
||
if (isfinite(kd)) {
|
||
pid->kd = kd;
|
||
} else {
|
||
ret = -3;
|
||
}
|
||
|
||
if (isfinite(integralLimit)) {
|
||
pid->integral_limit = integralLimit;
|
||
} else {
|
||
ret = -4;
|
||
}
|
||
|
||
if (isfinite(outputLimit)) {
|
||
pid->output_limit = outputLimit;
|
||
} else {
|
||
ret = -5;
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算PID控制器的输出
|
||
*
|
||
* @param pid 指向PID_t结构体的指针，包含PID控制器的状态和参数
|
||
* @param sp 设定值(Setpoint)，期望系统达到的目标值
|
||
* @param val 当前值(Current Value)，系统的实际测量值
|
||
* @param val_dot 当前值的导数(Derivative of Current Value)，即测量值的变化率（用于微分项计算）
|
||
* @param dt 时间增量(Time Increment)，两次调用之间的时间间隔，用于计算积分和微分
|
||
*
|
||
* @return float 返回PID控制器计算出的输出值
|
||
*/
|
||
float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt)
|
||
{
|
||
// 检查输入参数的有效性
|
||
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt) || dt < pid->dt_min) {
|
||
return pid->last_output;
|
||
}
|
||
|
||
// 计算误差
|
||
float error = sp - val;
|
||
|
||
// 根据模式计算微分项
|
||
float derivative = 0.0f;
|
||
switch (pid->mode) {
|
||
case PID_MODE_DERIVATIVE_CALC:
|
||
derivative = (error - pid->error_previous) / dt;
|
||
pid->error_previous = error;
|
||
break;
|
||
case PID_MODE_DERIVATIVE_CALC_NO_SP:
|
||
derivative = (-val - pid->error_previous) / dt;
|
||
pid->error_previous = -val;
|
||
break;
|
||
case PID_MODE_DERIVATIVE_SET:
|
||
derivative = -val_dot;
|
||
break;
|
||
default:
|
||
derivative = 0.0f;
|
||
break;
|
||
}
|
||
|
||
// 计算比例和微分项的输出
|
||
float output = (pid->kp * error) + (pid->kd * derivative);
|
||
|
||
// 计算积分项，并检查积分饱和
|
||
if (pid->ki > SIGMA) {
|
||
pid->integral += error * dt;
|
||
if (pid->integral > pid->integral_limit) {
|
||
pid->integral = pid->integral_limit;
|
||
} else if (pid->integral < -pid->integral_limit) {
|
||
pid->integral = -pid->integral_limit;
|
||
}
|
||
output += pid->ki * pid->integral;
|
||
}
|
||
|
||
// 限制输出范围
|
||
if (output > pid->output_limit) {
|
||
output = pid->output_limit;
|
||
} else if (output < -pid->output_limit) {
|
||
output = -pid->output_limit;
|
||
}
|
||
|
||
pid->last_output = output;
|
||
return output;
|
||
}
|
||
|
||
// 重置积分器
|
||
void resetPidIntegral(PID_t *pid)
|
||
{
|
||
pid->integral = 0.0f;
|
||
}
|