本文主要是介绍串级PID控制四轴飞行状态-分析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
参考网页： http://blog.csdn.net/nemol1990/article/details/45131603
一、概念
单极PID：当你知道系统当前状态和期望状态后，如何将系统从当前状态调整到期望状态是个问题，在此我们可以用PID进行调整，PID分为位置式和增量式，位置式适合舵机等系统，在此使用的是增量式。
公式：PID=P*e(n)+I*[(e(n)+e(n-1)+…+e(0)]+D*[e(n)-e(n-1)]
D后面的当前误差减前次误差也可以直接使用陀螺仪的数据代替，原理一样。单极PID适合线性系统，当输出量和被控制量呈线性关系时单极PID能获得较好的效果，但是四轴不是线性系统，现代学者认为，四轴通常可以简化为一个二阶阻尼系统。为什么四轴不是线性系统呢？首先，输出的电压和电机转速不是呈正比的，其次，螺旋桨转速和升力是平方倍关系，故单极PID在四轴上很难取得很好效果，能飞，但是不好飞。
串级PID就是两个PID串在一起，分为内环和外环PID。在此，我们使用内环PID控制，外环PI控制。单极PID输入的是期望角度，反馈的是角度数据，串级PID中外环输入反馈的也是角度数据，内环输入反馈的便是角速度数据。通俗来讲，内环就是你希望将四轴以多少度每秒的速度运动，然后他给你纠正过来，外环就是根据角度偏差告诉内环你该以多少度一秒运动。这样，即使外环数据剧烈变化，四轴的效果也不会显得很僵硬。
在内环中，PID三个数据作用分别是：P（将四轴从偏差角速度纠正回期望角速度）D（抑制系统运动）I（消除角速度控制静差）
外环PI中，两个数据的作用是：P（将四轴从偏差角度纠正回期望角度）I（消除角度控制静差）
二、单环PID
或许有些朋友看得懂框图，但是编程实现有一定困难，在这里笔者给出了伪代码：
三、串级PID
伪代码：
外环PID
当前角度误差 = 期望角度 - 当前角度
外环PID_P项 = 外环Kp * 当前角度误差当前角度误差积分及其积分限幅
外环PID_I项 = 外环Ki * 当前角度误差积分外环PID输出 = 外环PID_P项 + 外环PID_I项
内环PID
当前角速度误差 = 外环PID输出 - 当前角速度（直接用陀螺仪输出）
内环PID_P项 = 内环Kp * 当前角速度误差当前角速度误差积分及其积分限幅
内环PID_I项 = 内环Ki * 当前角速度误差积分当前角速度微分（本次角速度误差 - 上次角速度误差）
内环PID_D项 = 内环Kd * 当前角速度的微分
内环PID输出 = 内环PID_P项 + 内环PID_I项 + 内环PID_D项
四、以一个最简单的stc的四轴代码为例分析飞行控制
代码
#include <STC15F2K60S2.h> //STC15系列通用头文件#include <intrins.h> //STC特殊命令符号声明#include <MPU6050.H> //MPU6050数字陀螺仪#include <STC15W4KPWM.H> //单片机所有IO口初始化-PWM口初始化//#include <EEPROM.h> //STC-EEPROM内部存储#include <NRF24L01.h> //NRF24L01 2.4G无线收发模块//#include <STC15W4K-ADC.h> //STC15W4K-ADC 硬件ADC模数转换#include <IMU.H> //IMU飞行核心算法//==================================================//// 外围引脚定义//==================================================//sbit LEDH3=P5^4; //四轴航向灯 1=灭 0=亮sbit LEDH4=P1^0; //四轴航向灯 1=灭 0=亮sbit LEDH1=P3^2; //四轴航向灯 1=灭 0=亮sbit LEDH2=P3^3; //四轴航向灯 1=灭 0=亮unsigned int LED\_mun=0; //==================================================// // 飞行控制变量 //==================================================// unsigned char JieSuo; //断开/连接 解锁变量 unsigned int YouMen; //油门变量 unsigned int HangXiang; //航向变量 unsigned int HengGun; //横滚变量 unsigned int FuYang; //俯仰变量 unsigned char FYHG; //俯仰横滚变量 unsigned char GaoDu; //高度变量 unsigned char DianYa; //电压变量 unsigned int ADC1,ADC2; //adc模数转换 10位 结果变量 unsigned char SSLL,lastR0,ZT; //通讯状态 变量 //==================================================// // 全局函数定义 //==================================================// unsigned char TxBuf[12]; //设置发送长度，最高为32字节 unsigned char RxBuf[12]; //设置接收长度，最高为32字节 //==================================================// // PID算法变量 //==================================================// //连至上位机检查电机轴是否发生弯曲，发现问题电机及时更换 unsigned int YM=0; //油门变化速度控制， int speed0=0,speed1=0,speed2=0,speed3=0;//电机速度参数 int PWM0=0,PWM1=0,PWM2=0,PWM3=0; //加载至PWM模块的参数 int g_x=0,g_y=0,g_z=0; //陀螺仪矫正参数 char a_x=0,a_y=0,a_z=0; //角度矫正参数 //*****************角度参数************************************************* float Last_Angle_gx=0; //外环PI输出量 上一次陀螺仪数据 float Last_Angle_gy=0; //外环PI输出量 上一次陀螺仪数据 double Gyro_y=0,Gyro_x=0,Gyro_z=0; //Y轴陀螺仪数据暂存 double Accel_x=0,Accel_y=0,Accel_z=0; //X轴加速度值暂存 double Angle_ax=0,Angle_ay=0,Angle_az=0; //由加速度计算的加速度(弧度制) double Angle_gy=0,Angle_gx=0,Angle_gz=0; //由角速度计算的角速率(角度制) double Anglezlate=0; //Z轴相关 int data AngleX=0,AngleY=0; //四元数解算出的欧拉角 double Ax=0,Ay=0;Az=0; //加入遥控器控制量后的角度 //****************姿态处理和PID********************************************* float FR1=0,FR2=0,FR3=0; //方向控制数据变量 float xdata gx=0,gy=0; //加入遥控器控制量后的角度 float data PID_Output; //PID最终输出量 float xdata ERRORX_In=0; //内环P 内环I 内环D 内环误差积分 float xdata ERRORX_Out=0; //外环P 外环I 外环误差积分float xdata ERRORY_In=0;float xdata ERRORY_Out=0;float xdata ERRORZ_Out=0;float xdata Last_Ax=0;float xdata Last_Ay=0;float xdata Last_gx=0;float xdata Last_gy=0;//==================================================//// PID 手动微调参数值//==================================================//// D值只要不超过10都可以，D值在3以上10以下!!! D值不合适飞机就会荡#define Out_XP 15.0f //外环P#define Out_XI 0.01f //外环I#define Out_XD 5.0f //外环D#define In_XP 0.55f //内环P 720#define In_XI 0.01f //内环I#define In_XD 3.0f //内环D 720#define In_YP In_XP#define In_YI In_XI#define In_YD In_XD#define Out_YP Out_XP#define Out_YI Out_XI#define Out_YD Out_XD//float ZP=5.0,ZD=4.0; //自旋控制的P D#define ZP 3.0f#define ZD 1.0f //自旋控制的P D#define ERR_MAX 800 ////--------------------------------------------------//// PID算法飞控自平衡 函数//--------------------------------------------------//void Flight(void)interrupt 1 { //陀螺仪水平校准-后期更新// Gyro_x = GetData(GYRO_XOUT_H)-g_x;//陀螺仪 值// Gyro_y = GetData(GYRO_YOUT_H)-g_y;//减掉 校正值// Gyro_z = GetData(GYRO_ZOUT_H)-g_z; // 读取MCU6050 寄存器数据Gyro_x = GetData(GYRO_XOUT_H);//读出 X轴陀螺仪数据Gyro_y = GetData(GYRO_YOUT_H);//读出 Y轴陀螺仪数据Gyro_z = GetData(GYRO_ZOUT_H);//读出 Z轴陀螺仪数据 Accel_y= GetData(ACCEL_YOUT_H);//读出 X轴加速度数据Accel_x= GetData(ACCEL_XOUT_H);//读出 Y轴陀螺仪数据 Accel_z= GetData(ACCEL_ZOUT_H);//读出 Z轴加速度数据 //姿态数据算法 （借鉴STC官方算法）Last_Angle_gx=Angle_gx; //储存上一次角速度数据Last_Angle_gy=Angle_gy; //储存上一次角速度数据Angle_ax=(Accel_x)/8192; //加速度处理Angle_az=(Accel_z)/8192; //加速度量程 +-4g/SAngle_ay=(Accel_y)/8192; //转换关系 8192LSB/gAngle_gx=(Gyro_x)/65.5; //陀螺仪处理Angle_gy=(Gyro_y)/65.5; //陀螺仪量程 +-500度/SAngle_gz=(Gyro_z)/65.5; //转换关系65.5LSB/度//***********************************四元数解算***********************************IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);//0.174533为PI/180 目的是将角度转弧度//****以下是飞行控制算法***********************************************************************YM=YouMen; //输入油门量0-1000if(SSLL == lastR0) //如果RxBuf[0]的数据没有收到 即失联{if(++ZT >= 100){ZT = 101; //状态标识大于128即1秒没有收到数据，失控保护RxBuf[4] = 128;RxBuf[5] = 128; //触发失控保护 ，缓慢下降，俯仰横滚方向舵归中RxBuf[6] = 128;if(YouMen != 0) //油门{YouMen--; //油门在原值逐渐减小YM=YouMen;TxBuf[2]=YM/0xff;//发送 油门参数 高2位 TxBuf[3]=YM%0xff;//发送 油门参数 低8位}}}else{ ZT = 0; }lastR0 = SSLL;//****以下是飞行控制算法***********************************************************************//************** MPU6050 X轴指向 ***********************************************************// FR1=0;//关闭横滚FR1=((float)HengGun-128)/6; //得到 横滚数据变量Ax=-FR1+a_x-AngleX; //角度控制量加载至角度if(YM > 30) ERRORX_Out += Ax; //外环积分(油门小于某个值时不积分)else ERRORX_Out = 0; //油门小于定值时清除积分值if(ERRORX_Out > ERR_MAX) ERRORX_Out = ERR_MAX; //积分限幅else if(ERRORX_Out < -ERR_MAX) ERRORX_Out = -ERR_MAX; //积分限幅PID_Output = Ax*Out_XP + ERRORX_Out*Out_XI+(Ax-Last_Ax)*Out_XD; //外环PIDLast_Ax=Ax;// if(YM > 20) ERRORX_In += (Angle_gy - PID_Output); //内环积分(油门小于某个值时不积分)gy=PID_Output - Angle_gy;if(YM > 30) ERRORX_In += gy; //内环积分(油门小于某个值时不积分)else ERRORX_In = 0; //油门小于定值时清除积分值if(ERRORX_In > ERR_MAX) ERRORX_In = ERR_MAX;else if(ERRORX_In < -ERR_MAX) ERRORX_In = -ERR_MAX; //积分限幅// PID_Output = (Angle_gy + PID_Output)*In_XP + ERRORX_In*In_XI + (Angle_gy - Last_Angle_gy)*In_XD; //内环PIDPID_Output = gy*In_XP + ERRORX_In*In_XI + (gy-Last_gy)*In_XD;Last_gy=gy;if(PID_Output > 1000) PID_Output = 1000; //输出量限幅if(PID_Output < -1000) PID_Output = -1000;speed0 = 0 + PID_Output; speed1 = 0 - PID_Output;speed3 = 0 + PID_Output; speed2 = 0 - PID_Output;//**************MPU6050 Y轴指向**************************************************FR2=((float)FuYang-128)/6; //得到 俯仰数据变量Ay=-FR2-a_y-AngleY; //角度控制量加载至角度if(YM > 30) ERRORY_Out += Ay; //外环积分(油门小于某个值时不积分)else ERRORY_Out = 0; //油门小于定值时清除积分值if(ERRORY_Out > ERR_MAX) ERRORY_Out = ERR_MAX;else if(ERRORY_Out < -ERR_MAX) ERRORY_Out = -ERR_MAX; //积分限幅PID_Output = Ay*Out_YP + ERRORY_Out*Out_YI+(Ay-Last_Ay)*Out_YD; //外环PIDLast_Ay=Ay;gx=PID_Output - Angle_gx;if(YM > 30)ERRORY_In +=gx; //内环积分(油门小于某个值时不积分)else ERRORY_In = 0; //油门小于定值时清除积分值if(ERRORY_In > ERR_MAX) ERRORY_In = ERR_MAX;else if(ERRORY_In < -ERR_MAX) ERRORY_In = -ERR_MAX; //积分限幅// PID_Output = (Angle_gx + PID_Output)*In_YP + ERRORY_In*In_YI + (Angle_gx - Last_Angle_gx)*In_YD; //内环PIDPID_Output = gx*In_YP + ERRORY_In*In_YI + (gx - Last_gx)*In_YD;Last_gx=gx;if(PID_Output > 1000) PID_Output = 1000; //输出量限幅if(PID_Output < -1000) PID_Output = -1000;speed0 = speed0 + PID_Output; speed1 = speed1 + PID_Output;//加载到速度参数speed3 = speed3 - PID_Output; speed2 = speed2 - PID_Output;//************** MPU6050 Z轴指向 ***************************** FR3=((float)HangXiang-128)*1.5;//得到 航向数据变量Az=FR3+a_z-Angle_gz;if(YM > 30) ERRORZ_Out += Az;else ERRORZ_Out = 0; if(ERRORZ_Out > 800) ERRORZ_Out = 800;else if(ERRORZ_Out < -800) ERRORZ_Out = -800; //积分限幅PID_Output = Az*ZP + ERRORZ_Out * 0.2f + (Az - Anglezlate) * ZD;Anglezlate = Az;speed0 = speed0 + PID_Output; speed1 = speed1 - PID_Output;speed3 = speed3 - PID_Output; speed2 = speed2 + PID_Output;//*****************PID控制数值--X机型控制***************************************************//**************将速度参数加载至PWM模块************************************************* //速度参数控制，防止超过PWM参数范围0-1000（X型有效）PWM0=(YM+speed1);if(PWM0>1000)PWM0=1000;else if(PWM0<0)PWM0=0; // 5PWM1=(YM+speed2);if(PWM1>1000)PWM1=1000;else if(PWM1<0)PWM1=0; // 3PWM2=(YM+speed0);if(PWM2>1000)PWM2=1000;else if(PWM2<0)PWM2=0; // 4PWM3=(YM+speed3);if(PWM3>1000)PWM3=1000;else if(PWM3<0)PWM3=0; // 2//满足条件：（解锁=1；2.4G=5；油门大于30）才能控制电机if(JieSuo==1&&YM>=30){PWM(1000-PWM0,1000-PWM1,1000-PWM2,1000-PWM3);} //启动PWM 2 3 4 5else {PWM(1000,1000,1000,1000);} //关闭PWM} //--------------------------------------------------//// 时间延时 函数//--------------------------------------------------//void Delay(unsigned int x){unsigned int i,j;for(i=0;i<x;i++)for(j=0;j<250;j++);}//--------------------------------------------------//// 定时器0 初始化函数 V2.0//--------------------------------------------------//void Timer0Init(void) //10微秒@27.000MHz{TR0 = 0;AUXR &= 0x7F; //定时器时钟12T模式TMOD &= 0xF0; //设置定时器模式IE = 0x82;TL0 = 0x1C; //设置定时初值TH0 = 0xA8; //设置定时初值TF0 = 0; //清除TF0标志TR0 = 1; //定时器0开始计时}//--------------------------------------------------//// 程序 主函数//--------------------------------------------------//void main(void){// unsigned char DCDY;//电池电压 变量unsigned char SHU_mun;/*----华丽的分割线----华丽的分割线----华丽的分割线----华丽的分割线----*/PWMGO(); //初始化PWMDelay(10); // 延时 100// InitADC(); //ADC模数转换 初始化Delay(10); // 延时 100InitMPU6050(); //初始化MPU-6050Delay(10); // 延时 100init_NRF24L01() ; //NRF24L01 初始化nRF24L01_RX_Mode(RxBuf);//数据接收Delay(10); // 延时 100Timer0Init(); //初始化定时器/*----华丽的分割线----华丽的分割线--- ---*/Delay(100); //延时一会 1SYouMen =0; //初始化油门变量 HangXiang=128; //初始化航向变量 HengGun =128; //初始化横滚变量 FuYang =128; //初始化俯仰变量/*----华丽的分割线----华丽的分割线----*/EA = 1; //开总中断PWM(1000,1000,1000,1000);while(1){ nRF24L01_TX_Mode(TxBuf);//数据发送Delay(1);nRF24L01_RX_Mode(RxBuf);//数据接收Delay(1);SSLL =RxBuf[0]; //接收 失联变量JieSuo =RxBuf[1]; //接收 命令值if(SSLL != lastR0) //如果RxBuf[0]的数据没有收到 即失联YouMen =RxBuf[2]*0xff+RxBuf[3]; //接收 油门变量HangXiang=RxBuf[4]; //接收 航向变量 HengGun =RxBuf[5]; //接收 横滚变量 FuYang =RxBuf[6]; //接收 俯仰变量// XXGG =RxBuf[7]; //接收 设置参数变量a_x =RxBuf[8]-128; //接收a_y =RxBuf[9]-128; //接收a_z =RxBuf[10]-128; //接收/*----华丽的分割线---*/ LED_mun++;if(JieSuo==1) //解锁飞机 SHU_mun{ if(LED_mun>600) LED_mun=0; if(LED_mun>150){LEDH1=0;LEDH2=0;LEDH3=0;LEDH4=0; //点亮航向灯}else{LEDH1=1;LEDH2=1;LEDH3=1;LEDH4=1; //关航向灯}if(SHU_mun==1){PWM(600,600,600,600);Delay(1000); // 延时 SHU_mun=0;}}else //锁上飞机{if(LED_mun<50){SHU_mun=1;LEDH1=1;LEDH2=1;LEDH3=0;LEDH4=1; //点亮1航向灯}else if(LED_mun<100){LEDH1=1;LEDH2=1;LEDH3=1;LEDH4=0; //点亮2航向灯}else if(LED_mun<150){LEDH1=1;LEDH2=0;LEDH3=1;LEDH4=1; //点亮3航向灯}else if(LED_mun<200){LEDH1=0;LEDH2=1;LEDH3=1;LEDH4=1; //点亮4航向灯}else{ LED_mun=0; }}/*----华丽的分割线----*/}}解析
HangXiang=RxBuf[4]; //接收 航向变量 HengGun =RxBuf[5]; //接收 横滚变量 FuYang =RxBuf[6]; //接收 俯仰变量 a_x =RxBuf[8]-128; //角度矫正参数 a_y =RxBuf[9]-128; //角度矫正参数 a_z =RxBuf[10]-128; //角度矫正参数 Gyro\_x = GetData(GYRO_XOUT_H);//读出 X轴陀螺仪数据 Gyro\_y = GetData(GYRO_YOUT_H);//读出 Y轴陀螺仪数据 Gyro\_z = GetData(GYRO_ZOUT_H);//读出 Z轴陀螺仪数据 Accel\_x= GetData(ACCEL_XOUT_H);//读出 X轴加速度数据 Accel\_y= GetData(ACCEL_YOUT_H);//读出 Y轴加速度数据 Accel\_z= GetData(ACCEL_ZOUT_H);//读出 Z轴加速度数据 YM=YouMen; //输入油门量0-1000
四元数解算
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){float idata norm;float idata vx, vy, vz;float idata ex, ey, ez;norm = sqrt(ax*ax + ay*ay + az*az); //把加速度计的三维向量转成单维向量 ax = ax / norm;ay = ay / norm;az = az / norm;// 下面是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。 // 根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素// 所以这里的vx vy vz，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的// 重力单位向量。vx = 2*(q1*q3 - q0*q2);vy = 2*(q0*q1 + q2*q3);vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;ex = (ay*vz - az*vy) ;ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki;eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;gx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;q1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT;q2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT;q3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT;norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;AngleX = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰 换算成度AngleY = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll // AngleY = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 横滚}
无人机失联-关闭油门
if(YouMen != 0) //油门 { YouMen--; //油门在原值逐渐减小 }
横滚
FR1=((float)HengGun-128)/6; //得到 横滚数据变量 Ax=-FR1+a_x-AngleX; //角度控制量加载至角度if(YM > 30) ERRORX_Out += Ax; //外环积分(油门小于某个值时不积分)else ERRORX_Out = 0; //油门小于定值时清除积分值if(ERRORX_Out > ERR_MAX) ERRORX_Out = ERR_MAX; //积分限幅不超过800else if(ERRORX_Out < -ERR_MAX) ERRORX_Out = -ERR_MAX; //积分限幅不小于-800 PID_Output = Ax*Out_XP + ERRORX_Out*Out_XI+(Ax-Last_Ax)*Out_XD; //外环PIDLast_Ax=Ax;gy=PID_Output - Angle_gy;if(YM > 30) ERRORX_In += gy; //内环积分(油门小于某个值时不积分)else ERRORX_In = 0; //油门小于定值时清除积分值if(ERRORX_In > ERR_MAX) ERRORX_In = ERR_MAX;else if(ERRORX_In < -ERR_MAX) ERRORX_In = -ERR_MAX; //积分限幅PID_Output = gy*In_XP + ERRORX_In*In_XI + (gy-Last_gy)*In_XD; //内环PIDLast_gy=gy;if(PID_Output > 1000) PID_Output = 1000; //输出量限幅if(PID_Output < -1000) PID_Output = -1000;speed0 = 0 + PID_Output; speed1 = 0 - PID_Output;speed3 = 0 + PID_Output; speed2 = 0 - PID_Output;
俯仰
FR2=((float)FuYang-128)/6; //得到 俯仰数据变量Ay=-FR2-a_y-AngleY; //角度控制量加载至角度if(YM > 30) ERRORY_Out += Ay; //外环积分(油门小于某个值时不积分)else ERRORY_Out = 0; //油门小于定值时清除积分值if(ERRORY_Out > ERR_MAX) ERRORY_Out = ERR_MAX;else if(ERRORY_Out < -ERR_MAX) ERRORY_Out = -ERR_MAX; //积分限幅PID_Output = Ay*Out_YP + ERRORY_Out*Out_YI+(Ay-Last_Ay)*Out_YD; //外环PIDLast_Ay=Ay;gx=PID_Output - Angle_gx;if(YM > 30)ERRORY_In +=gx; //内环积分(油门小于某个值时不积分)else ERRORY_In = 0; //油门小于定值时清除积分值if(ERRORY_In > ERR_MAX) ERRORY_In = ERR_MAX;else if(ERRORY_In < -ERR_MAX) ERRORY_In = -ERR_MAX; //积分限幅PID_Output = gx*In_YP + ERRORY_In*In_YI + (gx - Last_gx)*In_YD;Last_gx=gx;if(PID_Output > 1000) PID_Output = 1000; //输出量限幅if(PID_Output < -1000) PID_Output = -1000;speed0 = speed0 + PID_Output; speed1 = speed1 + PID_Output;//加载到速度参数speed3 = speed3 - PID_Output; speed2 = speed2 - PID_Output;
航向
FR3=((float)HangXiang-128)*1.5;//得到 航向数据变量Az=FR3+a_z-Angle_gz;if(YM > 30) ERRORZ_Out += Az;else ERRORZ_Out = 0; if(ERRORZ_Out > 800) ERRORZ_Out = 800;else if(ERRORZ_Out < -800) ERRORZ_Out = -800; //积分限幅PID_Output = Az*ZP + ERRORZ_Out * 0.2f + (Az - Anglezlate) * ZD;Anglezlate = Az;speed0 = speed0 + PID_Output; speed1 = speed1 - PID_Output;speed3 = speed3 - PID_Output; speed2 = speed2 + PID_Output;
将速度参数加载至PWM模块
PWM0=(YM+speed1);if(PWM0>1000)PWM0=1000;else if(PWM0<0)PWM0=0; // 5PWM1=(YM+speed2);if(PWM1>1000)PWM1=1000;else if(PWM1<0)PWM1=0; // 3PWM2=(YM+speed0);if(PWM2>1000)PWM2=1000;else if(PWM2<0)PWM2=0; // 4PWM3=(YM+speed3);if(PWM3>1000)PWM3=1000;else if(PWM3<0)PWM3=0; // 2//满足条件：（解锁=1；2.4G=5；油门大于30）才能控制电机if(JieSuo==1&&YM>=30){PWM(1000-PWM0,1000-PWM1,1000-PWM2,1000-PWM3);} //启动PWM 2 3 4 5else {PWM(1000,1000,1000,1000);} //关闭PWM
这篇关于串级PID控制四轴飞行状态-分析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
