大四轴四旋翼制作思路算法过程

作者：WZY

时间 2013 年 2 月 2 日 11:57:37

项目进展了这么久第一次写日志，总结一下这一段时间的经验。首先，我通过李想的 stm32 教学视频对 stm32 进行了简单的了解，发现 stm32 用起来比 51 都方便，原来一直被“arm 难学”误导了，单片机只是工具，真正编算法才是难点，所以以后在做项目时不应该过分考 虑单片机是否好用这个问题，只要适合项目的单片机就应该拿来用。

李想视频里编译环境是 MDK，所以在编程时一直使用 MDK 编译，感觉和学 51 时用的 keil 一模一样，比较好上手。由于项目所要做的第一步是要做姿态解算，所以在初步了解 stm32 原理以后，就开始从 MPU6050 传感器读数据。MPU6050 是一个集成了加速度计和陀螺仪的 传感器，使用 I2C 和单片机进行通信，总体来说读取数据还是比较简单的。但是也遇到一些 比较烦人的问题，卡了好久。一个是变量类型搞错了

这是一段读取 MPU6050 陀螺仪 x 轴数据的程序，先通过 I2C 函数把陀螺仪 X 轴数据的低 8 位和高 8 位读取出来，分别放入 BUF[0]和 BUF[1]中，在通过移位把两个数据合成一个数据传 入 G_X 变量，G_X 是一个 short 型变量，而 imu_measure.gx 是一个 double 型变量，编译器 编译是自动把 G_X 转成 double 型除以 16.4 赋值给 imu_measure.gx。曾经犯过一个错误就是 把 BUF 数组数据相合后直接除以 16.4 然后赋值给 imu_measure.gx，结果通过串口传回来的 数据就出现了错误。以后要避免这种错误。

在新添加文件后要记着把文件添加进工程，否则编译会出错。 至于为什么除

以 16.4，参考《MPU-6050 寄存器映射》第 32 页

在 MPU6050 初始化时把 Full Scale Range 设置为 ±2000°/s，所以灵敏度为 16.4 LSB/°/s， 所以在读出寄存器值的基础上除以 16.4 就是角速度。

而对于加速度，设置为±8g，灵敏度为 4096，所以在读出值的基础上除以 4096 就是加速 度，单位为 g，再乘以 9.8 单位就是 m/s^2，所以 8192/9.8=835.066，除以这个值就是加速度 （m/s^2）。同理，在理解 MPU6050 初始化的设置时，参考这本手册即可。

要设置为+-4g 即 0000100=0x08

还有一个就是 extern 变量的使用。正确的用法是在初始化的地方定义（函数外定义，如 果在函数内则会是局部变量），然后在 External_Variable.h 文件中用 extern 关键字再定义一遍 （注意，这里不要赋值），告诉编译器，这个变量在其他地方已经定义过，直接使用即可。 然后需要用到全局变量的文件开头进行#Include “External_Variable.h”，就可以了。

MPU6050 传感器初始化时，使用 I2C 总线函数即可，例如：

Single_Write(MPU6050_Addr,PWR_MGMT_1, 0x00);

//解除休眠状态

分别写入 MPU6050 地址，寄存器地址，和写入的值即可。出现一个问题就是 MPU6050 的 I2C 地址在手册上写的是 7 位固定，为 0x68（1101000），但有的例程上给的是直接写入 0x68， 实际在使用过程中写入 0xD0（11010000）才有反应，即 7 为固定位加一位硬件设置的地址 位（BIT0=0）。

在 MPU6050 初始化后就可以不停的读取传感器值进行姿态解算了。在姿态解算前，我先 对四元数进行了初始化，即飞控板在静止状态下通过加速度计解算欧拉角，再通过四元数公 式计算（即函数 quat_init()）。其实感觉这里不初始化也可以，因为姿态解算速度很快，第 一次的值对以后影响并不大。

随后就开始真正的姿态解算了，首先要对读取的传感器值进行移动均值滤波。定义了

Filter_ACC 结构体，先把读取值加到这个结构体变量里，每加一次，积分标记位加 1。当定 时器中断触发。姿态解算周期到来时，进入 if(IMUupdate_flag == TRUE){}里面求平均值并对 变量清零，完成移动均值滤波。

滤波过后，就开始用 IMUupdate（）函数进行解算，这个函数是参考

http://www.amobbs.com/forum.php?mod=viewthread&tid=5492189&highlight=??·%

E8????ˉ?ˉ?

在实际编程中发现，在输入 IMUupdata()函数前如果角速度不除以一个比例系数，解算出 的姿态角就会出现超调或者，但是如果系数过大就会出现调不够的现象，所以实验的几次以 后找到了一个经验值。

进行使用的，基本原理是把加速度计解算出的姿态和上次解算出的姿态（认为是正确的）进 行向量叉乘，得到两个向量的差，然后把这个差乘以比例系数和角速度数据融合得到认为是 正确的角速度里，将正确的角速度代入到龙格库塔方程里推算出当前正确的姿态。方程里所 需的半周期 halfT 是由 TIM2 中断产生的（TIM2 每 2ms 中断一次，即 T=2ms，halfT=1ms）。 在这个过程里还引入里积分环节来消除静差。

比如陀螺 X 轴读取时始终有个 -32 °/s 的误差存在，在姿态解算后，滚转角始终不是平 的，会有一个误差，但是会发现这个误差会逐渐减小，这就是积分环节在起作用。

其实在一开始，通过上位机看 stm32 姿态解算的状态并不是很好，最后发现是 uart1 传输 函数里面有个 Delayms（1），晕死，本来对速度的要求就很高，结果每传输一次就延时 1ms， 不出问题才怪。结果吧这个延时函数去掉以后干脆不出数据了，卡了好久，发现是自己思维 进入死胡同了！忘记串口传输完毕是要等待传输结束的：

结果加上这句话后形式一片大好~顺利的解算出了姿态，而且效果很好。

这里使用的上位机软件是阿莫论坛上一位大虾提供的，很好用，不足的地方就是传送速率过 快会卡，不能迅速显示，不过功能还是挺强大的，可以数据保存截图什么的。

下一步的任务就是使用 HMC5883 对偏航角进行修正，由于加速度计不能读出偏航角的数 据，所以偏航角由于陀螺仪误差的缘故会一直向一个方向偏。再然后就是读取气压高度。

2013 年 2 月 3 日 23:14:52

今天主要解决的是全姿态罗盘。说来很惭愧，大部分时间卡在一个很细小的问题上，就是 HMC5883L 的 I2C 总线地址上。一开始翻看 MWC 的程序，磁阻仪的地址上 0x1E，于是用 I2C 向磁阻仪写入 0x1E，结果一点反应都木有啊~试了各种方法，一度以为是 MPU6050 的 I2C 辅助位没有设置好（其实设置是没问题的，让 MPU6050 的 I2C_Master 功能 disable，ByPass 功能打开，主芯片可以直接控制 HMC5883L，这两个寄存器分别在 0x37 和 0x6A 两个寄存器

里），在这里设置了好久。后来突然想到上篇日志里写的地址移位的问题，查看 MWC 飞控 的 I2C 写入函数，果然地址进去以后先要左移一位，顿时泪奔，0x1E 左移一位是 0x3C，按 这个地址写入瞬间各种正常~

但是在磁阻仪数据读出后，立马又发现了一个问题。飞控板转动 90°，上位机显示实际 的转动是小于 90°的。然后就各种逛论坛啊，各种没思路。于是决定先睡一会儿，困死了， 结果刚躺下立马就有思路了，果然过于专注一个问题思维容易进入死胡同。其实磁阻仪的每 个轴的数据并不是关于 0 对称的，这是一个误差。另一个误差是每个轴的变化量是不同的， 以 X,Y 轴为例，如果以想两轴的数据确定一个点（X,Y），那么如果把飞控板转一圈得到的数 据画出的圆其实是一个椭圆，而且和原点存在偏移。如图：

蓝色点是飞控板在 XY 平面旋转一圈采集到的数据，红色圆圈是用 matlab 拟合得到的圆。 通过 matlab 拟合，我们得到了下图数据： 其中数据从左到右分别为椭圆圆心的坐标（X,Y），然后是椭圆长半轴和短半轴的长度。

所以我们可以利用这些数据对磁阻仪进行校正。

其中椭圆圆心的偏移量分别为磁阻仪 XY 轴数据的偏移量，用原始数据减去这个偏移量即可。 然后由于 Y 轴比 X 轴长，所以把 X 轴数据乘以长半轴和短半轴的商来校正 X 轴数据，这样 XY 轴数据就变成一个没有偏移的圆，通过 atan2 函数就可以求方向角了。

如图，我把飞控板旋转 90°，可以看到上位机读到的数据从 180°左右变到了 270°左右。

但是接下来又存在一个问题，就是这个方法只能在水平面内使用，如果有滚转或俯仰角， 通过磁阻仪计算得到的偏航角就会有很大幅度的变化。然后又是一顿查资料，找到几篇关于 全姿态罗盘的论文，参考上面的把磁阻仪三轴数据变化到平面的公式编入程序中发现，根本 没有什么变化。可能是这个飞控板的传感器安装和论文里所用的安装方位不一样的缘故吧。 然后又是一阵挠头，突然想到 MWC 飞控里应该有相应的程序。于是翻 MWC 的程序，结果 还真找到了。

heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z)

上面是 MWC 全姿态罗盘的程序，Est.V.axis 分别是重力向量在机体坐标系下在机体坐标系各 轴的分量（即机体坐标系下重力向量），EstM.V.axis 同理为机体坐标系下磁场的向量。用这 个坐标变换并用 atan2 函数就可算出偏航角。

按照这个思路把程序写出来并实验了一下。

下图中红色为偏航角，黄色为滚转角。可以看到当滚转角发生变化时，偏航角只发生了小幅 的变动，可以看出全姿态罗盘算法起作用了。而偏航角的小幅变动我推测是由于 Z 轴没校正 好的缘故。Z 轴的校正没有用 matlab 拟合，只是简单的找了最大值和最小值算偏移量（由 于只是简单的手动旋转，可能没有找到真正的最大值和最小值），然后用 Z 轴数据的量程和 Y 轴数据的量程做比较得出比例系数。其实如果把三轴数据统计会得到一个椭球体，然后用 matlab 拟合得到椭球体的原点偏移和各轴的长度，相信可以得到更准确的磁阻仪校正。

至于如何用 matlab 拟合，是从论坛上找到的现成的程序，改天把它改成椭球体的拟合。 数据处理是通过串口助手读取数据存到 word 里，然后将文本准换成表格，以空格分列，然 后把各列数据存到 Excel 里，最后用 matlab 读取数据。由于我的 matlab 貌似存在问题，得 用主程序图标以管理员方式运行，进入后把目录设置成拟合程序的目录，然后用 XY=xlsread(‘file’)函数读取数据。

在程序里，今天做的最大的修改就是把均值滤波去掉了。因为发现在姿态解算前积分量只 积分了一次，也就是说就是当前读取传感器的值，所以干脆去掉了。而且传感器最快的读取 速度也就 1KHz，姿态解算的速率是 500Hz，没有必要滤波了。如果以后装到飞机上发现噪 声严重的话再滤波和降低姿态解算速率。

2013 年 2 月 4 日 22:27:33

今天做的主要一件事就是把陀螺仪和磁力计的数据进行融合。因为陀螺仪在很微小的转动 时几乎察觉不不到飞控板的转动，所以需要磁力计的数据进行校正。而磁力计虽然昨天做了 全姿态罗盘的变换，但是在存在俯仰和滚转角时，偏航角任然有扰动，所以又需要陀螺仪来 校正，所以我要把这两个传感器的数据相互融合来取长补短。

其实一开始的想法是通过磁力计解算偏航角，然后把加计和陀螺仪解算的俯仰滚转角和磁 力计解算的偏航角一并转成四元数，用于下次四元数解算。但是实际效果并不是很好。纠结 了一会儿想到一个办法就是把加计和陀螺仪解算的偏航角和磁力计解算的偏航角融合，然后 引入一个积分环节，每次积分的值是陀螺仪解算的偏航角和最终解算偏航角的差，用这个积 分环节去校正由于加计和陀螺仪解算的偏航角的静差（这个偏航角有累积误差）。

这个方法试验了一下效果不错，但是就是存在一个问题。比如由 180°转到 360°（0°）， 由于当磁力计的偏航角到达 360°时，加计和陀螺仪的偏航角未必到达 360°，所以就会出 现一个现象，再稍微顺时针转一下，磁力计的偏航角又变成几度，而加计和陀螺仪的偏航角

还在 300 多度。这样积分环节就会把最终解算的偏航角逆时针转到磁力计的偏航角（大概几 度那里）。这是一个很不好的现象，所以我在这个基础上对算法又进行了改进。由于我们需 要的是陀螺仪数据，所以我们可以直接从角速度入手，如下图：

把角速度计和磁力计数据融合，gz 为角速度数据，乘以负数是和定义的转动方向有关， 我定义的是顺时针转动角度从 0°变到 360°，而 gz 数据正好相反，所以要乘负数。由于每 2ms 调用一次 IMUupdata()，所以 gz*0.002 就是 2ms 转过的角度，对此积分就是实时的角度 把磁力计解算角度和上次解算的偏航角做差得到偏航角的误差，乘以一个系数 0.002（经验 值）再积分用来修正角速度计的累积误差实验表明，角速度计再快速转动时误差较小，但是 转动速度较慢时通过积分几乎得不到偏航角的变化，此时就需要用磁力计校正通过数据融合， 即避免了角速度计的累积误差又可以用角速度计解算的角度校正滚转俯仰时磁力计产生的 误差（从±15°减小到±3°）

这样就很好的避免了上述现象。效果如下：

上图中黄色是滚转角，蓝色是俯仰角（左面图例有误）。当我把飞控板做了三次滚转和三

次俯仰动作，可以看到偏航角变化幅度很小。滚转时偏航角的变化是由于用手滚转时不可避

免的会影响偏航角。基本上误差在±3°以内，而原来未用陀螺仪校正时可以达到±15°， 所以，数据融合起到了很好的效果。

程序中变化较大的就是原来提到的陀螺数据除掉的那个系数，如下图：

这句代码被删掉了，这个系数也被直接放在 MPU6050 的宏定义里：

16.4*35=574 16.4 是陀螺仪灵敏度，35 是系数（本来应该是 57.3，即角度值转化为弧度制， 但实验发现用 35 效果较好）。

原来的除以一个系数后来发现是由于我没把角度值转化成弧度制，看来以后还要细心。

然后我给 BMP085 写了驱动，其实 BMP085 一开始并不需要对其寄存器写一些值进行初始 化，而是一开始从 BMP085 的 EEPROM 中读取一些值用来以后的数据校正。在使用 BMP085 时直接向寄存器写值读取即可。但是 BMP085 有个比较麻烦的地方就是，当你给寄存器写入 值准备读取后，它会有个转换时间，如果你在这里加延时等待，就会大大减缓飞控的主频率。 所以我还是用了原来使用超声波计的方法，用一个标记位，先写入值准备读取然后跳过去执 行其他的命令，当循环几次过来发现标记位触发然后接着读取，这样就会节约很多的时间。

上面是一段读取代码，BMP085_Read_flag 就是标记位，从 1 到 4 中间隔了三段，每段 2ms （IMUupdata 函数的调用周期），这样从 BMP085_Read_flag=1 到 BMP085_Read_flag=4 中间 就隔了 6ms，而手册上写的读取时间是 4.5ms，所以可以读取。

不过问题就是，读取后的结果有点诡异。

不知道哪里出错了，等明天用普通的延时函数读取一下，看是不是这种方法不能用。

2013 年 2 月 5 日 13:08:50

OK，忙了一天继续写日志，感觉快养成一个习惯了。 首先今天上午在实验飞控的时候发

现飞控在 0°的时候仍然存在当磁力计偏航角先从 0°

多变到 350°多时，融合后的偏航角会从 0°多顺时针转到 350°多，也就是它绕了一个大 圈去校正误差。这个问题上文提到过，但是当时以为已经解决了，现在看来还是没有，只是 原来逻辑出错了。

所以我又设计了新的代码解决这个问题，那就是如果误差大于 180°就把积分值反方向加， 这样就不会绕大圈校正误差了。代码如下：

把程序拷进去以后发现这个问题被很好的解决了，当我在 0°左右变化时，比如从 0°多 变到 350°多，偏航角不会再绕一个大圈去校正了，效果如下：

接下来是卡了一天的 BMP085 数据读取问题，接着昨天的那个两个大气压，各种改程序啊， 然后参考 MWC 的还用外部中断判断是否转换完毕，各种调不通啊。。。还读出过 5000 多 Pa 的气压，海拔-2000 米什么的。

后来灵感一来发现，我由于在把别人程序拷过来的时候嫌麻烦（别人程序和自己的定义不 一样），把 uint_32 什么的强制转换直接去掉了，因为之前程序里也有好多强制转换直接忽 略也没出什么问题，于是我把这些强制转换都加上，uint_32 对应的是 unsigned long。发现 数据就对了，看来调 bug 还是得靠憋灵感。。。

不过气压即使读出来发现气压计精度真的很差，静止不动的情况下从一开始的 0m 能飘到 20 多 m，无语，看来以后当普通的海拔高度计用吧。定高用更精确的气压计或者超声波什 么的。

2013 年 2 月 6 日 23:12:01

今天最大的改动是先把所有的主循环任务放到一个新的函数 loop()里，以便以后添加新的 任务

今天发现一个容易出错的问题就是 MDK 编程里往函数里输入数组直接输入数组名字即可， 不需要加[]，加上反而会报错。

还有一个卡了好久的问题就是把滑动均值滤波函数和数据处理函数放到ARHS函数里上位 机就没有三个角度的信息，好奇怪，调了好久也没调通，但是只要放在 READ_MPU6050()函 数里就没问题，索性就放到这个函数里，反正读取数据就要连带滤波和数据处理。

今天最大的加入就是把 PWM 输出模块加进去了，虽然是直接拷贝别人的，不过大概看了 一下发现也不是很难。基本原理是用 TIM4 定时器先预分频，72M 降到 24M，然后设置一个 计数上限 1000，即 PWM 的频率是 24M/1000=24k Hz，然后 TIM4 有 4 个通道，每个通道有 寄存器，用通道寄存器和总寄存器比对输出 PWM，有点类似 8051F 的 PCA 高速输出模式。

但是又出现一个问题，定时器的预装填值到底用不用减 1，PWM 例程里 1000 上限装填的 是 999，但是更多的例程是不减 1 的，以后有示波器专门实验一下。

2013 年 2 月 8 日 21:39:33

基本上飞控已经初具雏形，现在 PWM 输出和输入都加上了，现在 PWM 输入调通了，接 下来再调 PWM 输出。

在编程中发现，每个通用定时器（2~6）都有四个通道，相当于 PCA，非常强大，可以产 生四路 PWM 或者检测输入的四路 PWM。不过每个定时器的端口是由限制的，芯片是定义

好的。参见手册（《STM32 中文参考手册》）81 页。其中，我用 TIM2 输入 PWM，所以对 应 PA0~3，用 TIM4 输出 PWM，对应 PB6~9。

而原来产生任务时钟基的定时器变为了 TIM3 任务，留出 TIM2 给 PWM 输入，其实用基本 定时器 6 或 7 就可以给任务产生时基，但是还不会配置基本定时器，留给以后改变吧。这样 可以把 TIM3 通用定时器留给其他功能。毕竟简单任务就应该配置简单定时器

然后我在程序中加入了滑动均值滤波器，基本原理是依次更新数组的数据，然后每次调用 都把数组所有数据相加再除以数组大小。代码如下

接下来是头疼了好久的 PWM 输入的问题，程序是参考的 MWC 飞控。MWC 飞控接收遥 控信号给了两种方式，一种是用一个端口检测一路 PPM 信号，另一种是用 8 个端口依次扫 描每个端口的 PWM 脉宽。这里我采用了后一种，这样可以直接使用遥控接收机的信号，否 则要用 PPM 信号需要从接收机里的一个引脚里引出 PPM 信号，或者用解码器，都比较麻烦。

MWC 的流程是先初始化 TIM2，用 TIM2 的四个通道进行读取。当时犯了一个错误就是 MWC 程序 TIM2 已经 GPIOA 端口的 APB 总线时钟在程序其他地方已经打开了，所以程序这 里没打开时钟的代码，而我直接用人家的代码没有任何反应，后来才发现这点赶紧把 GPIO， NVIC，TIM2 的初始化代码补全，然后就正常了。

MWC 的端口扫描的基本原理是触发 TIM2 中断后进入中断，然后来一个 for 循环循环判断 四次（因为有四路输入），看是哪个通道产生的中断，判断出以后用一个 switch 语句把捕 捉到的高电平（或低电平）时间赋给变量 val，然后根据 state->state 标记位判断是高电位时 间计数还是低电平时间计数，分别赋给 state->rise 和 state->fall。当然，要获取一个 rise 一 个 fall 是两次进入中断函数完成的。然后他判断这两个计数大小，相减或者求相减的补数得 到高电平时间。这是一个非常巧妙的方式。

但是一开始的时候整个过程非常占用 CPU 时间，以至于 PWM 输入功能一打开，CPU 只执 行 PWM 输入功能，其他的都不执行。然后我就想找关闭 PWM 输入功能的语句。试了好久， 关 GPIO 时钟，关中断向量，关 TIM2，都没用。没办法，从初始化函数开始一部分加一部分 的关闭尝试，当关闭 TIM2 时钟是发现中断关闭了。这就好说了，我用任务时钟基的标记位 Control_counter == 1 时打开 TIM2 时钟，然后当 TIM2 中断执行 16 次（每个高电平至少需要

两次中断读取，4 个高电平就 8 次，保守一些让中断执行 16 次）后关闭 TIM2 时钟，这样就 实现了每 2ms 读取一次 PWM 输入的功能。把程序烧进去以后一切就都正常了。

上图是程序运行后通道 1 输入 PWM 后的结果，可以看到检测到了值而且在变化。我用另一 个 stm32 产生了 50Hz（舵机标准信号）的 PWM，脉宽从 0%变到 90%（飞控 PWM 输入计数 应该从 0 变到 18000，满值是 20000）。可以看到，图上读数从 19000 变到 1200 左右，说 明 PWM 输入起作用了。由于在 0%和 100%附近 PWM 信号会失真，所以最大超过 18000 最 小未达到 0 也是可以理解的，当然也有可能是没有显示出来的缘故，向上位机输出的频率是 20Hz，而扫描的频率是 500Hz。

然后我一次把 PWM 信号线插入通道 2 3 4，也读出了数据，说明四个通道都没问题。

上图是上位机读到的通道 1 的捕捉值变化，纵坐标是 200 代表 20000，可以看到 PWM 输入 很好的跟随了另一个 stm32 的 PWM 输出（从 0 变到 18000 再从 18000 变到 0），但是偶尔 会出现噪声。如

但是这不是最致命的，PWM 跟随期间有一段时间会出现下面的情况

噪声很严重，推测是上升沿和下降沿判断出问题了，看来需要对于这个过程多一些判断，避 免出现上面的过程。

2013 年 2 月 9 日 12:44:54

读取多路 PWM 没进展中，各种噪声各种紊乱。。。。 读取函数重新改了，改得类似原

来 8051F 开发的 PCA 函数了（break 语句卡了好久）。但

是读取值向上偏移 2000，去学校用示波器试试，现在不知道是产生 PWM 的 stm32 缘故还 是读取的缘故。

只能读出一个通道，是因为 TIM_GetCapture 函数分 TIM_GetCapture1，TIM_GetCapture2， TIM_GetCapture3，TIM_GetCapture4，四个通道。

关闭看门狗中断，提高 TIM2 中断优先级

循环变量不要用全局，污染变量。

实在不行还是用 PPM 吧

2013 年 2 月 11 日 12:19:28

PWM 输入搞不定，暂且用上升沿下降沿切换这样读取吧。。。以后再说

本文来源：https://www.bwwdw.com/article/rlc.html