四旋翼飞机制作流程(共18页)

1、硬件(yn jin)设计：总体(zngt)思路： 整个机架采用(ciyng)PCB板，将四个电机固定在PCB板的四个角，外接电池。硬件模块： 单片机、惯性测量模块(IMU)、无线通讯模块、电机驱动模块、续流二极管、电源管理模块(稳压与充放电)、直流有刷电机、大电流放电电池、遥控器。硬件选型：模块名称元件名称数量单片机STM32F103CBT61惯性测量模块(IMU)MPU6050(三轴加速度计+三轴陀螺仪)1无线通讯模块NRF24L011电机驱动模块AO3400 5.8A4续流二极管SS34 3A4电源管理模块稳压TPS79333 3.3V1充放电TP4057 USB兼容5V充电1直流有刷电机

2、空心杯有刷直流电机7*16mm4大电流放电电池250mAh 20C1遥控器JOYPAD游戏手柄1硬件工作综述： 单片机负责整个系统的协调工作；惯性测量模块(IMU)负责测量四旋翼的姿态；无线通讯模块负责四旋翼与遥控器的通讯；电机(dinj)驱动模块负责驱动电机；续流二极管负责对电机进行续流；电源管理模块中的稳压模块负责整个系统的供电，电源管理模块中的充放电模块负责对电池充电；有刷电机负责提供四旋翼的飞行动力；大电流放电电池负责四旋翼的能量来源；遥控器负责对四旋翼进行遥控和控制。硬件(yn jin)设计功能模块图：实际(shj)效果图与相关参数：尺寸(ch cun)：对角电机轴距10 x10cm

3、重量(zhngling)：33.2g(带电池)软件设计：总体(zngt)思路： 惯性测量模块(IMU)测量出当前飞机的三轴加速度与三轴角速度并传送给单片机处理，由单片机进行基于四元数的姿态解算，求解出当前飞机的pitch、roll、yaw三个角度值，然后根据这三个角度经过PID控制运算，输出四路PWM控制四个直流有刷电机的加减速从而达到飞机的平衡悬停。 其中，惯性测量模块(IMU)的加速度计由于噪声比较大，所以需要对其进行滤波处理；而遥控器则是对飞机进行实时的姿态控制；最后由于四旋翼制作的特殊性，在调试PID参数阶段会频繁的烧写程序，鉴于此，笔者开发了基于(jy)NRF24L01的Bootlo

4、ader技术，免除了烧写Flash的物理连线限制，可实现远程程序一键下载。姿态(zti)解算： 姿态解算属于四旋翼制作的核心部分，如果姿态解算能够实时的反应出飞机的状态，那么对于控制来讲就相对来说比较容易了。而姿态结算所要做的事情就是两个坐标系之间的正确转化(地理坐标系与载体坐标系)，这种转化有很多种表示方法，例如欧拉角法、方向(fngxing)余弦矩阵法、四元数法、旋转矢量法等。笔者采用的是应用广泛的四元数法，而旋转矢量法则是一种基于四元数法的改进四元数法。 四元数本是用于描述四维空间向量的一种方法，对于他的线性变换也就是在四维空间中的拉伸和旋转，显而易见，我们用四元数的向量乘法来表示三维空

5、间中的旋转是绰绰有余的。 通过惯性测量模块(IMU)传送过来的当前飞机的三轴加速度和三轴角速度的值，这样一个三维的向量，转化为四维向量，然后在四维空间中做线性变换(也可以说在三维空间中旋转)后输出，利用四元数与欧拉角的关系(一定要注意旋转顺序)，将当前四元数转换为欧拉角pitch、roll、yaw即得到当前飞机的姿态。以下给出笔者姿态融合的代码，该代码网上都有，笔者在这里做了些许注释，方便理解。/*函数名：void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)说明：IMU单元数据融合，更新姿态四元数入

6、口(r ku)：float gx陀螺仪x分量float gy陀螺仪y分量(fn ling)float gz陀螺仪z分量(fn ling)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;/在当前载体坐标系中，重力分量在三

7、个轴上的分量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参数,因为

8、这里归一化了recipNorm = invSqrt(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm;/将当前(dngqin)姿态的重力在三个轴上的分量分离出来/就是方向余弦旋转矩阵的第三列,注意是地理坐标系(n系)到载体坐标系(b系)的,不要(byo)弄反了.如果书上是b系到n系,转置即可/惯性测量器件测量的都是关于b系的值，为了方便，我们一般将b系转换到n系进行导航参数(cnsh)求解。但是这里并不需要这样做，因为这里是对陀螺仪进行补偿halfvx = g_q1 * g_q3 - g_q

9、0 * g_q2;halfvy = g_q0 * g_q1 + g_q2 * g_q3;halfvz = g_q0 * g_q0 - 0.5f + g_q3 * g_q3;/计算由当前姿态的重力在三个轴上的分量与加速度计测得的重力在三个轴上的分量的差,这里采用三维空间的差积(向量积)方法求差/计算公式由矩阵运算推导而来 公式参见/wiki/Cross_product 中的Mnemonic部分halfex = (ay * halfvz - az * halfvy);halfey = (az * halfvx - ax * halfvz);halfez = (ax * halfvy - ay *

10、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/

11、不进行积分运算，只进行比例(bl)调节g_integralFBx = 0.0f;g_integralFBy = 0.0f;g_integralFBz = 0.0f;/直接应用比例调节(tioji)，修正陀螺仪的值gx += g_twoKp * halfex;gy += g_twoKp * halfey;gz += g_twoKp * halfez;/以下(yxi)为四元数微分方程.将陀螺仪和四元数结合起来，是姿态更新的核心算子/计算方法由矩阵运算推导而来/. 1 /q = - * q x Omega 式中左边是四元数的倒数,右边的x是四元数乘法,Omega是陀螺仪的值(即角速度)/ 2/ ./

12、q0 0-wx-wy-wzq0/ ./q1wx 0wz -wyq1/ . = * /q2wy -wz 0wx q2/ . /q3wz wy-wx0 q3gx *= (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 *

13、gx);g_q3 += ( qa * gz + qb * gy - qc * gx);/单位(dnwi)化四元数,意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换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;/四元(s yun)数到欧拉角转换，转换顺序为Z-Y-X,参见.pdf一文(y wn),P24/注意此

14、时的转换顺序是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; /Yawg_Roll = asin(-2 * g_q1 * g_q3 + 2 * g_q0* g_q2) * 180 / PI; /Rollg_Pitch = atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1

15、* g_q1 - 2 * g_q2* g_q2 + 1) * 180 / PI; /Pitch 注意其中的快速开方函数来自维基百科，精度0.175%。并且注意输入的陀螺仪必须是弧度制(这一点在进入函数的时候已经做了转换)，否则姿态解算是错误的。 针对上述代码我还想说明一个笔者发现的问题：有很多网友和许多外国的四轴代码(CrazyFlie)在这个姿态解算的加速度计部分都做了零点校准处理。意思就是在开机的时候读取一定次数的加速度计的值，然后平均一下，得到一个初始状态的偏移量，最后在输出的时候加速度计减掉这个值，然后再给到姿态解算代码部分。而笔者在刚开始移植代码的时候是没有做这个零点校准处理的(当然

16、，陀螺仪必须要做零点处理，因为陀螺仪的原理，必须要在静止时输出为0)，包括现在依旧没有对加速度计做零点校准处理，仍然可以获得较为实时的姿态。 那么显然对加速度计做不做零点校准处理都是可行的。为什么呢？经过我的分析，首先在这段代码中，我们对加速度计进行了归一化处理，我们知道在数学当中，对数值进行单位化意味着长度不变而只改变方向，对于加速度计来讲，他的”长度”就是加速度的大小，他的”方向”就是加速度的方向。所以我们对加速度计做了单位化之后，其加速度的大小我们就无从而知，但是我们利用了他的方向来进行姿态解算。就这一点来讲，无论我们做不做零点校准处理，进来的加速度的值始终都抛弃掉大小，并关注方向，与零

17、点校准处理无关。另一方面，由于我们生活在重力场里面，那么加速度计在静止状态下测量的是重力加速度，会有一个g的输出。而我们理想的加速度计应该是输出0，而在有加速度的时候应该输出相应的加速度，但是现实是我们生活在一个重力场里面，所以必定有一个重力输出。那么零点校准处理的核心就是我们对于加速度计的理解问题，如果做了零点校准处理，那么我们使用的加速度计就成为了”真正的”加速度计，当有重力的时候他输出为0，有加速度的时候就输出加速度；当我们没有做零点校准处理的时候，那么我们使用的加速度计就成了”重力”加速度计。但是细心的你其实(qsh)可以发现那个并不是真正的加速度计，我将传感器反过来放的话输出就不是0

18、了，而是z轴上的负值输出。显然这个零点标准处理做的不那么标准。况且这种处理方式是非常粗糙的，因为加速度计的噪声十分的大，数据波动非常厉害，我做了16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，其输出仍然有14左右的波动。可见加速度计确实不好处理，除非用Kalman滤波。 鉴于以上两点原因，本人就没有做加速度计的零点校准处理(chl)。当需要测量飞机的加速度大小并实现定位时，那么就需要做零点校准处理了。而当我们只需要解算姿态，那么加速度计就不需要做零点校准处理。 以上是笔者对于加速度计零点(ln din)校准处理的愚见，如有错误，还望共同学习。 最后想说明一点的是关于陀螺仪的数据

19、转化，笔者在最开始编写姿态解算代码时，发现角度的变化与实时姿态差了好几个数量级，体现出来的现象就是稍微移动一下飞机，姿态就呼呼的飞速变化。之前一直以为是姿态解算中的Kp和Ki的系数问题，后来才发现是陀螺仪的数据没有转化成标准单位(/s)输出，没有参看pdf上的量程单位，所以没有做数据转化处理，在这里提醒一下各位，不要犯笔者这种低级错误了。PID控制(kngzh)： PID控制属于自动化领域，由于笔者的本科出生于自动化专业，所以对于自动控制原理有一点理论上的认识。P是比例，I是积分，D是微分，这是最基本的定义。对于一个系统，我们想要控制他，目前的理论是引入负反馈，这个概念相当重要，是由维纳提出来

20、的。意思是，将输出引入到输入端，并且用输入减去输出，这就是著名的负反馈系统。很显然，我们要做的是输出跟随输入，使得系统可控。也就是说要求输出和输入的误差为0，即输出等于输入。在实际的系统中，输出与输入肯定是存在误差的，这种误差就通过PID来控制使得满足输出与输入误差为0。当系统由于干扰出现误差时，此时的P参数就起到了“立竿见影”的作用，将当前系统误差第一时间反应出来，也就是当前误差多少，我就给你多少输出值来补偿你的误差。这种调节方式的特点是快速而有劲，相应来说就是发散且不稳定的；而D参数则具有一种预见性，这种预见性可以提前预知系统的行为，比如距离设定值是越来越远还是越来越近，前者D的作用越强，

21、后者D的作用越弱。可以发现(fxin)D参数与P参数具有一定的互补性质，P会引起发散，而D则是抑制发散，使系统非常敏感；最后I参数是积分，在连续系统中是时间的积分，在数字系统中是时间的累加。这种累加无疑会造成系统的不稳定，如果系统长时间处于不平衡的位置，那么由于时间的累计，I的作用会变得越来越强，甚至超过了P的作用，那么系统必定失控。但是他的作用有时候确实不可忽略的：消除静差。 在这里笔者尤其提醒大家一点，如果此时系统的输出达到了我们给定的期望值，也就是说输出与输入误差为0，即现在的PID控制器输入0，所以输出也是0，也就是说此时的执行机构是不会输出的，让设备处于(chy)自由运动阶段。而非我

22、们认为的当你观察到一个系统处于稳定运行并达到给定值的时候，他的执行机构是一直在输出的，这是错误的。 浅谈完PID，对于四旋翼的控制，笔者采用的就是经典控制论中的PID控制，利用的是期望姿态(pitch=0,roll=0,yaw=0)与当前姿态的误差，通过PID的控制作用输出四路不同的PWM驱动电机让飞机调整自己的姿态满足当前姿态与期望姿态的误差为0的目标，这也是PID控制器的目标。以下是笔者的PID控制代码：/*函数(hnsh)名：void Quadrotor_Control(const float Exp_Pitch, const float Exp_Roll, const float Ex

23、p_Yaw)说明：四旋翼控制函数，用于PWM计算(j sun)和输出入口(r ku)：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;/ -

24、 得到当前系统的误差-利用期望角度减去当前角度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

25、(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;elsePWM4_LED = 1;/ - 积分运算(yn sun)与积分误差限幅if (fabs(g_Attitude_Error.g_Error_Pitch) 在姿态误差角小于20时引入积分/Pitch/累加误差(wch)g_Attitude_

26、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) =

27、 ROLL_I_MAX)g_Attitude_Error.g_ErrorI_Roll = ROLL_I_MAX;else if (g_Attitude_Error.g_ErrorI_Roll 这里的微分D运算并非传统意义上的利用前一次的误差(wch)减去上一次的误差得来/ - 而是直接利用陀螺仪的值来替代微分项,这样的处理非常好,因为巧妙利用了硬件设施,陀螺仪本身(bnshn)就是具有增量的效果outputPWM_Pitch = (s16)(g_PID_Kp * g_Attitude_Error.g_Error_Pitch + g_PID_Ki * g_Attitude_Error.g_Err

28、orI_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控制量到四个电机(dinj)-X模

29、式控制 /特别注意，这里输出反相了，因为误差是反的 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

30、- 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 = 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_

31、PWM = g_MaxPWM;if (g_motor4_PWM = g_MaxPWM)g_motor4_PWM = g_MaxPWM;if (g_Fly_Enable)/允许(ynx)起飞,给出PWMPWM_Set(g_motor1_PWM, g_motor2_PWM, g_motor3_PWM, g_motor4_PWM);elsePWM_Set(0, 0, 0, 0);/停机 在这段代码中，首先得到期望值与当前值的误差，然后经过积分分离与抗积分饱和处理后，计算PID输出，关键点在于三轴PID输出与四电机的融合处理，接着对运算结果(ji gu)进行反向清零和正向限幅处理。 我们知道四旋翼目前

32、有两种运行模式，一种成为+模式，一种成为x模式。前者表示四旋翼的运动方向与其中一对电机的轴线重合，后者则是将前一种方式旋转45度的结果。相对而言，x模式稳定一些。但如果要完成翻跟头等特技动作，可能需要用+模式。笔者观看了网易公开课关于四旋翼的TED，他们的四轴运动方式全部是+模式。笔者在这里就不细讲+模式与x模式怎么融合，这部分网上都有，其实也不难，想好符号和力矩关系，自己都可以写出来。笔者就是(jish)这么过来的。 而对于PID的参数整定来讲，因为笔者制作的是小四轴，惯性小，很灵敏。所以P和D参数的耦合比大四轴严重很多，在调试的时候注意两者的关系。先整定P，再整定D，然后反过来迭代P，再迭

33、代D，直到找到一个最佳值。如果发现无论如何都找不到更好的效果时，考虑降低参数，因为可能在迭代的过程中已经超过了极值。加速度计滤波(lb)： 在前面的姿态解算部分已经提到有必要对加速度计的值进行滤波。笔者为了达到滤波的最佳效果，当没有考虑实时性时，采用了方才讨论的16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，效果确实理想很多，但是代价就是延迟较为严重；而在考虑实时性的要求之后，笔者去除了FIR低通滤波，只用了8深度的窗口滑动滤波。虽然效果来讲肯定没有前述的要好，但是对于姿态解算的误差来讲，静止时波动差不多在0.10.2左右(有FIR滤波则稳定不动)。针对于本四旋翼的设计，0.1

34、0.2的误差显得微不足道，所以(suy)就放弃了高阶的FIR滤波。当然，这只是在静止状态下的测试，如果打开电机，引入电机的高频机械震动，那么加速度计的值又会产生新的噪声。笔者将四旋翼拿在手上测试他的角度变化，发现在大油门时大致有4左右的偏差，这个误差还是较为严重的。鉴于此，笔者才做FIR滤波。但是在实际飞行过程中，当只有8深度的窗口滑动滤波时，似乎可以平衡，没有拿在手上测试的4误差。所以在这里笔者就偷懒了，直接采用8深度的窗口滑动滤波，放弃了FIR低通滤波。具体的原因，如果有网友愿意讨论可以联系我。以下是笔者的8深度窗口滑动滤波代码，算法经过优化，减少了数组的移动和求和运算，利用了循环(xnh

35、un)队列的原理避免了求和运算：/*函数名：void IMU_Filter(void)说明：IMU滤波，包括加速度计的滑动滤波和陀螺仪的标定入口：无出口：无备注：采用窗口滑动滤波法，长度为ACC_FILTER_DELAY用控制周期3ms*ACC_FILTER_DELAY得到滞后时间常数属于一阶滞后的FIR滤波器，具体的之后环节有待对加速度计采样观察后FFT查看频谱后给出滞后的时间常数*/void IMU_Filter(void)s32 resultx = 0;static s32 s_resulttmpxACC_FILTER_DELAY = 0;static u8 s_bufferCounte

36、rx = 0;static s32 s_totalx = 0;s32 resulty = 0;static s32 s_resulttmpyACC_FILTER_DELAY = 0;static u8 s_bufferCountery = 0;static s32 s_totaly = 0;s32 resultz = 0;static s32 s_resulttmpzACC_FILTER_DELAY = 0;static u8 s_bufferCounterz = 0;static s32 s_totalz = 0;/加速度计滤波(lb)s_totalx -= s_resulttmpxs_bu

37、fferCounterx;/从总和中删除头部元素的值，履行(lxng)头部指针职责s_resulttmpxs_bufferCounterx = g_MPU6050Data.accel_x;/将采样值放到尾部指针(zhzhn)处，履行尾部指针职责s_totalx += g_MPU6050Data.accel_x; /更新总和resultx = s_totalx / ACC_FILTER_DELAY; /计算平均值，并输入到一个固定变量中s_bufferCounterx+; /更新指针if (s_bufferCounterx = ACC_FILTER_DELAY) /到达队列边界s_bufferC

38、ounterx = 0;g_MPU6050Data_Filter.accel_x_f = resultx;s_totaly -= s_resulttmpys_bufferCountery;s_resulttmpys_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_

39、MPU6050Data_Filter.accel_y_f = resulty;s_totalz -= s_resulttmpzs_bufferCounterz;s_resulttmpzs_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 =