参考网页: 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/S Angle_ay=(Accel_y)/8192; //转换关系 8192LSB/g Angle_gx=(Gyro_x)/65.5; //陀螺仪处理 Angle_gy=(Gyro_y)/65.5; //陀螺仪量程 +-500度/S Angle_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-1000 if(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; //外环PID Last_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; //内环PID PID_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; //外环PID Last_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; //内环PID 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; //************** 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; // 5 PWM1=(YM+speed2);if(PWM1>1000)PWM1=1000;else if(PWM1<0)PWM1=0; // 3 PWM2=(YM+speed0);if(PWM2>1000)PWM2=1000;else if(PWM2<0)PWM2=0; // 4 PWM3=(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 5 else {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(); //初始化PWM Delay(10); // 延时 100 // InitADC(); //ADC模数转换 初始化 Delay(10); // 延时 100 InitMPU6050(); //初始化MPU-6050 Delay(10); // 延时 100 init_NRF24L01() ; //NRF24L01 初始化 nRF24L01_RX_Mode(RxBuf);//数据接收 Delay(10); // 延时 100 Timer0Init(); //初始化定时器 /*----华丽的分割线----华丽的分割线--- ---*/ Delay(100); //延时一会 1S YouMen =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; //积分限幅不超过800
else if(ERRORX_Out < -ERR_MAX) ERRORX_Out = -ERR_MAX; //积分限幅不小于-800
PID_Output = Ax*Out_XP + ERRORX_Out*Out_XI+(Ax-Last_Ax)*Out_XD; //外环PID
Last_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; //内环PID
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;
俯仰
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; //外环PID
Last_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; // 5
PWM1=(YM+speed2);if(PWM1>1000)PWM1=1000;else if(PWM1<0)PWM1=0; // 3
PWM2=(YM+speed0);if(PWM2>1000)PWM2=1000;else if(PWM2<0)PWM2=0; // 4
PWM3=(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 5
else
{PWM(1000,1000,1000,1000);} //关闭PWM