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

魔城烟雨2024-08-24 12:53

#目录

文章目录

摘要

本节主要学习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_ */
相关推荐
Stanford_11062 小时前
如何利用Python进行数据分析与可视化的具体操作指南
开发语言·c++·python·微信小程序·微信公众平台·twitter·微信开放平台
@小博的博客3 小时前
【Linux探索学习】第二篇Linux的基本指令（2）——开启Linux学习第二篇
linux·运维·学习
千里马-horse4 小时前
Async++ 源码分析8--partitioner.h
开发语言·c++·async++·partitioner
格林威4 小时前
常规线扫描镜头有哪些类型？能做什么？
人工智能·深度学习·数码相机·算法·计算机视觉·视觉检测·工业镜头
Lucis__5 小时前
再探类&对象——C++入门进阶
开发语言·c++
007php0075 小时前
某大厂跳动面试：计算机网络相关问题解析与总结
java·开发语言·学习·计算机网络·mysql·面试·职场和发展
知识分享小能手5 小时前
微信小程序入门学习教程，从入门到精通，微信小程序核心 API 详解与案例(13）
前端·javascript·学习·react.js·微信小程序·小程序·vue
递归不收敛5 小时前
吴恩达机器学习课程（PyTorch 适配）学习笔记：3.3 推荐系统全面解析
pytorch·学习·机器学习
北京不会遇到西雅图6 小时前
【SLAM】【后端优化】不同优化方法对比
c++·机器人
jndingxin6 小时前
c++多线程（6）------ 条件变量
开发语言·c++
热门推荐
01两千字总结：Codex 国内如何安装和使用的教程，以及如何设置中文回答02GitHub 镜像站点03UV安装并设置国内源0446个Nano-banana 精选提示词，持续更新中05Linux下V2Ray安装配置指南06智能库存管理的需求预测模型：从业务痛点到落地代码的完整实践07GitLab 零基础入门指南：从安装到项目管理全流程08Cursor Plan Mode：AI 终于知道先想后做了09一文了解国产算子编程语言 TileLang，TileLang 对国产开源生态的影响与启示10KGG转MP3工具|非KGM文件|解密音频