笔者目前在读研究生，研究的方向正是飞行器的定位与导航。去年10月份开始设计的微型四旋翼，近日才完成整个系统的设计并且飞机可以较为稳定的悬停在空中。
下面就将笔者最近整理的制作过程梳理一遍，还希望与有兴趣的网友共同讨论。
笔者将分为硬件设计与软件设计两大部分来分别阐述系统的构成：
硬件设计：
总体思路：
整个机架采用PCB板，将四个电机固定在PCB板的四个角，外接电池。
硬件模块：
单片机、惯性测量模块(IMU)、无线通讯模块、电机驱动模块、续流二极管、电源管理模块(稳压与充放电)、直流有刷电机、大电流放电电池、遥控器。
硬件选型：
|
模块名称 |
元件名称 |
数量 |
|
|
单片机 |
STM32F103CBT6 |
1 |
|
|
惯性测量模块(IMU) |
MPU6050(三轴加速度计+三轴陀螺仪) |
1 |
|
|
无线通讯模块 |
NRF24L01 |
1 |
|
|
电机驱动模块 |
AO3400 5.8A |
4 |
|
|
续流二极管 |
SS34 3A |
4 |
|
|
电源管理模块 |
稳压 |
TPS79333 3.3V |
1 |
|
充放电 |
TP4057 USB兼容5V充电 |
1 |
|
|
直流有刷电机 |
空心杯有刷直流电机7*16mm |
4 |
|
|
大电流放电电池 |
250mAh 20C |
1 |
|
|
遥控器 |
JOYPAD游戏手柄 |
1 |
|
硬件工作综述：
单片机负责整个系统的协调工作；惯性测量模块(IMU)负责测量四旋翼的姿态；无线通讯模块负责四旋翼与遥控器的通讯；电机驱动模块负责驱动电机；续流二极管负责对电机进行续流；电源管理模块中的稳压模块负责整个系统的供电，电源管理模块中的充放电模块负责对电池充电；有刷电机负责提供四旋翼的飞行动力；大电流放电电池负责四旋翼的能量来源；遥控器负责对四旋翼进行遥控和控制。
硬件设计功能模块图：
实际效果图与相关参数：
尺寸：对角电机轴距10x10cm
重量：33.2g(带电池)
软件设计：
总体思路：
惯性测量模块(IMU)测量出当前飞机的三轴加速度与三轴角速度并传送给单片机处理，由单片机进行基于四元数的姿态解算，求解出当前飞机的pitch、roll、yaw三个角度值，然后根据这三个角度经过PID控制运算，输出四路PWM控制四个直流有刷电机的加减速从而达到飞机的平衡悬停。
其中，惯性测量模块(IMU)的加速度计由于噪声比较大，所以需要对其进行滤波处理；而遥控器则是对飞机进行实时的姿态控制；最后由于四旋翼制作的特殊性，在调试PID参数阶段会频繁的烧写程序，鉴于此，笔者开发了基于NRF24L01的Bootloader技术，免除了烧写Flash的物理连线限制，可实现远程程序一键下载。
姿态解算：
姿态解算属于四旋翼制作的核心部分，如果姿态解算能够实时的反应出飞机的状态，那么对于控制来讲就相对来说比较容易了。而姿态结算所要做的事情就是两个坐标系之间的正确转化(地理坐标系与载体坐标系)，这种转化有很多种表示方法，例如欧拉角法、方向余弦矩阵法、四元数法、旋转矢量法等。笔者采用的是应用广泛的四元数法，而旋转矢量法则是一种基于四元数法的改进四元数法。
四元数本是用于描述四维空间向量的一种方法，对于他的线性变换也就是在四维空间中的拉伸和旋转，显而易见，我们用四元数的向量乘法来表示三维空间中的旋转是绰绰有余的。
通过惯性测量模块(IMU)传送过来的当前飞机的三轴加速度和三轴角速度的值，这样一个三维的向量，转化为四维向量，然后在四维空间中做线性变换(也可以说在三维空间中旋转)后输出，利用四元数与欧拉角的关系(一定要注意旋转顺序)，将当前四元数转换为欧拉角pitch、roll、yaw即得到当前飞机的姿态。
以下给出笔者姿态融合的代码，该代码网上都有，笔者在这里做了些许注释，方便理解。
/*********************************************************************************
函数名：void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)
说明：IMU单元数据融合，更新姿态四元数
入口：float gx 陀螺仪x分量
float gy 陀螺仪y分量
float gz 陀螺仪z分量
float ax 加速度计x分量
float ay 加速度计y分量
float az 加速度计z分量
出口：无
备注：核心思想:利用陀螺仪来计算高速动态下的姿态,利用加速度计来进行角度修正
**********************************************************************************/
void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm; //平方根的倒数
float halfvx, halfvy, halfvz; //在当前载体坐标系中，重力分量在三个轴上的分量
float halfex, halfey, halfez; //当前加速度计测得的重力加速度在三个轴上的分量与当前姿态在三个轴上的重力分量的误差,这里采用差积的方式
float qa, qb, qc;
gx = gx * PI / 180; //转换为弧度制
gy = gy * PI / 180;
gz = gz * PI / 180;
//如果加速度计处于*落体状态，可能会出现这种情况，不进行姿态解算，因为会产生分母无穷大的情况
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
//单位化加速度计,意义在于在变更了加速度计的量程之后不需要修改Kp参数,因为这里归一化了
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
//将当前姿态的重力在三个轴上的分量分离出来
//就是方向余弦旋转矩阵的第三列,注意是地理坐标系(n系)到载体坐标系(b系)的,不要弄反了.如果书上是b系到n系,转置即可
//始终记着你操纵的是飞机,所以一切的量都要以他的坐标系来做参考,所以坐标需要从地理坐标系上转换到载体坐标系上
halfvx = g_q1 * g_q3 - g_q0 * g_q2;
halfvy = g_q0 * g_q1 + g_q2 * g_q3;
halfvz = g_q0 * g_q0 - 0.5f + g_q3 * g_q3;
//计算由当前姿态的重力在三个轴上的分量与加速度计测得的重力在三个轴上的分量的差,这里采用三维空间的差积(向量积)方法求差
//计算公式由矩阵运算推导而来 公式参见http://en.wikipedia.org/wiki/Cross_product 中的Mnemonic部分
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
//积分求误差,关于当前姿态分离出的重力分量与当前加速度计测得的重力分量的差值进行积分消除误差
if(g_twoKi > 0.0f)
{
g_integralFBx += g_twoKi * halfex * CNTLCYCLE; //Ki积分
g_integralFBy += g_twoKi * halfey * CNTLCYCLE;
g_integralFBz += g_twoKi * halfez * CNTLCYCLE;
gx += g_integralFBx; //将积分误差反馈到陀螺仪上，修正陀螺仪的值
gy += g_integralFBy;
gz += g_integralFBz;
}
else //不进行积分运算，只进行比例调节
{
g_integralFBx = 0.0f;
g_integralFBy = 0.0f;
g_integralFBz = 0.0f;
}
//直接应用比例调节，修正陀螺仪的值
gx += g_twoKp * halfex;
gy += g_twoKp * halfey;
gz += g_twoKp * halfez;
}
//以下为四元数微分方程.将陀螺仪和四元数结合起来，是姿态更新的核心算子
//计算方法由矩阵运算推导而来
// . 1
// q = - * q x Omega 式中左边是四元数的倒数,右边的x是四元数乘法,Omega是陀螺仪的值(即角速度)
// 2
// .
// [q0] [0 -wx -wy -wz] [q0]
// .
// [q1] [wx 0 wz -wy] [q1]
// . = *
// [q2] [wy -wz 0 wx ] [q2]
// .
// [q3] [wz wy -wx 0 ] [q3]
gx *= (0.5f * CNTLCYCLE);
gy *= (0.5f * CNTLCYCLE);
gz *= (0.5f * CNTLCYCLE);
qa = g_q0;
qb = g_q1;
qc = g_q2;
g_q0 += (-qb * gx - qc * gy - g_q3 * gz);
g_q1 += ( qa * gx + qc * gz - g_q3 * gy);
g_q2 += ( qa * gy - qb * gz + g_q3 * gx);
g_q3 += ( qa * gz + qb * gy - qc * gx);
//单位化四元数,意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换
recipNorm = invSqrt(g_q0 * g_q0 + g_q1 * g_q1 + g_q2 * g_q2 + g_q3 * g_q3);
g_q0 *= recipNorm;
g_q1 *= recipNorm;
g_q2 *= recipNorm;
g_q3 *= recipNorm;
//四元数到欧拉角转换，转换顺序为Z-Y-X,参见<Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors>.pdf一文,P24
//注意此时的转换顺序是1-2-3，即X-Y-Z。但是由于画图方便，作者这里做了一个转换，即调换Z和X，所以顺序没变
g_Yaw = atan2(2 * g_q1 * g_q2 + 2 * g_q0 * g_q3, g_q1 * g_q1 + g_q0 * g_q0 - g_q3 * g_q3 - g_q2 * g_q2) * 180 / PI; //Yaw
g_Roll = asin(-2 * g_q1 * g_q3 + 2 * g_q0* g_q2) * 180 / PI; //Roll
g_Pitch = atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1 * g_q1 - 2 * g_q2* g_q2 + 1) * 180 / PI; //Pitch
}
注意其中的快速开方函数来自*，精度0.175%。并且注意输入的陀螺仪必须是弧度制(这一点在进入函数的时候已经做了转换)，否则姿态解算是错误的。
针对上述代码我还想说明一个笔者发现的问题：有很多网友和许多外国的四轴代码(CrazyFlie)在这个姿态解算的加速度计部分都做了零点校准处理。意思就是在开机的时候读取一定次数的加速度计的值，然后平均一下，得到一个初始状态的偏移量，最后在输出的时候加速度计减掉这个值，然后再给到姿态解算代码部分。而笔者在刚开始移植代码的时候是没有做这个零点校准处理的(当然，陀螺仪必须要做零点处理，因为陀螺仪的原理，必须要在静止时输出为0)，包括现在依旧没有对加速度计做零点校准处理，仍然可以获得较为实时的姿态。
那么显然对加速度计做不做零点校准处理都是可行的。为什么呢？经过我的分析，首先在这段代码中，我们对加速度计进行了归一化处理，我们知道在数学当中，对数值进行单位化意味着长度不变而只改变方向，对于加速度计来讲，他的”长度”就是加速度的大小，他的”方向”就是加速度的方向。所以我们对加速度计做了单位化之后，其加速度的大小我们就无从而知，但是我们利用了他的方向来进行姿态解算。就这一点来讲，无论我们做不做零点校准处理，进来的加速度的值始终都抛弃掉大小，并关注方向，与零点校准处理无关。另一方面，由于我们生活在重力场里面，那么加速度计在静止状态下测量的是重力加速度，会有一个g的输出。而我们理想的加速度计应该是输出0，而在有加速度的时候应该输出相应的加速度，但是现实是我们生活在一个重力场里面，所以必定有一个重力输出。那么零点校准处理的核心就是我们对于加速度计的理解问题，如果做了零点校准处理，那么我们使用的加速度计就成为了”真正的”加速度计，当有重力的时候他输出为0，有加速度的时候就输出加速度；当我们没有做零点校准处理的时候，那么我们使用的加速度计就成了”重力”加速度计。但是细心的你其实可以发现那个并不是真正的加速度计，我将传感器反过来放的话输出就不是0了，而是z轴上的负值输出。显然这个零点标准处理做的不那么标准。况且这种处理方式是非常粗糙的，因为加速度计的噪声十分的大，数据波动非常厉害，我做了16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，其输出仍然有1~4左右的波动。可见加速度计确实不好处理，除非用Kalman滤波。
鉴于以上两点原因，本人就没有做加速度计的零点校准处理。当需要测量飞机的加速度大小并实现定位时，那么就需要做零点校准处理了。而当我们只需要解算姿态，那么加速度计就不需要做零点校准处理。
以上是笔者对于加速度计零点校准处理的愚见，如有错误，还望共同学习。
最后想说明一点的是关于陀螺仪的数据转化，笔者在最开始编写姿态解算代码时，发现角度的变化与实时姿态差了好几个数量级，体现出来的现象就是稍微移动一下飞机，姿态就呼呼的飞速变化。之前一直以为是姿态解算中的Kp和Ki的系数问题，后来才发现是陀螺仪的数据没有转化成标准单位(°/s)输出，没有参看pdf上的量程单位，所以没有做数据转化处理，在这里提醒一下各位，不要犯笔者这种低级错误了。
PID控制：
PID控制属于自动化领域，由于笔者的本科出生于自动化专业，所以对于自动控制原理有一点理论上的认识。P是比例，I是积分，D是微分，这是最基本的定义。对于一个系统，我们想要控制他，目前的理论是引入负反馈，这个概念相当重要，是由维纳提出来的。意思是，将输出引入到输入端，并且用输入减去输出，这就是著名的负反馈系统。很显然，我们要做的是输出跟随输入，使得系统可控。也就是说要求输出和输入的误差为0，即输出等于输入。在实际的系统中，输出与输入肯定是存在误差的，这种误差就通过PID来控制使得满足输出与输入误差为0。当系统由于干扰出现误差时，此时的P参数就起到了“立竿见影”的作用，将当前系统误差第一时间反应出来，也就是当前误差多少，我就给你多少输出值来补偿你的误差。这种调节方式的特点是快速而有劲，相应来说就是发散且不稳定的；而D参数则具有一种预见性，这种预见性可以提前预知系统的行为，比如距离设定值是越来越远还是越来越近，前者D的作用越强，后者D的作用越弱。可以发现D参数与P参数具有一定的互补性质，P会引起发散，而D则是抑制发散，使系统非常敏感；最后I参数是积分，在连续系统中是时间的积分，在数字系统中是时间的累加。这种累加无疑会造成系统的不稳定，如果系统长时间处于不平衡的位置，那么由于时间的累计，I的作用会变得越来越强，甚至超过了P的作用，那么系统必定失控。但是他的作用有时候确实不可忽略的：消除静差。
在这里笔者尤其提醒大家一点，如果此时系统的输出达到了我们给定的期望值，也就是说输出与输入误差为0，即现在的PID控制器输入0，所以输出也是0，也就是说此时的执行机构是不会输出的，让设备处于*运动阶段。而非我们认为的当你观察到一个系统处于稳定运行并达到给定值的时候，他的执行机构是一直在输出的，这是错误的。
浅谈完PID，对于四旋翼的控制，笔者采用的就是经典控制论中的PID控制，利用的是期望姿态(pitch=0,roll=0,yaw=0)与当前姿态的误差，通过PID的控制作用输出四路不同的PWM驱动电机让飞机调整自己的姿态满足当前姿态与期望姿态的误差为0的目标，这也是PID控制器的目标。
以下是笔者的PID控制代码：
/************************************************************************************************
函数名：void Quadrotor_Control(const float Exp_Pitch, const float Exp_Roll, const float Exp_Yaw)
说明：四旋翼控制函数，用于PWM计算和输出
入口：const float Exp_Pitch 期望倾仰角
const float Exp_Roll 期望横滚角
const float Exp_Yaw 期望偏航角
出口：无
备注：当前控制环为姿态控制环
其中有 大角度放弃控制 和 悬停黄灯指示
************************************************************************************************/
void Quadrotor_Control(const float Exp_Pitch, const float Exp_Roll, const float Exp_Yaw)
{
s16 outputPWM_Pitch, outputPWM_Roll, outputPWM_Yaw;
// --- 得到当前系统的误差-->利用期望角度减去当前角度
g_Attitude_Error.g_Error_Pitch = Exp_Pitch - g_Pitch;
g_Attitude_Error.g_Error_Roll = Exp_Roll - g_Roll;
g_Attitude_Error.g_Error_Yaw = Exp_Yaw - g_Yaw;
// --- 倾角太大，放弃控制
if (fabs(g_Attitude_Error.g_Error_Pitch) >= 55 || fabs(g_Attitude_Error.g_Error_Roll) >= 55)
{
PWM2_LED = 0; //蓝灯亮起
PWM_Set(0, 0, 0, 0); //停机
return ;
}
PWM2_LED = 1; //蓝灯熄灭
// --- 稳定指示灯,黄色.当角度值小于3°时，判定为基本稳定,黄灯亮起
if (fabs(g_Attitude_Error.g_Error_Pitch) <= 3 && fabs(g_Attitude_Error.g_Error_Roll) <= 3)
PWM4_LED = 0;
else
PWM4_LED = 1;
// --- 积分运算与积分误差限幅
if (fabs(g_Attitude_Error.g_Error_Pitch) <= 20) //积分分离-->在姿态误差角小于20°时引入积分
{ //Pitch
//累加误差
g_Attitude_Error.g_ErrorI_Pitch += g_Attitude_Error.g_Error_Pitch;
//积分限幅
if (g_Attitude_Error.g_ErrorI_Pitch >= PITCH_I_MAX)
g_Attitude_Error.g_ErrorI_Pitch = PITCH_I_MAX;
else if (g_Attitude_Error.g_ErrorI_Pitch <= -PITCH_I_MAX)
g_Attitude_Error.g_ErrorI_Pitch = -PITCH_I_MAX;
}
if (fabs(g_Attitude_Error.g_Error_Roll) <= 20)
{ //Roll
//累加误差
g_Attitude_Error.g_ErrorI_Roll += g_Attitude_Error.g_Error_Roll;
//积分限幅
if (g_Attitude_Error.g_ErrorI_Roll >= ROLL_I_MAX)
g_Attitude_Error.g_ErrorI_Roll = ROLL_I_MAX;
else if (g_Attitude_Error.g_ErrorI_Roll <= -ROLL_I_MAX)
g_Attitude_Error.g_ErrorI_Roll = -ROLL_I_MAX;
}
// --- PID运算-->这里的微分D运算并非传统意义上的利用前一次的误差减去上一次的误差得来
// --- 而是直接利用陀螺仪的值来替代微分项,这样的处理非常好,因为巧妙利用了硬件设施,陀螺仪本身就是具有增量的效果
outputPWM_Pitch = (s16)(g_PID_Kp * g_Attitude_Error.g_Error_Pitch +
g_PID_Ki * g_Attitude_Error.g_ErrorI_Pitch - g_PID_Kd * g_MPU6050Data_Filter.gyro_x_c);
outputPWM_Roll = (s16)(g_PID_Kp * g_Attitude_Error.g_Error_Roll +
g_PID_Ki * g_Attitude_Error.g_ErrorI_Roll - g_PID_Kd * g_MPU6050Data_Filter.gyro_y_c);
outputPWM_Yaw = (s16)(g_PID_Yaw_Kp * g_Attitude_Error.g_Error_Yaw);
// --- 给出PWM控制量到四个电机-->X模式控制
//特别注意，这里输出反相了，因为误差是反的
g_motor1_PWM = g_BasePWM + outputPWM_Pitch + outputPWM_Roll + outputPWM_Yaw;
g_motor2_PWM = g_BasePWM - outputPWM_Pitch + outputPWM_Roll - outputPWM_Yaw;
g_motor3_PWM = g_BasePWM - outputPWM_Pitch - outputPWM_Roll + outputPWM_Yaw;
g_motor4_PWM = g_BasePWM + outputPWM_Pitch - outputPWM_Roll - outputPWM_Yaw;
// --- PWM反向清零,因为没有反转
if (g_motor1_PWM < 0)
g_motor1_PWM = 0;
if (g_motor2_PWM < 0)
g_motor2_PWM = 0;
if (g_motor3_PWM < 0)
g_motor3_PWM = 0;
if (g_motor4_PWM < 0)
g_motor4_PWM = 0;
// --- PWM限幅
if (g_motor1_PWM >= g_MaxPWM)
g_motor1_PWM = g_MaxPWM;
if (g_motor2_PWM >= g_MaxPWM)
g_motor2_PWM = g_MaxPWM;
if (g_motor3_PWM >= g_MaxPWM)
g_motor3_PWM = g_MaxPWM;
if (g_motor4_PWM >= g_MaxPWM)
g_motor4_PWM = g_MaxPWM;
if (g_Fly_Enable) //允许起飞,给出PWM
PWM_Set(g_motor1_PWM, g_motor2_PWM, g_motor3_PWM, g_motor4_PWM);
else
PWM_Set(0, 0, 0, 0); //停机
}
在这段代码中，首先得到期望值与当前值的误差，然后经过积分分离与抗积分饱和处理后，计算PID输出，关键点在于三轴PID输出与四电机的融合处理，接着对运算结果进行反向清零和正向限幅处理。
我们知道四旋翼目前有两种运行模式，一种成为+模式，一种成为x模式。前者表示四旋翼的运动方向与其中一对电机的轴线重合，后者则是将前一种方式旋转45度的结果。相对而言，x模式稳定一些。但如果要完成翻跟头等特技动作，可能需要用+模式。笔者观看了网易公开课关于四旋翼的TED，他们的四轴运动方式全部是+模式。笔者在这里就不细讲+模式与x模式怎么融合，这部分网上都有，其实也不难，想好符号和力矩关系，自己都可以写出来。笔者就是这么过来的。
而对于PID的参数整定来讲，因为笔者制作的是小四轴，惯性小，很灵敏。所以P和D参数的耦合比大四轴严重很多，在调试的时候注意两者的关系。先整定P，再整定D，然后反过来迭代P，再迭代D，直到找到一个最佳值。如果发现无论如何都找不到更好的效果时，考虑降低参数，因为可能在迭代的过程中已经超过了极值。
加速度计滤波：
在前面的姿态解算部分已经提到有必要对加速度计的值进行滤波。笔者为了达到滤波的最佳效果，当没有考虑实时性时，采用了方才讨论的16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，效果确实理想很多，但是代价就是延迟较为严重；而在考虑实时性的要求之后，笔者去除了FIR低通滤波，只用了8深度的窗口滑动滤波。虽然效果来讲肯定没有前述的要好，但是对于姿态解算的误差来讲，静止时波动差不多在0.1~0.2°左右(有FIR滤波则稳定不动)。针对于本四旋翼的设计，0.1~0.2°的误差显得微不足道，所以就放弃了高阶的FIR滤波。当然，这只是在静止状态下的测试，如果打开电机，引入电机的高频机械震动，那么加速度计的值又会产生新的噪声。笔者将四旋翼拿在手上测试他的角度变化，发现在大油门时大致有4°左右的偏差，这个误差还是较为严重的。鉴于此，笔者才做FIR滤波。但是在实际飞行过程中，当只有8深度的窗口滑动滤波时，似乎可以平衡，没有拿在手上测试的4°误差。所以在这里笔者就偷懒了，直接采用8深度的窗口滑动滤波，放弃了FIR低通滤波。具体的原因，如果有网友愿意讨论可以联系我。
以下是笔者的8深度窗口滑动滤波代码，算法经过优化，减少了数组的移动和求和运算，利用了循环队列的原理避免了求和运算：
/**************************************************
函数名：void IMU_Filter(void)
说明：IMU滤波，包括加速度计的滑动滤波和陀螺仪的标定
入口：无
出口：无
备注：采用窗口滑动滤波法，长度为ACC_FILTER_DELAY
用控制周期3ms*ACC_FILTER_DELAY得到滞后时间常数
属于一阶滞后的FIR滤波器，具体的之后环节
有待对加速度计采样观察后FFT查看频谱后给出
滞后的时间常数
**************************************************/
void IMU_Filter(void)
{
s32 resultx = 0;
static s32 s_resulttmpx[ACC_FILTER_DELAY] = {0};
static u8 s_bufferCounterx = 0;
static s32 s_totalx = 0;
s32 resulty = 0;
static s32 s_resulttmpy[ACC_FILTER_DELAY] = {0};
static u8 s_bufferCountery = 0;
static s32 s_totaly = 0;
s32 resultz = 0;
static s32 s_resulttmpz[ACC_FILTER_DELAY] = {0};
static u8 s_bufferCounterz = 0;
static s32 s_totalz = 0;
//加速度计滤波
s_totalx -= s_resulttmpx[s_bufferCounterx]; //从总和中删除头部元素的值，履行头部指针职责
s_resulttmpx[s_bufferCounterx] = g_MPU6050Data.accel_x; //将采样值放到尾部指针处，履行尾部指针职责
s_totalx += g_MPU6050Data.accel_x; //更新总和
resultx = s_totalx / ACC_FILTER_DELAY; //计算平均值，并输入到一个固定变量中
s_bufferCounterx++; //更新指针
if (s_bufferCounterx == ACC_FILTER_DELAY) //到达队列边界
s_bufferCounterx = 0;
g_MPU6050Data_Filter.accel_x_f = resultx;
s_totaly -= s_resulttmpy[s_bufferCountery];
s_resulttmpy[s_bufferCountery] = g_MPU6050Data.accel_y;
s_totaly += g_MPU6050Data.accel_y;
resulty = s_totaly / ACC_FILTER_DELAY;
s_bufferCountery++;
if (s_bufferCountery == ACC_FILTER_DELAY)
s_bufferCountery = 0;
g_MPU6050Data_Filter.accel_y_f = resulty;
s_totalz -= s_resulttmpz[s_bufferCounterz];
s_resulttmpz[s_bufferCounterz] = g_MPU6050Data.accel_z;
s_totalz += g_MPU6050Data.accel_z;
resultz = s_totalz / ACC_FILTER_DELAY;
s_bufferCounterz++;
if (s_bufferCounterz == ACC_FILTER_DELAY)
s_bufferCounterz = 0;
g_MPU6050Data_Filter.accel_z_f = resultz;
}
基于NRF24L01的Bootloader：
这一块内容属于独立与四旋翼开发的部分，因为在最初设计之时，想到PID调试需要反复整定参数，就需要不断的烧写程序来变更参数，这样就需要重复的插拔连线，显得麻烦。所以笔者就在无线模块NRF24L01的基础之上，开发了Bootloader技术，使得下载程序通过无线模块下载程序到Flash中，这样极大的简化了参数整定的过程。
笔者在这里就不详细介绍Bootloader的原理了，简单点说就是在Flash中开辟两个区域：A区域和B区域。其中A区域称之为Bootloader，用以实现Flash的烧写工作，相当于代替了J-LINK；B区域就是我们运行代码的区域，也就是Bootloader将要操作的Flash区域，我们的代码就在这里运行。单片机在开机后首先运行A区域的Bootloader代码，这段代码等待NRF24L01接收二进制程序代码，在接收的同时，就一边将接收到的二进制程序代码烧写进B区域中。等全部接收完毕，同时也烧写完毕。之后通过在汇编修改栈顶指针并跳转到程序的APP代码起始位置即可。
以下为Bootloader中的APP函数跳转关键代码：
/***************************************
函数名：void IAP_Load_App(u32 appxaddr)
说明：跳到APP程序
入口：u32 appxaddr: 应用程序的起始地址
出口：无
备注：无
****************************************/
void IAP_Load_App(u32 appxaddr)
{
if(((*(vu32*)appxaddr) & 0x2FFE0000) == 0x20000000) //检查栈顶地址是否合法
{
Jump_To_App = (IAP_FunEntrance)*(vu32*)(appxaddr + 4); //用户代码区第二个字为程序开始地址(复位地址)-->详见startup.s Line61
//(vu32*)(appxaddr + 4) --> 将FLASH的首地址强制转换为vu32的指针
//*(vu32*)(appxaddr + 4) --> 解引用该地址上存放的APP跳转地址,即main函数入口
//(IAP_FunEntrance)*(vu32*)(appxaddr + 4) --> 将main函数入口地址强制转换为指向函数的指针给Jump_To_App
MSR_MSP(*(vu32*)appxaddr); //初始化APP堆栈指针(用户代码区的第一个字用于存放栈顶地址)
Jump_To_App(); //跳转到APP,执行APP
}
}
尤其注意Jump_To_App和Jump_To_App()的用法，前提是Jump_To_App本身就是一个指向函数的指针。定义：
typedef void (*IAP_FunEntrance)(void); //定义一个指向函数的指针 IAP_FunEntrance Jump_To_App;
至此，整个设计到此结束，如果有不明白的地方，欢迎留言讨论，或者直接与本作者联系QQ249442907，欢迎各大网友共同讨论学习。