四旋翼无人机从0到1的实现（二十五）无人机工程中PID

Author：家有仙妻谢掌柜
Date：2021/2/18

今年会更新一个系列，小四轴无人机从功能设计→思维导图→原理图设计→PCBLayout→焊接PCB→程序代码的编写→整机调试一系列，以此记录自己的成长历程！
这个小四轴无人机是大学时期学习制作的，加上现在工作学习对嵌入式的理解更加深入，因此想要重新梳理一下小四轴，之后在此基础上实现大四轴的飞控设计，这些都将在工作之余完成！

//小四轴无人机设计

#include "pid.h"

/*******************************************************************************
 * fuction	pid_parameter_init
 * brief	pid参数的初始化
 * param	无
 * return	无
 *******************************************************************************/  
void pid_parameter_init(void)
{
	/* 角度外环参数初始化 */
// PID Pitch,Roll;
 //PID PitchRate,RollRate,YawRate;
// kp,ki,kd;
// Diff,Integral;
// Integral_Max;
// Err,PreErr;
// Pout,Iout,Dout;
// Dt;
	Pitch.kp = 8.0f;
	Pitch.ki = 0.0f;
	Pitch.kd = 0.1f;
	Pitch.Dt = 0.005f;
	
	Roll.kp = Pitch.kp;
	Roll.ki = Pitch.ki;
	Roll.kd = Pitch.kd;
	Roll.Dt = Pitch.Dt;
	
	/* 角环参数初始速度内化 */
	PitchRate.kp = 2.8f;
	PitchRate.ki = 0.1f;
	PitchRate.kd = 0.1f;
	PitchRate.Dt = 0.001f;
	PitchRate.Integral_Max = 200;
	
	RollRate.kp = PitchRate.kp;
	RollRate.ki = PitchRate.ki;
	RollRate.kd = PitchRate.kd;
	RollRate.Dt = PitchRate.Dt;
	RollRate.Integral_Max = 200;
	
	YawRate.kp = 6.0f;
	YawRate.ki = 30.0f;
	YawRate.kd = 0.1f;
	YawRate.Dt = 0.001f;
	YawRate.Integral_Max = 300;	
	
}
/*******************************************************************************
 * fuction	pid_gyro_control
 * brief	pid角速度内环控制
 * param	期望值 反馈值 PID内型系数（pitch roll）
 * return	output
 *******************************************************************************/  
float pid_gyro_control(PID*pid,float expect,float feedback)
{
	float output = 0.0f;
	/* 算偏差值 */
	pid->Err = expect-feedback;
	/* 获得微分 */
	pid->Diff = (pid->Err-pid->PreErr)/pid->Dt;
	pid->PreErr = pid->Err;
//	/* 比例项输出 */
//	pid->Pout = pid->kp*pid->Err;
	
	/* 积分项输出 */    //需要有个保护 有一个最小值，有一个最大值 炒过就不积分
	if(fabs(pid->Err)>0.5f&&fabs(pid->Err)<800)
	pid->Integral += pid->Err*pid->Dt;
	if(pid->Integral>pid->Integral_Max)
	   pid->Integral = pid->Integral_Max;
	if(pid->Integral<-pid->Integral_Max)
	   pid->Integral=-pid->Integral_Max;
		/* 比例项输出 */
	pid->Pout = pid->kp*pid->Err;
	/* 积累分项输出 */
	pid->Iout = pid->ki*pid->Integral;
	
	/* 微分项输出 */
	pid->Dout = pid->kd* pid->Diff;
	/* 控制器的总输出 */
	output = pid->Pout+ pid->Dout +pid->Iout;
	return output;
}
/*******************************************************************************
 * fuction	pid_angle_control
 * brief	pid角度外环控制
 * param	期望值 反馈值 PID系数（pitch roll）
 * return	output
 *******************************************************************************/  
float pid_angle_control(PID*pid,float expect,float feedback)
{
	/* 算偏差值， */
	float output = 0.0f;
	pid->Err = expect-feedback;
	pid->Diff = (pid->Err-pid->PreErr)/pid->Dt;
	pid->PreErr = pid->Err;
	/* 比例项输出 */
	pid->Pout = pid->kp*pid->Err;
	pid->Dout = pid->kd* pid->Diff;
	/* 控制器的总输出 */
	output = pid->Pout+ pid->Dout;
	return output;
	
}

#ifndef _PID_H__
#define _PID_H__

#include "board_define.h"
#include "var_global.h"
//void all_init(void);
float pid_angle_control(PID*pid,float expect,float feedback);
void pid_parameter_init(void);
float pid_gyro_control(PID*pid,float expect,float feedback);

#endif