四旋翼自主垂直起降飞行器设计本科毕业论文.doc

四旋翼自主垂直起降飞行器设计
摘要：四旋翼飞行器是一种结构新颖、性能优越的垂直起降飞行器，具有操作灵活、带负载能力强等特点，具有重要的军事和民用价值，以及研究价值。在深入了解四旋翼飞行器的研究现状、关键技术与应用前景的基础之上，根据四旋翼飞行器飞行原理，建立系统动力学模型，确定了系统组成和总体设计方案。
首先根据设计方案采购了简单飞行器机体模型，选择合适的直流无刷电机作为系统动力装置，设计线性度良好、功率满足一定要求的无刷电机驱动器，以满足四旋翼飞行器带载飞行需要。选取了功能强大且容易开发的微处理器、传感器和相关电子元器件满足系统需要，并做了大量的系统软硬件调试工作，最终完成了整体设计。飞行器运动姿态测量单元主要由惯性测量单元(IMU)和三轴磁传感器组成，根据其传感器的性能指标，构建数学模型，从而得到载体准确的航向信息和相应姿势，为飞行器系统稳定飞行提供重要的保障。 四旋翼无人飞行器要实现稳定飞行是以平衡控制为前提的，由于该飞行器具有六自由度而只有四个控制量的欠驱动的控制系统，通过调节四个电机转速实现飞行器平衡稳定飞行，因此平衡控制是四旋翼飞行器运动中的关键。根据系统动力学模型设计控制算法，设计四旋翼飞行器控制系统控制规律，主要包括两个控制回路：姿态控制回路、位置控制回路。在仿真软件平台上，通过仿真验证后将算法移植到处理器中，进行控制算法验证及实验研究，优化飞行控制算法参数。最后，为了满足四旋翼无人飞行器的稳定飞行控制的要求，设计实时性高的控制系统软件程序，进行相关实验调试工作。最终设计出能够实现一键飞行、高效的四旋翼自主飞行器。
关键词：四旋翼飞行器 瑞萨R5F100LEA PWM 陀螺仪 超声传感器

Abstract
Four rotor aircraft is a kind of novel structure, superior performance of vertical take-off and landing aircraft, has many characteristics, such as flexible operation and load ability, has the important value of military and civilian, and research value. In understanding the research status of four rotor aircraft, the key technology and the application prospect, according to the principle of four rotor aircraft flight, establish a system dynamics model, the system composition and the overall design scheme is determined. First according to the design plan to purchase the simple vehicle body model, select the appropriate brushless dc motor as the power unit system, design good linearity and power meet certain requirements of brushless motor drive, to meet the needs of the four rotor aircraft flying on load. Selected the powerful and easy development of microprocessors, sensors and related electronic components meet the needs of the system, and made a lot of system hardware and software debugging, finally completed the overall design. Aircraft motion measurement unit is mainly composed of inertial measurement unit (IMU) and a three-axis magnetic sensor, according to the sensor performance index, build the mathematical model of the carrier to get the exact course information and the corresponding position, provide important guarantee for aircraft flying system stable. Four rotor unmanned aerial vehicles to achieve stable flight is the premise of the balance control, due to the aircraft has six degrees of freedom only four control of underactuated control system, by adjusting the motor speed to achieve four aircraft flying balanced and stable, so the balance control is key in the four rotor aircraft movement. On the simulation software platform, through the simulation verify the algorithm after transplantation into the processor, verifies the control algorithm and experimental research, optimization of flight control algorithm parameters. Finally, in order to meet the requirements of the stability of the four rotor unmanned spacecraft flight control, high real-time performance of control system software design, carries on the related experimental debugging. A key to achieve ultimately designed to fly, high efficient and automatic four rotor aircraft.
Keywords: four rotor aircraft renesas R5F100LEA PWM gyroscope
ultrasonic sensors

目 录
一、系统方案比较与设计 6
二、单元电路设计与计算 7
1. 四旋翼飞行器飞行模式 7
2. 系统硬件芯片的选择 13
3、飞行姿态的测量 15
4、超声波传感器高度控制 15
5、电源模块 16
6、硬件电路图 16
三、程序设计 17
四、系统测试 18
1、基本部分测试 18
2、测试方法 18
3、测试结果 18
五、结论 19
六、参考文献 19
七、附录 20
1、仪器设备清单 20
2、程序清单 20
1主函数 20
3、子函数 21
一、系统方案比较与设计
1.方案一：以瑞萨R5F100LEA为核心控制超声波测距系统，通过主控芯片记录超声波发射的时间和收到反射波的时间，当收到超声波的反射波时，接受电路利用PWM调制法，使飞行器根据不同高度，控制脉冲占空比，从而控制速度。该方法较易实现，仅由电机直接驱动，电路简单，利于一键起飞。
2.方案二：考虑以上方案，受外力影响大，稍有倾斜，飞行器将失去平衡。为了达到控制飞行器平衡稳定飞行，外加 MPU6050模块，MPU-6050中陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为数字量，这些数字量输入到主控芯片，结合精密算法，调节脉冲占空比，调节四个电机转速来改变旋翼转速，实现升力的变化，从而控制飞行器的姿态和位置。
通过比较两种方案，确定了使用方案二，虽然算法复杂，但困难是可能被克服的，能够完成基本任务；而方案一，算法粗糙，一旦采用，面临的各种问题是难以想象的，所以确立为方案二。
二、单元电路设计与计算
1、带防撞的四旋翼飞行器
1.1 四旋翼飞行器飞行模式
四旋翼飞行器是一种由固连在刚性十字交叉结构上的4个电机驱动的一种飞行器。飞行器动作依靠4个电机的转速差进行控制，其机械结构相对简单，可由电机直接驱动，无需复杂的传动装置，便于微型化。相较于典型的传统直升机，仅仅装配有一个主转子和一个尾桨，通过控制舵机来改变螺旋桨的桨距角，从而控制直升机的姿态和位置的飞行模式，四旋翼飞行器是通过调节四个电机转速来改变旋翼转速，实现升力的变化，从而控制飞行器的姿态和位置。由于飞行器是通过改变旋翼转速实现升力变化，按照旋翼布置方式可分为十字模式和X模式，如图l所示，四旋翼飞行器只有四个输入力，同时却有六个状态输出，是一种六自由度的垂直升降机，因此这种欠驱动系统非常适合静态和准静态条件下飞行。 对于姿态测量和控制的编程算法来说，十字模式较X模式简单，前者的飞行模式是通过只改变单个机翼转速的方法而完成前后左右四个方向的飞行；而后者的飞行模式是通过同时控制两个机翼转速的方法而完成前后左右四个方向的飞行。十字模式算法简单，容易操作，飞行平稳。X模式算法复杂，可操作性不强。综上所述，采用了十字模式算法。
前

图1 十字模式算法

1.2四旋翼飞行器产生基本动作的原理
四旋翼飞行器结构形式如图2所示，电机1 和电机3 逆时针旋转的同时，电机2 和电机4 顺时针旋转，因此当飞行器平衡飞行时，陀螺效应和空气动力扭矩效应均被抵消。与传统的直升机相比，四旋翼飞行器有下列优势：各个旋翼对机身所施加的反扭矩与旋翼的旋转方向相反，因此当电机1 和电机3逆时针旋转的同时，电机2和电机4顺时针旋转，可以平衡旋翼对机身的反扭矩。 四旋翼飞行器在空间共有6个自由度（分别沿3个坐标轴作平移和旋转动作），这6个自由度的控制都可以通过调节不同电机的转速来实现。

图2 四旋翼飞行器的结构形式
基本运动状态分别是：<1>垂直运动；<2>俯仰运动；<3>滚转运动；<4>偏航运动；<5>前后运动；<6>侧向运动。在图3中，电机1和电机3作逆时针旋转，电机2和电机4作顺时针旋转，规定沿x轴正方向运动称为向前运动，箭头在旋翼的运动平面上方表示此电机转速提高，在下方表示此电机转速下降。图3（a）垂直运动：垂直运动相对来说比较容易。在图中，因有两对电机转向相反，可以平衡其对机身的反扭矩，当同时增加四个电机的输出功率，旋翼转速增加使得总的拉力增大，当总拉力足以克服整机的重量时，四旋翼飞行器便离地垂直上升；反之，同时减小四个电机的输出功率，四旋翼飞行器则垂直下降，直至平衡落地，实现了沿z轴的垂直运动。当外界扰动量为零时，在旋翼产生的升力等于飞行器的自重时，飞行器便保持悬停状态。保证四个旋翼转速同步增加或减小是垂直运动的关键。 图3（b）俯仰运动：电机1的转速上升，电机3的转速下降，电机2、电机4的转速保持不变。为了不因为旋翼转速的改变引起四旋翼飞行器整体扭矩及总拉力改变，旋翼1与旋翼3转速该变量的大小应相等。由于旋翼1的升力上升，旋翼3的升力下降，产生的不平衡力矩使机身绕y轴旋转（方向如图所示），同理，当电机1的转速下降，电机3的转速上升，机身便绕y轴向另一个方向旋转，
实现飞行器的俯仰运动。 图3（c）滚转运动：与图b的原理相同，在图c中，改变电机2和电机4的转速，保持电机1和电机3的转速不变，则可使机身绕x轴旋转（正向和反向），实现飞行器的滚转运动。 图3（d）偏航运动：四旋翼飞行器偏航运动可以借助旋翼产生的反扭矩来实现。旋翼转动过程中由于空气阻力作用会形成与转动方向相反的反扭矩，为了克服反扭矩影响，可使四个旋翼中的两个正转，两个反转，且对角线上的来年各个旋翼转动方向相同。反扭矩的大小与旋翼转速有关，当四个电机转速相同时，四个旋翼产生的反扭矩相互平衡，四旋翼飞行器不发生转动；当四个电机转速不完全相同时，不平衡的反扭矩会引起四旋翼飞行器转动。在图d中，当电机1和电机3的转速上升，电机2和电机4的转速下降时，旋翼1和旋翼3对机身的反扭矩大于旋翼2和旋翼4对机身的反扭矩，机身便在富余反扭矩的作用下绕z轴转动，实现飞行器的偏航运动，转向与电机1、电机3的转向相反。 图3（e）前后运动：要想实现飞行器在水平面内前后、左右的运动，必须在水平面内对飞行器施加一定的力。在图e中，增加电机3转速，使拉力增大，相应减小电机1转速，使拉力减小，同时保持其它两个电机转速不变，反扭矩仍然要保持平衡。按图b的理论，飞行器首先发生一定程度的倾斜，从而使旋翼拉力产生水平分量，因此可以实现飞行器的前飞运动。向后飞行与向前飞行正好相反。当然在图b图c中，飞行器在产生俯仰、翻滚运动的同时也会产生沿x、y轴的水平运动。 图3（f）倾向运动：由于结构对称，所以倾向飞行的工作原理与前后运动完全一样。

图3 四旋翼飞行器沿各自由度的运动
1.3 四旋翼直升机的动力学原理
不像普通的直升机有可变螺距角度，四旋翼直升机由固定俯仰角转子和转子速度控制，以产生预期的升力。四旋翼的基本运动可以用图4来描述。

图4 四旋翼三维受力图
直升机的垂直运动可以在同一时间通过改变所有的转子速度来实现。沿着X轴的运动与在Y轴方向上倾斜有关。这种倾斜可通过降低旋翼1、2的速度，增加旋翼3、4的速度来实现。这种倾斜也可以沿X轴的加速度。类似的，沿着Y轴的运动与在X轴方向上倾斜有关。偏航运动是利用旋翼产生的力矩来实现的。常规直升机有尾桨，以平衡由主旋翼产生的力矩。但在四旋翼的情况下，旋翼的旋转方向是用来平衡和减少这些力矩的。这也可以被用来产生预期的偏航运动。为了在顺时针方向上转向，必须增加旋翼2、4的速度以克服旋翼1和3产生的力矩。因此一个好的控制器应能达到预期的偏航角，同时保持固定的倾斜角度和高度。假设一个固定在支架上的结构在该直升机重心处，此处Z轴是指向上的，它的身体轴心是与惯性系的位置矢量（x,y,z)和三个分别代表了俯仰滚转与偏航的欧拉角（θ，Ψ，Φ），有关的式1用来表示旋 转
在上式中和分别表示了和
每个旋翼产生的力矩相当于纵向的力。这些力矩已通过实验观察到在低速情况下与力是呈线性的。这里有4个输入的力与6个输出的参数（）因此直升机是一个欠驱动系统。两个旋翼的旋转方向是顺时针方向，另外两个是逆时针的，为了平衡力矩并且实现所需要的偏航运动。根据力和力矩平衡所得方程如下：

上面的是阻尼系数，接下来我们假设阻力为0，因为阻力在低速时可以忽略的。为了简单起见，我们定义输入为：

是对于轴的转动惯量，是力与力矩的比例因子。
表示了在Z轴方向上的加速度，、表示the roll and pitch inputs
表示了一个偏航力矩。因此运动方程变成了：

假设重心在对角线的交点处，若重心向上（或向下）移动d单位，则角速度对力更不敏感，从而稳定性增加了。使旋翼的力指向中心同样增加稳定性，同时减少
The roll and pitch moments和垂直方向上的总推力。
2. 系统硬件芯片的选择
2.1 飞行器控制板主控芯片R5F100LEA，如图5所示。

图5 主控芯片R5F100LEA引脚图
主控芯片R5F100LEA是瑞萨公司生产的一款单片机芯片，采用64引脚LQFP封装，64KB的ROM容量，拥有Flash存储器。本产品相较于C8051有以下特点：拥有15个端口，采用引脚复用方式，既节省了引脚资源，又拓展了功能。适合在复杂应用且微型飞行器上应用。
2.2 MPU 6050 如图6

图6 MPU6050引脚图
MPU-6050为全球首例整合性6轴运动处理组件，采用最小最薄包装 (4x4x0.9mm QFN) 符合RoHS及环境标准，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量的包装空间。MPU-6000（6050）整合了3轴陀螺仪、3轴加速器，并含可藉由第二个I2C端口连接其他厂牌之加速器、磁力传感器、或其他传感器的数位运动处理(DMP: Digital Motion Processor)硬件加速引擎，由主要I2C端口以单一数据流的形式，具有向应用端输出完整的9轴融合演算技术InvenSense的运动处理资料库，可处理运动感测的复杂数据，降低了运动处理运算对操作系统的负荷，并为应用开发提供架构化的API等功能。MPU-6000（6050）的角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps)，可准确追踪快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为±2g、±4g±8g与±16g。产品传输可透过最高至400kHz的IC或最高达20MHz的SPI（MPU-6050没有SPI）。MPU-6000可在不同电压下工作，VDD供电电压介为2.5V±5%、3.0V±5%或3.3V±5%，逻辑接口VVDIO供电为1.8V± 5%（MPU6000仅用VDD）。MPU-6050中陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为数字量。
2.3 HC-SR04 如图7

图7 HC-SR04实物图
HC-SR04超声波测距模块包括超声波发射器、接收器与控制电路，性能稳定，测度距离精确，模块高精度，盲区小。具有可提供2cm-400cm的非接触式距离感测功能，测距精度可达高到3mm。基本工作原理：(1)采用IO口TRIG触发测距，给至少10us的高电平信号;(2)模块自动发送8个40khz的方波，自动检测是否有信号返回；(3)有信号返回，通过IO口ECHO输出一个高电平，高电平持续的时间就是超声波从发射到返回的时间。测试距离=(高电平时间*声速(340M/S))/2。
3、飞行姿态的测量
对无人机的控制来说测量无人机的飞行姿态是势必不可少。我们用一个惯性测量装置（陀螺仪mpu6050）来执行这一任务。这架旋翼机的姿态是确定利用互补滤波器为每个旋转轴。该过滤器的工程计算误差信号之间的估计角（），并参考角（）在直接从加速度计算。在积分前，这个错误信号减去了初始角速度信号。应该指出的是，比率传感器可以测量在体内固定轴，而加速度测量倾斜，在地球上的固定轴。
4、超声波传感器高度控制
超声波传感器高度控制是通过PID闭环控制算法实现的，这种控制采
用闭合反馈，通过超声波传感器发射的时间和收到反射波的时间差，计算距离，反馈给主控芯片，同时控制四个电机的转速。PID是比例(P)、积分(I)、微分(D)控制算法。但并不是必须同时具备这三种算法，也可以是PD,PI,甚至可以只有P算法控制。采用了以上三种算法比例(P)、积分(I)、微分(D)控制超声波传感器反馈的脉冲，以控制四个电机的转速，越过障碍。积分，反应系统的累计偏差，使系统消除稳态误差，提高无差度，因为有差，积分调节就进行，直至无误差；微分，具有预见性，能预见偏差变化的趋势，产生超前的控制作用，在偏差还没有形成之前，已被微分调节作用消除，因此可以改善系统的动态性能。但是微分对噪声干扰有放大作用，加强微分对系统抗干扰不利积分和微分都不能单独起作用，必须与比例控制配合。采用比例控制规律能较快地克服扰动的影响，它的作用于输出值较快，但不能很好稳定在一个理想的数值，不良的结果是虽较能有效的克服扰动的影响，但有余差出现。
5、电源模块
为四旋翼飞行器飞行控制系统、无刷电机等提供电源。由于四旋翼飞行器载重量有限，所以电源系统应该尽可能 轻，能量密度大。考虑到线性稳压方式转换效率不高，能量损耗大的缺点，本系统改用开关电源的方式来稳压。但开关电源会产生较强的电磁干扰，而且存在电源波纹，这样对PPM的遥控方式影响就很大。
6、硬件电路图
整体连接电路如图8

图8 硬件连接电路图

三、程序设计
程序流程图 如图9

图 9 流程图

四、系统测试
1、基本部分测试
测试仪器：秒表、数字万用表、示波器、R5F100LEA仿真机等。
2、测试方法：
数字万用表主要用来测试分立元件的电阻、压降、漏电流、截止/导通状态等参数；
示波器用于测试各PWM波形；
秒表用于产品测试，按照基本要求对制成的四旋翼飞行器进行飞行时间的测量；
R5F100LEA仿真器用来测试软件；
3、测试结果：
测试结果显示本系统以实现需求中基本功能，能满足在时间限度里飞行所要求的距离，不足之处在于系统功能相对不够完善，在传感器模块还需要进行完善。此次测试目的在于发现系统缺陷与漏洞，从测试结果中没有发现严重系统缺陷。
五、结论
本系统完成了硬件电路的设计并结合了软件开发程序，使四旋翼飞行器能够一键启动后，平衡稳定的飞行任务。通过各种方案的讨论和尝试，在经过多次的整体软硬件结合调试，不断地对系统进行优化，设计采用简单的动力学原理结合软件开发平台，就能够简单高效的完成任务，若采用其他类型的功能传感器，功能会更加强大，且若将算法设计精准，功能完成会更加流畅。
六、参考文献
黄智伟.全国大学生电子设计竞赛训练教程.电子工业出版社，2010
李晓林.单片机原理与接口技术.电子工业出版社，2010
黄智伟.全国大学生电子设计竞赛训练教程.电子工业出版社，2010
黄智伟.全国大学生电子设计竞赛训练教程.电子工业出版社，2010
J.C Zufferey, D. Floreano Evolving Vision-Based Flying Robots. Proceedings of the 2nd International Workshop on Biologically Motivated Computer Vision, LNCS 2525, pp. 592-600, Berlin, Springer-Verlag,2002.
A. Elfes, S.S. Bueno and al, Robotic Airship for Exploration of planetary Bodies with an Atmosphere Autonomy Challenges. Autonomous Robots Journal: Kluwer Academic Publishers, 2003.
S. Sastry, A mathematical introduction to robotic manipulation . Boca Raton, FL, 1994.
P. Mllhaupt, Analysis and Control of Underactuated Mechanical Nonminimum-phase Systems. Phd thesis, Department of MechanicalEngineering, EPFL, 1999.

七、附录
1、仪器设备清单
序号
名称
型号
数量
备注
1
四旋翼飞行器

1

2
MCU
R5F100LEA
1
16位微处理器
3
陀螺仪
MPU-6050
1

4
超声传感器
HC-SR04
1

2、程序清单
1主函数
void main(void)
{
R_MAIN_UserInit();
/* Start user code. Do not edit comment generated here */
/*????????*/
while(get_systemtimer()<5000); //wait 5 sec
//clr_systemtimer();

curatt_init();
power=750;
delaytime=0;
while (1U)
{
task_mpu6050();
if(OK==get_mpu6050_mode())
{
atttitude_calculate(4000,fgyrox,fgyroy,fgyroz);
if(flag!=0)
{
flag=0;
}
else
{
flag=1;
q4_to_euler_rpy(&curattX,&currrpy);
euler_control(&currrpy,&target);
}
control_task(power);
delaytime++;
if(delaytime>500)
{
delaytime=600;
//power=0;
}
}
;
}
/* End user code. Do not edit comment generated here */
}

3、子函数
1.
#include "mytype.h"
#include "math.h"

Q4 curattX={1,0,0,0};
RPY currrpy={0,0,0};
float math_rsqrt(float number)
{

long i;
float x2, y;

x2 = number * 0.5F;
y = number;
i = * ( long * ) &y; // evil floating point bit level hacking
i = 0x5f3759df - ( i >> 1 ); // what the fuck
y = * ( float * ) &i;
y = y * ( 1.5F - ( x2 * y * y ) ); // 1st iteration

return y;
}

void curatt_init(void)
{
curattX.w = 1;
curattX.x = 0;
curattX.y = 0;
curattX.z = 0;
}

void atttitude_calculate(unsigned int timer_us,float gx,float gy,float gz)
{
float a_rsqrt;
float qa,qb,qc;
float timesec=timer_us*0.000001;
gx *= (0.5f * timesec); // pre-multiply common factors
gy *= (0.5f * timesec);
gz *= (0.5f * timesec);
qa = curattX.w;
qb = curattX.x;
qc = curattX.y;
curattX.w += (-qb * gx - qc * gy - curattX.z * gz);
curattX.x += (qa * gx + qc * gz - curattX.z * gy);
curattX.y += (qa * gy - qb * gz + curattX.z * gx);
curattX.z += (qa * gz + qb * gy - qc * gx);

a_rsqrt = math_rsqrt(curattX.w*curattX.w + curattX.x*curattX.x + curattX.y*curattX.y + curattX.z*curattX.z);
curattX.w *= a_rsqrt;
curattX.x *= a_rsqrt;
curattX.y *= a_rsqrt;
curattX.z *= a_rsqrt;
}

void q4_to_euler_rpy(Q4 *dat,RPY* ang)
{
float gx, gy, gz,x_2,w_2;

gx = 2 * (dat->x*dat->z - dat->w*dat->y);
gy = 2 * (dat->w*dat->x + dat->y*dat->z);
x_2 = dat->x*dat->x;
w_2 = dat->w*dat->w;
gz = w_2 -x_2 - dat->y*dat->y + dat->z*dat->z;
gz*= gz;

ang->yaw = -1*atan2(2*dat->x*dat->y - 2*dat->w*dat->z, 2*w_2 + 2*x_2 - 1);// * 180 / PI;
ang->roll =-1* atan(gx* math_rsqrt(gy*gy + gz));// * 180 / PI;
ang->pitch = atan(gy* math_rsqrt(gx*gx + gz));// * 180 / PI;
}
2.
static void updat_pid(PIDP* pid)
{
float ilimit1=pid->ilimit*(-1);
pid->istate+=(pid->errdat)*pid->iGain;
if(pid->istate>pid->ilimit)pid->istate=pid->ilimit;
if(pid->istate<ilimit1)pid->istate=ilimit1;
pid->pidout = pid->pGain*pid->errdat+pid->istate+(pid->errdat-pid->perr)*pid->dGain; pid->perr=pid->errdat;
}

static void pid_init(PIDP* pid)
{
pid->istate=0;
pid->perr=0;
pid->errdat=0;
pid->pidout=0;
}

void control_init(void)
{
curatt_init();
check_mpu6050();
pid_init(&Xaxespid);
pid_init(&Yaxespid);
pid_init(&Zaxespid);
pid_init(&XGpid);
pid_init(&YGpid);
pid_init(&ZGpid);
}

/*
功能：X型 马达分配驱动分配

*/
enum{
L_F,
L_B,
R_B,
R_F
};

static void x_mode_updat_motodat(S16 basedat,S16 Xaxesdat,S16 Yaxesdat,S16 Zaxesdat)
{
S16 pwm[4];
pwm[L_F]=basedat + Xaxesdat + Yaxesdat - Zaxesdat;
pwm[L_B]=basedat - Xaxesdat + Yaxesdat + Zaxesdat;
pwm[R_B]=basedat - Xaxesdat - Yaxesdat - Zaxesdat;
pwm[R_F]=basedat + Xaxesdat - Yaxesdat + Zaxesdat;
updat_pwm(pwm);
};

static void gpid_pro_xmode(float Xpidout,float Ypidout,float Zpidout)
{
XGpid.errdat = Xpidout - fgyrox;
YGpid.errdat = Ypidout - fgyroy;
ZGpid.errdat = Zpidout - fgyroz;
updat_pid(&XGpid);
updat_pid(&YGpid);
updat_pid(&ZGpid);
}

/*
功能：欧拉角控制 角度单环控制
*/
#define MAX_ERR 0.5
U8 euler_control(RPY *cureuler,RPY *target)
{

/*取误差*/
Zaxespid.errdat = (target->yaw) - cureuler->yaw;
Yaxespid.errdat = (target->roll) - cureuler->roll;
Xaxespid.errdat = (target->pitch) - cureuler->pitch;
/*限制误差*/
if(Zaxespid.errdat>MAX_ERR)Zaxespid.errdat=MAX_ERR;
else if(Zaxespid.errdat<-MAX_ERR)Zaxespid.errdat=-MAX_ERR;
if(Yaxespid.errdat>MAX_ERR)Yaxespid.errdat=MAX_ERR;
else if(Yaxespid.errdat<-MAX_ERR)Yaxespid.errdat=-MAX_ERR;
if(Xaxespid.errdat>MAX_ERR)Xaxespid.errdat=MAX_ERR;
else if(Xaxespid.errdat<-MAX_ERR)Xaxe