从零开始学习PX4源码4(PID控制器)

#目录

摘要

---

本节主要学习PX4的PID控制源码，欢迎批评指正。

---

1.PX4 PID控制器源码

---

1.pid.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文件

---

#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_ */