单片机应用技术实训设计报告
（四轴飞行器）
所属系部： 电气信息工程系
班 级： （2014）通信技术1班
姓 名： 彭世秋
指导老师： 施 芸
摘 要
为了满足四轴飞行器的设计要求，设计了以微控制器为核心的控制系统和算法。首先进行了各单元电路方案的比较论证，确定了硬件设计方案。四轴飞行器采用了固连在刚性十字架交叉结构上的4个无刷电机驱动的一种飞行器，以ARM公司的Context-M3 CPU內核为基础,围绕新的Context-M3 CPU內核演化而来的意法半导体公司的STM32F103高容量单片机作为控制核心，工作频率高达72MHz，标准工作电压3.3V，适合各种类型的消费类电子和工业应用, 该控制器为32位低功耗单片机，有助于降低系统功耗，削减总系统的构建成本。采用电调驱动无刷电机，且具有控制简单、功率大、带5V电源输出等特性。通过采用MPU-6050整合的3轴陀螺仪、3轴加速器，并含可藉由第二个I2C端口连接其他厂牌之加速器、磁力传感器、或其他传感器的数位运动处理(DMP: Digital Motion Processor)硬件加速引擎，而本系统采用的是利用MPU-6050内部DMP计算出四元数，通过公式转换为欧拉角，通过串级PID计算出PWM增量，实时调节四轴飞行器的姿态，串级PID具有调试简单，效率高，稳定性好等优点。依靠蓝牙、NRF24L模块和四轴飞行器进行数据交换。
关键词：四轴飞行器，MPU-6050，STM32F103，串级PID，欧拉角 , DMP ，四元数
目 录
目录
一、系统方案论证 4
1.1 姿态模块的论证与选择 4
1.2 电源模块的论证与选择 4
1.3飞行方式的论证与选择 5
1.4 无刷电机驱动模块的论证与选择 5
1.5 单片机的选择 6
1.6 遥控方式的选择 6
二、系统理论分析与计算 7
2.1 串级PID控制算法的分析 7
2.1.1串级PID理论分析 7
2.1.2 串级PID整定方法的分析 7
2.1.3 串级PID框图 8
2.2 欧拉角的理论与计算 8
2.2.1 欧拉角的理论分析 8
2.2.2 四元数计算出欧拉角的理论分析 9
2.2.3 四元数计算出欧拉角的程序设计 10
2.2.4 硬件加速引擎（DMP）理论分析 12
2.3 四轴飞行器和飞行结构分析 13
2.3.1 结构形式 13
2.3.2 工作原理 14
三、电路与程序设计 17
3.1 电路的设计 17
3.1.1 系统总体框图 17
3.1.2 MPU6050电路原理图 17
3.1.3电机驱动系统框图 18
3.1.4电源模块 18
3.1.5 单片机最小系统电路设计 19
3.1.6 电调连接原理图 19
3.2程序的设计 20
3.2.1程序功能描述与设计思路 20
3.2.2程序流程图 21
四、测试方案与测试结果 22
4.1测试方案 22
4.2 测试结果及分析 22
4.2.1测试分析与结论 22
五、专用周总结 23
六、参考资料 25
六、附录： 26
一、系统方案论证
系统主要由单片机控制模块、姿态采集模块、电源模块、电机驱动模块、无刷电机、机架和防撞圈等七部分组成，采用“＋”型飞行模式，下面分别论证这几个模块的选择。
1.1 姿态模块的论证与选择
方案一：MPU6050三轴陀螺仪。MPU6050三轴陀螺仪就是可以在同一时间内测量六个不同方向的加速、移动轨迹以及位置的测量装置。单轴的话，就只可以测定一个方向的量，那么一个三轴陀螺就可以代替三个单轴陀螺。它现在已经成为激光陀螺的发展趋向，具有可靠性很好、结构简单不复杂、重量很轻和体积很小等等特点，但是其输出数据需要大量的浮点预算才能保证较高的精度，这样会影响单片机对最终的姿态控制的响应速率，优点是改芯片资料多，应用成熟，价格便宜，是一款非常适合初学者入门的芯片。
方案二：光纤陀螺仪。光纤陀螺仪是以光导纤维线圈为基础的敏感元件， 由激光二极管发射出的光线朝两个方向沿光导纤维传播。光传播路径的变化，决定了敏感元件的角位移。光纤陀螺仪寿命长，动态范围大，瞬时启动，结构简单，尺寸小，重量轻，但是成本较高。
方案三：AHRS模块。AHRS模块包含了MPU6050(集成3轴陀螺仪和3轴加速度计)、HMC5883L（3轴地磁传感器），BPM180气压高度计等模块。且AHRS模块内部已经进行一些数据处理，通过串口直接输出飞行器的当前姿态状态，减少了单片机进行姿态解算的运行时间消耗，进一步提高了单片机对飞行器的姿态控制，但是该模块为集成模块，价格较高，不适合初学者学习QFN封装的焊接。
综合以上三种方案，我们选择了方案一。
1.2 电源模块的论证与选择
飞行器的电机电源由12伏的航模专用锂电池直接提供，而我们采用的电调为12V，带5V电源输出，STM32单片机的工作电压为3.3V，所以系统需要进行一次电压转换，为控制核心供电，其质量直接决定了系统的稳定性。
方案一：ME6219系列是以CMOS工艺制造的高精度，低噪音，超快响应低压线性稳压器。这个系列的稳压器内置固定的参考电压源，误差修正电路，限流电路，相位补偿电路以及低内阻的MOSFET，达到高纹波抑制，低输出噪音，超快低压差性能。高精度输出电压高。
方案二：AMS1117。AMS1117系列稳压器有可调版与多种固定电压版，设计用于提供1A输出电流且工作压差可低至1V。在最大输出电流时，AMS1117器件的压差保证最大不超过1.3V，并随负载电流的减小而逐渐降低。AMS1117的片上微调把基准电压调整到1.5%的误差以内，而且电流限制也得到了调整，以尽量减少因稳压器和电源电路超载而造成的压力。但是能提供的电流较小，且在大电流工作状态下易发热。
综合以上两种方案，选择方案二。
1.3飞行方式的论证与选择
方案一：十字飞行方式。四轴的四个电机以十字的方式排列，调整的时候应该对角调整，我们这里主要用于测试算法。优点：十字飞行模式调试简单，非常适合初学者掌握四轴飞行器飞行算法，缺点：它灵活性和可调性有限。
方案二：X行飞行方式。四轴的四个电机以X字的方式排列，调整的时候应该相邻两个调节，灵活性和可调性较高。X型飞行方式非常自由灵活，旋转方式多样，可以花样飞行，也可以做出很多高难度动作，但是该方式参数较难调试，不适合初学者使用。
综合以上两种方案，选择了方案一。
1.4 无刷电机驱动模块的论证与选择
方案一：自己制作无刷电机驱动电路，这个难度大，容易炸机，不适合初学者采用，但是自己制作驱动模块可以进一步学习模拟电路。
方案二：采用电调模块，该模块应用已经非常成熟，且和自己制作电调相比，具有控制简单，自带5V电源输出，可以省去5V电源设计，简化电路结构，减少电路占用面积，稳定性非常高，控制灵活，非常适合初学者采用。
综合以上两种方案，选择了方案二。
1.5 单片机的选择
方案一：采用国产STC15W4K61S4系列单片机，该单片机8位单片机，成本低，可以工作在1T模式下，最高内部震荡频率为30MHZ，可以省去最小系统的设计，不需要外接晶振和复位电路，可以省去电路设计的工作量。但是该单片机处理效率低，运行速度慢，不适合姿态计算和数据融合的计算量相当大的应用场合。
方案二：采用ST公司的STM32F103系列单片机，该单片机采用ARM Context-M3内核，该单片机为32位，最高工作频率为72MHZ，具有处理速度快，工作效率高，适合应用于工业控制。该单片机资料多，采用ST公司集成库开发具有开发简单，开发周期短，效率高等优点，非常适合用于四轴飞行器的控制核心。
综合以上两种方案，选择了方案二。
1.6 遥控方式的选择
方案一：采用蓝牙方式控制四轴飞行器，因为蓝牙控制简单，很方便的和手机和PC连接，控制容易。且采用匿名四轴飞行器的匿名上位机和四轴连接可以分析四轴数据，对PID进行设置和针对游戏手柄控制四轴，非常适合初学者，且做四轴算法验证试验。
方案二：采用NRF24L01模块，该模块工作频段为2.4GHz，传输距离远，采用SPI和MCU通信，可实现长达1千米左右的远距离控制，但是必须自己制作遥控器，适合对四轴算法调试成功后对四轴进行控制，这个也是以后的制作目标。
方案三：采用现在比较流行的多通道遥控器，内部为2.4模块和MCU构成，直接输出PPM脉宽，单片机只需捕获该脉宽就可以对四轴进行控制，该遥控器已经产品化，使用简单，稳定高效，也不需要自己编写通信协议。但是该遥控器价格昂贵，目前还没有资金支持。
综合以上三个方案，选择了方案一。
二、系统理论分析与计算
2.1 串级PID控制算法的分析
2.1.1串级PID理论分析
ROLL和PIT轴向按照以上公式计算PID输出，但YAW轴比较特殊，因为偏航角法线方向刚好和地球重力平行，这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度计。如果不使用罗盘的话，我们可以单纯的通过角速度积分来测得偏航角，缺点是由于积分环节中存在积分漂移，偏航角随着时间的推移会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。串级PID：采用的角度P和角速度PID的双闭环PID算法------>角度的误差被作为期望输入到角速度控制器中 （角度的微分就是角速度）
对于本系统则采用了将角度控制与角速度控制级联的方式组成整个串级 PID 控制器。串级 PID 算法中，角速度内环占着极为重要的地位。在对四旋翼飞行的物理模型进行分析后，可以知道造成系统不稳定的物理表现之一就是不稳定的角速度。因此，若能够直接对系统的角速度进行较好的闭环控制，必然会改善系统的动态特性及其稳定性，通常也把角速度内环称为增稳环节。而角度外环的作用则体现在对四轴飞行器的姿态角的精确控制。
外环：输入为角度,输出为角速度
内环：输入为角速度，输出为PWM增量
使用串级PID，分为：角度环控制PID环，和角速度控制环稳定环。主调为角度环（外环），副调为角速度环（内环）。
参数整定原则为先内后外，故在整定内环时将外环的PID均设为0
所谓外环就是只是一个P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飞机震荡。
震荡的特点是：频率小、幅度大。
2.1.2 串级PID整定方法的分析
调试该系统分为6步:
第一步：将内外环PID都归0，适当增加内环的P，调整P至四轴从正面朝上自然转动到正面朝下时能感受到阻力，且没有抖动，有抖动就应减小P，当P减小到无抖动或者轻微抖动时即可。
第二步：让内环的D慢慢增加，到你用手能明显感受到转动四轴产生排斥外力的阻力即可，D能抑制P产生的振荡，但是D过大也会导致高频振荡，调整D至系统无振荡且能抑制外界的力即可。
第三步：给内环一点点I，注意的是I的积分要在油门开启后才开始，油门关闭就清0，且必须有积分限幅。I推荐取越小越好，我取的是0.01，I取大了会导致系统振荡。
将内环P减半，将外环P调至内环的50-70倍，根据系统产生的高频振荡降低内环的D，直至高频振荡消除即可。
给外环一点点I，同3.
第四步：根据实际情况对参数进行优化调整，调整过程中要注意区分各个参数的作用，时刻记住，P是回复力，大了会低频振荡，D是抑制力，大了会高频振荡，I是静差消除力，越小越好，大了会产生振荡。
2.1.3 串级PID框图
图1 串级PID控制原理框图
2.2 欧拉角的理论与计算
2.2.1 欧拉角的理论分析
在3D模型中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系：
定义 分别为绕Z轴、Y轴、X轴的旋转角度，如 果用Tait-Bryan angle表示，分别为航向角（Yaw）、俯仰角（Pitch）、
横滚角（Roll）。
欧拉角的动态定义把欧拉角看成3次连续转动而形成的，按照转动轴线及其顺序选择的不同，有12种定义方法。这里我们主要分析航空次序欧拉角，航空次序欧拉角是按照Z-Y-X的顺序定义的绕Z轴旋转代表航向角（Yaw），绕Y轴旋转代表俯仰（Pitch），绕X轴旋转代表横滚角（Roll）。
2.2.2 四元数计算出欧拉角的理论分析
一、四元数的定义
=1
通过旋转轴和绕该轴旋转的角度可以构造一个四元数：
其中 是绕旋转轴旋转的角度， 为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。
二、欧拉角到四元数的转换
三、四元数到欧拉角的转换
arctan和arcsin的结果是 ，这并不能覆盖所有朝向(对于 角 的取值范围已经满足)，因此需要用atan2来代替arctan。
四、在其他坐标系下使用
在其他坐标系下，需根据坐标轴的定义，调整一下以上公式。如在Direct3D中，笛卡尔坐标系的X轴变为Z轴，Y轴变为X轴，Z轴变为Y轴（无需考虑方向）。
2.2.3 四元数计算出欧拉角的程序设计
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //定义四元素
void IMU_Update(void)
{
float norm;
float gx = MPU6500_Gyro.X * GYRO_GR,gy = MPU6500_Gyro.Y * GYRO_GR,gz = MPU6500_Gyro.Z * GYRO_GR;//角度之间的单位转换
float ax = Acc_Avg.X,ay = Acc_Avg.Y,az = Acc_Avg.Z;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q1q1 = q1 * q1;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float vx, vy, vz;
float ex, ey, ez;
float q0_yawq0_yaw = q0_yaw * q0_yaw;
float q1_yawq1_yaw = q1_yaw * q1_yaw;
float q2_yawq2_yaw = q2_yaw * q2_yaw;
float q3_yawq3_yaw = q3_yaw * q3_yaw;
float q1_yawq2_yaw = q1_yaw * q2_yaw;
float q0_yawq3_yaw = q0_yaw * q3_yaw;
//**************************Yaw轴计算******************************
//Yaw轴四元素的微分方程
q0_yaw = q0_yaw + (-q1_yaw * gx - q2_yaw * gy - q3_yaw * gz) * SAMPLE_HALF_T;
q1_yaw = q1_yaw + (q0_yaw * gx + q2_yaw * gz - q3_yaw * gy) * SAMPLE_HALF_T;
q2_yaw = q2_yaw + (q0_yaw * gy - q1_yaw * gz + q3_yaw * gx) * SAMPLE_HALF_T;
q3_yaw = q3_yaw + (q0_yaw * gz + q1_yaw * gy - q2_yaw * gx) * SAMPLE_HALF_T;
//规范化Yaw轴四元数
norm = sqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
q0_yaw = q0_yaw / norm;
q1_yaw = q1_yaw / norm;
q2_yaw = q2_yaw / norm;
q3_yaw = q3_yaw / norm;
if(ax * ay * az == 0)
return ;
//规范化加速度计值
norm = sqrt(ax * ax + ay * ay + az * az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
//估计重力方向和流量/变迁
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//向量外积再相减得到差分就是误差
ex = (ay * vz - az * vy) ;
ey = (az * vx - ax * vz) ;
ez = (ax * vy - ay * vx) ;
//对误差进行PI计算
ex_int = ex_int + ex * IMU_KI;
ey_int = ey_int + ey * IMU_KI;
ez_int = ez_int + ez * IMU_KI;
//校正陀螺仪
gx = gx + IMU_KP * ex + ex_int;
gy = gy + IMU_KP * ey + ey_int;
gz = gz + IMU_KP * ez + ez_int;
//四元素的微分方程
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * SAMPLE_HALF_T;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * SAMPLE_HALF_T;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * SAMPLE_HALF_T;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * SAMPLE_HALF_T;
//规范化Pitch、Roll轴四元数
norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//求解欧拉角
Angle.X = atan2(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * 57.3f;
Angle.Y = asin(-2 * q1q3 + 2 * q0q2) * 57.3f;
Angle.Z = atan2(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1) * 57.3f;
}
程序说明:该程序由开源四轴BlackHole1提供。
2.2.4 硬件加速引擎（DMP）理论分析
MPU-6050整合了3轴陀螺仪、3轴加速器，数字运动处理(DMP: Digital Motion Processor)硬件加速引擎，该DMP功能的最强大功能就是它使用的是MPU6050整合的内部加速引擎，避开上文的利用MPU6050原始数据，通过各种算法得到稳定的四元数，而我们利用官方的DMP驱动程序直接读取出四元数的数值，省下大量的工作量，在利用四元数计算公式计算出稳定，有效的欧拉角。但是因为没有采用地磁补偿，航向角（Yaw）会随时间产生误差。
计算出欧拉角的部分程序如下：
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
2.3 四轴飞行器和飞行结构分析
2.3.1 结构形式
旋翼对称分布在机体的前后、左右四个方向，四个旋翼处于同一高度平面，且四个旋
翼的结构和半径都相同，四个电机对称的安装在飞行器的支架端，支架中间空间安放飞行控
制计算机和外部设备
图4四轴结构图
2.3.2 工作原理
四旋翼飞行器通过调节四个电机转速来改变旋翼转速，实现升力的变化，从而控制飞
行器的姿态和位置。四旋翼飞行器是一种六自由度的垂直升降机，但只有四个输入力，同时
却有六个状态输出，所以它又是一种欠驱动系统。
四轴飞行器的电机 1 和电机 3 逆时针旋转的同时，电机 2 和电机 4 顺时针旋转，
因此当飞行器平衡飞行时，陀螺效应和空气动力扭矩效应均被抵消。在上图中，电机 1 和电机 3 作逆时针旋转，电机 2 和电机 4 作顺时针旋转，规定沿x 轴正方向运动称为向前运动，↑箭头表示此电机转速提高，↓表示此电机转速下降。
在上图中，电机 1 和电机 3 作逆时针旋转，电机 2 和电机 4 作顺时针旋转，规定沿x 轴正方向运动称为向前运动，箭头在旋翼的运动平面上方表示此电机转速提高，在下方表示此电机转速下降。
（ 1）垂直运动：同时增加四个电机的输出功率，旋翼转速增加使得总的拉力增大，当总拉力足以克服整机的重量时，四旋翼飞行器便离地垂直上升；反之，同时减小四个电机的输出功率，四旋翼飞行器则垂直下降，直至平衡落地，实现了沿 z 轴的垂直运动。当外界扰动量为零时，在旋翼产生的升力等于飞行器的自重时，飞行器便保持悬停状态。
（ 2）俯仰运动：在图（ b）中，电机 1 的转速上升，电机 3 的转速下降（改变量大小应相等），电机 2、电机 4 的转速保持不变。由于旋翼 1 的升力上升，旋翼 3 的升力下降，产生的不平衡力矩使机身绕 y 轴旋转，同理，当电机 1 的转速下降，电机 3 的转速上升，机身便绕 y 轴向另一个方向旋转，实现飞行器的俯仰运动。
（ 3）滚转运动：与图 b 的原理相同，在图 c 中，改变电机 2 和电机 4 的转速，保持电机 1 和电机 3 的转速不变，则可使机身绕 x 轴旋转（正向和反向），实现飞行器的滚转运动。
（ 4）偏航运动：旋翼转动过程中由于空气阻力作用会形成与转动方向相反的反扭矩，为了克服反扭矩影响，可使四个旋翼中的两个正转，两个反转，且对角线上的各个旋翼转动
方向相同。反扭矩的大小与旋翼转速有关，当四个电机转速相同时，四个旋翼产生的反扭矩
相互平衡，四旋翼飞行器不发生转动；当四个电机转速不完全相同时，不平衡的反扭矩会引起四旋翼飞行器转动。在图 d 中，当电机 1 和电机 3 的转速上升，电机 2 和电机 4 的
转速下降时，旋翼 1 和旋翼 3 对机身的反扭矩大于旋翼 2 和旋翼 4 对机身的反扭矩，机身便在富余反扭矩的作用下绕 z 轴转动，实现飞行器的偏航运动，转向与电机 1、电机 3 的转向相反。
（ 5）前后运动：要想实现飞行器在水平面内前后、左右的运动，必须在水平面内对飞行器施加一定的力。在图 e 中，增加电机 3 转速，使拉力增大，相应减小电机 1 转速，使拉力减小，同时保持其它两个电机转速不变，反扭矩仍然要保持平衡。按图 b 的理论，飞行器首先发生一定程度的倾斜，从而使旋翼拉力产生水平分量，因此可以实现飞行器的前飞运动。向后飞行与向前飞行正好相反。（在图 b 图 c 中，飞行器在产生俯仰、翻滚运动的同时也会产生沿 x、 y 轴的水平运动。）
（ 6）倾向运动：在图 f 中，由于结构对称，所以倾向飞行的工作原理与前后运动完全一样。
三、电路与程序设计
3.1 电路的设计
3.1.1 系统总体框图
系统总体框图如图2所示。
图5 系统总体框图
3.1.2 MPU6050电路原理图
图7 MPU6050子系统电路
3.1.3电机驱动系统框图
图8 电机驱动子系统框图
3.1.4电源模块
电源由滤波部分、稳压部分组成。这里5V电源直接采用电调输出的5V电源，这里只是把5V电源通过ASM1117输出稳定的3.3V电源。
图9 电源子系统电路
3.1.5 单片机最小系统电路设计
图10 单片机最小系统电路
3.1.6 电调连接原理图
图11 电调连接原理图
3.2程序的设计
3.2.1程序功能描述与设计思路
1、程序功能描述
根据题目要求软件部分主要分为三部分，第一部分为无刷电机驱动部分，利用STM32单片机内部定时器的多路PWM输出功能，实现无刷电机驱动；第二部分为MPU6050模块数据接收部分，利用STM32单片机内部串口USART1和蓝牙模块通信，利用STM32模拟IIC接口和MPU6050进行数据传输；第三部分是姿态控制部分，根据接收到的欧拉角与目标欧拉角之间的差值，运用串级PID控制算法，解算出相应电机的PWM调整量，设定定时的PWM输出，使飞行器姿态平衡。扩展部分就是利用IAP在应用中编程方式，先以SWD方式下载一个Bootloader程序到STM32中，在利用蓝牙进行无线对城西进行升级，这样就省去了复杂的修改程序后又烧写到单片机中的复杂操作。
2、程序设计思路
本设计的程序结合STM32单片机的特点，主要实现思路为：单片机上电、采用按键方式对电调进行解锁，之后利用定时器设定电机PWM周期、串口初始化，在串口中断服务函数中不断接收MPU6050模块发送的数据，这里我们移植了TI单片机的利用MPU6050内部DMP计算出欧拉角，直接读出欧拉角，省去软件数据融合计算量，提高程序效率。在初始化结束后，设定目标姿态、进入循环函数，不断检测MPU6050数据是否接受完成，一旦接收到有效的姿态数据，就进入姿态控制函数。在姿态控制函数内部，利用当前四轴飞行器的Roll（翻滚） 和 pitch （俯仰）、yaw（偏航） 数据，结合串级PID控制算法，解算出不同姿态时，利用空气动力学原理，融合出每个无刷电机需要的调整量控制电机，这样不断的接受姿态数据，解算数据，调整量输出，使四轴飞行器稳定的飞行。
3.2.2程序流程图
图12 程序流程图
四、测试方案与测试结果
4.1测试方案
1、硬件测试
首先，先对电路电源进行检查，用万用表进行检查，确保无误之后通电，然后烧写一个测试最小系统是否工作正常的程序。
采用分模块测试外围电路是否工作正常。
2、软件仿真测试
在调试程序之前，先用示波器观察STM32单片机PWM输出，并用程序模仿飞行器的飞行方法，通过对其仿真来测试PWM的稳定度。
MPU6050 模块（6轴姿态仪）配合电脑匿名四轴串口调试助手进行数据的查看，方便调试。
3、硬件软件联调
通过单片机编程，模仿出PWM，调整出电调需要的周期和占空比，并测量是否能通过电机驱动来是飞行器起飞，通过多次测试，找出飞行器起飞时的PWM值。
同过MPU6050 模块（6轴姿态仪）使四轴飞行器稳定的起飞，并悬停在空中；再进行测试，使四轴飞行器前进和后退；最后进行降落的测试。
4.2 测试结果及分析
4.2.1测试分析与结论
根据上述测试数据，可以得出以下结论：
采用STM32作为控制核心通过IIC和MPU6050进行数据传输，采用蓝牙和PC机上的匿名四轴调试助手进行数据分析，遥控控制等调试操作。本次设计共完成了遥控控制四轴飞行器平衡离开地面，但由于我们采用的是MPU6050的6轴传感器，yaw轴处理不恰当，会产生随时间的漂移，很难控制。故后期还需要对yaw轴的算法进行改进，在下一代作品中应该采用一个9轴的传感器。
最后实现控制方法为采用匿名四轴上位机，通过蓝牙方式和四轴飞行器进行数据交换。因NRF24L01模块还没调试，还未达到室外飞行的要求。
五、专用周总结
首先谈一下对四轴飞行器的向往，近几年无人机火了，连汪峰求婚都用的是国产大疆无人机，各大科技公司也开始推出自己的无人机产品，对于一个无人机来说，我对它一开始还不怎么了解，只是充满无限的向往，我以为要让它起飞要考虑很多空气动力学，和数学建模的问题，而这些对于我来说是不可以想象的，太难了吧！但是你自己不尝试怎么知道自己能不能了，所以开始在百度上搜索四轴飞行器的资料，刚开始只是了解，看了网友大神自己对四轴飞行器的了解。我也开始一步一步的深深扎了进去，对四轴充满无限的的好奇。看了无数的博客，看了无数的论文，也收藏了很多个知名的四轴论坛。我也渐渐明确我应该做些什么准备。其实总结一下就是，只要自己感觉自己能完成本次设计任务，就大胆去做，不要永远停留在想去做，而是要去付出行动，大胆去尝试，就算自己最后失败，你也会收获很多的，这几天在学习Linux操作系统，其实刚开始Unix操作系统是个失败的大工程，可是就是那么一个人把失败的东西转换成了现在非常成功的东西。
刚开始我的计划是做一个小四轴验证算法，找一找成就感。因为对于一个初学者来说，大四轴的难度太大，成本高，也许花了很多钱并没有什么结果，所以就要开始自己学PCB电路板的制作，所以一股闹开始学习郭天祥老师录得一个Altium Designer的视频，说实话。我的单片机很多知识都是从郭天祥的十天征服单片机上面学来的，很有幸能够再次学习到他录得一个PCB电路板制作的一个视频，讲的非常细。对于我这个连英语三级都没过的人来说，用起英文版的软件用起来也得心应手。大概学了半个多月的时间，自以为可以开始了，可是又需要会CAD画出PCB的外形轮廓，后来得知老师那有一个大疆无人机的机架，所以就没有开始对小四轴的设计，而是对飞控板的设计。当时也有很多的人给我说就不要去设计飞控了，难度太大，还是去卖一块飞控板，买个遥控器就自己组装呗，可是这样怎么学习了就算自己失败了，我也要自己去设计飞控板，当我拿到原来无人机的KK飞控板时，怎么这飞控板如此简单。单片机、陀螺仪、各种接口，连电源都没有接。只有一个AMS1117的一个电源芯片，后来才得知电调是可以输出5V电源的，这样我就可以省去12V转5V的电源电路设计，可谓是一大喜事啊！从原理图开始，从STM32最小系统开始和外围电路的连接。到最后的PCB设计，可谓是感受到电路板设计的强大之处，在对PCB检查了很多次之后，发到了厂家开始打样，我也在开发板上测试MPU6050，包括去学习什么是X、Y、Z三个方向的加速度，和X、Y、Z三个方向的角速度，这就构成了6轴立体模型。也开始学习卡尔曼滤波和互补滤波，先后利用匿名四轴上位机观察滤波前后的数据曲线的变化，可是准备了那么多，也学习了利用四元数计算出欧拉角，可是算法我并不是很懂，但是我知道怎么用。也在一个开源电子论坛上面学习到了可以利用MPU6050内部的(DMP: Digital Motion Processor)硬件加速引擎，可以省去滤波和复杂的四元数数据融合计算，可谓是调高了软件的执行效率，最后我还惊奇的发现DMP居然可以自动校准。实在是太强大了。
在收到打样会来的电路后，开始对电路板进行焊接，这也是我第一次焊接贴片元件，其中还有高难度的QFN的封装，还好我比较机智，有困难找度娘，没错，看了一个QFN的焊接视频之后，我也开始了电路板的攻坚克难阶段。最后在废掉一块电路板之后，我终于完成了电路板的所有焊接和调试。可谓是越战越勇，可不知最难得还是算法的移植问题。
在对无刷电机的驱动，和MPU6050及蓝牙和PC机上面的匿名四轴上位机通信协议搞懂之后，就开始学习串级PID，以前参加2015年全国大学生电子设计大赛时学过单级PID算法，可是这个用的是串级PID，又是一个崭新的概念。其中的学习过程就不一一说明了。可谓是跋山涉水，终于可以整体调试了。我在对PID参数进行调试时，通常会利用蓝牙接受上位机数据后，修改PID变量的值，达到无线，连续的修改参数。在很多个日日夜夜里，没有放弃，最后终于可以让飞行器离开地面，可是稳定性不是很好，因为没有采用地磁传感器，所以利用的是角速度的Z轴数据来进行计算，导致yaw（航向角）有随时间的误差越来越大，导致四轴飞行器会在外界的干扰下发生旋转，导致无法稳定飞行，这个问题由于时间的关系，还没有得以解决，以及NRF24L01模块还没有调试，暂且利用蓝牙进行控制。本次制作可谓是收获满满，也获得了一丝丝的胜利。为接下来的设计打好基础。没有人是一口吃胖的，所以验证了我的那句座右铭：付出努力，不放弃，哪怕是失败，那也是一种收获。加油！
最后在此感谢给我支持和指导的盛磊老师和施云老师！致敬
六、参考资料
【1】 匿名四轴开源源代码，上位机。ANO-MR-F1-140104经典pid
【2】 郭天祥DXP设计教程
【3】 《无感无刷直流电机之电调设计全攻略》By: timegate 墨鸢
【4】 《卡尔曼滤波与组合导航原理》 秦永元
【5】 《STM32F1开发指南-库函数版本_V3.1》 刘军 正点原子出品
【6】 开源电子网www.openv.com提供技术指导。
【7】 电子发烧友网 http://www.elecfans.com/四轴飞行器论坛提供技术参考
六、附录：