#目录
文章目录
- 摘要
- [1.PX4 PID控制器源码](#1.PX4 PID控制器源码)
摘要
本节主要学习PX4的PID控制源码,欢迎批评指正。
1.PX4 PID控制器源码
1.pid.cpp文件
cpp
#include "pid.h"
#include <math.h>
#include <px4_platform_common/defines.h>
//定义最小的sigma值
#define SIGMA 0.000001f
/***************************************************************************************************
*函数原型:__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
*函数功能:pid初始化
*修改日期:2024-8-23
*修改人员:coco
*备注功能: 无
****************************************************************************************************/
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
{
//使用哪种模式
pid->mode = mode;
//控制时间常数
pid->dt_min = dt_min;
//PID参数-KP
pid->kp = 0.0f;
//PID参数-KI
pid->ki = 0.0f;
//PID参数-KD
pid->kd = 0.0f;
//PID积分总和
pid->integral = 0.0f;
//PID积分限制输出多少
pid->integral_limit = 0.0f;
//PID输出限制输出多少
pid->output_limit = 0.0f;
//历史误差
pid->error_previous = 0.0f;
//历史输出
pid->last_output = 0.0f;
}
/***************************************************************************************************************************************
*函数原型:__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
*函数功能:pid参数设置
*修改日期:2024-8-23
*修改人员:coco
*备注功能: 无
****************************************************************************************************************************************/
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
//定义初始化变量为0
int ret = 0;
//判断参数kp是否是有限制大
if (PX4_ISFINITE(kp))
{
pid->kp = kp;
} else
{
ret = 1;
}
//判断参数ki是否是有限制大
if (PX4_ISFINITE(ki))
{
pid->ki = ki;
} else
{
ret = 1;
}
//判断参数kd是否是有限制大
if (PX4_ISFINITE(kd))
{
pid->kd = kd;
} else
{
ret = 1;
}
//判断积分输出是否是有限制大
if (PX4_ISFINITE(integral_limit))
{
pid->integral_limit = integral_limit;
} else
{
ret = 1;
}
//判断pid输出是否是有限制大
if (PX4_ISFINITE(output_limit))
{
pid->output_limit = output_limit;
} else
{
ret = 1;
}
//如果输出是0说明是正常,输出是1说明异常了
return ret;
}
/***************************************************************************************************************************************
*函数原型:__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
*函数功能:pid计算过程
*修改日期:2024-8-23
*修改人员:coco
*备注功能: 无
****************************************************************************************************************************************/
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
//只要有一个参数不是有限大,直接输出上次的历史值
if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt))
{
return pid->last_output;
}
//定义局部临时变量
float i, d;
/*计算当前误差值=目标值-实际值 current error value */
float error = sp - val;
/*计算当前误差的微分---- current error derivative */
if (pid->mode == PID_MODE_DERIVATIV_CALC) //模式1---正常计算微分
{
//(当前误差-上次误差)/时间
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
//更新历史午餐值
pid->error_previous = error;
} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) //从先前值计算离散导数,设定点导数将被忽略,pid_calculate()中的val_dot将被忽略
{
//可以看出这个是根据实际值计算,注意这里一定要用负值,这样才有阻尼的效果
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
//更新误差
pid->error_previous = -val;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) //3---通过外部设定
{
d = -val_dot;
} else //否则不用微分
{
d = 0.0f;
}
//判断微分作用如果不是有限的就设定0
if (!PX4_ISFINITE(d))
{
d = 0.0f;
}
/*计算PD输出----- calculate PD output */
float output = (error * pid->kp) + (d * pid->kd);
//积分参数是否超过0.000001f
if (pid->ki > SIGMA)
{
//计算误差积分并检查饱和度------Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
/*检查饱和度----- check for saturation */
if (PX4_ISFINITE(i))
{
//(输出是比较小或者(PD输出+I积分总和的绝对值小于输出限制))同时需要积分没有饱和才采用新积分值
if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
fabsf(i) <= pid->integral_limit)
{
/*未饱和,使用新的积分值---- not saturated, use new integral value */
pid->integral = i;
}
}
/*增加积分输出到PID输出------add I component to output */
output += pid->integral * pid->ki;
}
/*限制输出----limit output */
if (PX4_ISFINITE(output))
{
//输出限制大于很小
if (pid->output_limit > SIGMA)
{
if (output > pid->output_limit)
{
output = pid->output_limit;
} else if (output < -pid->output_limit)
{
output = -pid->output_limit;
}
}
//更新输出
pid->last_output = output;
}
//更新输出
return pid->last_output;
}
/***************************************************************************************************************************************
*函数原型:__EXPORT void pid_reset_integral(PID_t *pid)
*函数功能:pid复位积分项
*修改人员:coco
*备注功能: 无
****************************************************************************************************************************************/
__EXPORT void pid_reset_integral(PID_t *pid)
{
pid->integral = 0.0f;
}
/****************************************************************************************************************************************
* 文件结束
****************************************************************************************************************************************/
2.pid.h文件
cpp
#ifndef PID_H_
#define PID_H_
#include <stdint.h>
__BEGIN_DECLS
typedef enum PID_MODE {
/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
PID_MODE_DERIVATIV_NONE = 0,
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
* val_dot in pid_calculate() will be ignored */
PID_MODE_DERIVATIV_CALC,
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
* setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
PID_MODE_DERIVATIV_CALC_NO_SP,
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
PID_MODE_DERIVATIV_SET
} pid_mode_t;
typedef struct {
pid_mode_t mode;
float dt_min;
float kp;
float ki;
float kd;
float integral;
float integral_limit;
float output_limit;
float error_previous;
float last_output;
} PID_t;
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
__EXPORT void pid_reset_integral(PID_t *pid);
__END_DECLS
#endif /* PID_H_ */