手把手教你使用MPU6050做四轴飞控

手把手教大家使用当下最流行的一款六轴（三轴加速度+三轴角速度(陀螺仪)）传感器：MPU6050，该传感器广泛用于四轴、平衡车和空中鼠标等设计，具有非常广泛的应用范围。

本文使用 STM32F1 来驱动 MPU6050，读取其原始数据，并利用其自带的 DMP 实现姿态解算，结合匿名四轴上位机软件和 LCD 显示。

先来看效果

http://mpvideo.qpic.cn/0b2e3qaagaaa34alqjxlczqvbxgdapoaaaya.f10002.mp4?dis_k=50a64dfb3f5dcb5fc04a24cab7ba83ac&dis_t=1645065639&vid=wxv_2235505592226529281&format_id=10002&support_redirect=0&mmversion=false

1、MPU6050 简介

MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。

MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。有了 DMP，我们可以使用 InvenSense 公司提供的运动处理资料库，非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。

MPU6050 的特点包括：

① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）

② 具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec的 3 轴角速度感测器(陀螺仪)

③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器

④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移

⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算数据、感测器同步化、姿势感应等的负荷

⑥ 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求

⑦ 自带一个数字温度传感器

⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS

⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G 中断、零动作感应、触击感应、摇动感应功能

⑩ VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC 可低至 1.8V± 5%

⑪ 陀螺仪工作电流：5mA，陀螺仪待机电流：5uA；加速器工作电流：500uA，加速器省电模式电流：40uA@10Hz

⑫ 自带 1024 字节 FIFO，有助于降低系统功耗

⑬ 高达 400Khz 的 IIC 通信接口

⑭ 超小封装尺寸：4x4x0.9mm（QFN）

MPU6050 传感器的检测轴：

MPU6050 的内部框图：

SCL 和 SDA 是连接 MCU 的 IIC 接口，MCU 通过这个 IIC 接口来控制 MPU6050，另外还有一个 IIC 接口：AUX_CL 和 AUX_DA，这个接口可用来连接外部从设备，比如磁传感器，这样就可以组成一个九轴传感器。VLOGIC 是 IO 口电压，该引脚最低可以到 1.8V，我们一般直接接 VDD 即可。AD0 是从 IIC 接口（接 MCU）的地址控制引脚，该引脚控制 IIC 地址的最低位。如果接 GND，则 MPU6050 的 IIC 地址是：0X68，如果接 VDD，则是 0X69，注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）

利用 STM32F1 读取 MPU6050 的加速度和角度传感器数据（非中断方式），需要哪些初始化步骤：

1）初始化 IIC 接口

MPU6050 采用 IIC 与 STM32F1 通信，所以我们需要先初始化与 MPU6050 连接的 SDA 和SCL 数据线。

2）复位 MPU6050

这一步让 MPU6050 内部所有寄存器恢复默认值，通过对电源管理寄存器 1（0X6B）的 bit7写 1 实现。复位后，电源管理寄存器 1 恢复默认值(0X40)，然后必须设置该寄存器为 0X00，以唤醒 MPU6050，进入正常工作状态。

3）设置角速度传感器（陀螺仪）和加速度传感器的满量程范围

这一步，我们设置两个传感器的满量程范围(FSR)，分别通过陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为±2000dps，加速度传感器的满量程范围为±2g。

4）设置其他参数

还需要配置的参数有：关闭中断、关闭 AUX IIC 接口、禁止 FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF）等。可以不用中断方式读取数据，所以关闭中断，然后也没用到 AUX IIC 接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器（0X38）和用户控制寄存器（0X6A）控制。

MPU6050 可以使用 FIFO 存储传感器数据，不过本文我们没有用到，所以关闭所有 FIFO 通道，这个通过 FIFO 使能寄存器（0X23）控制，默认都是 0，所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）控制，采样率我们一般设置为 50 即可。数字低通滤波器(DLPF)则通过配置寄存器（0X1A）设置，一般设置 DLPF 为带宽的 1/2 即可。

5）配置系统时钟源并使能角速度传感器和加速度传感器

系统时钟源同样是通过电源管理寄存器 1（0X6B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是 0（内部 8M RC 震荡），不过我们一般设置为 1，选择 x 轴陀螺 PLL作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器 2（0X6C）来设置，设置对应位为 0 即可开启。

至此，MPU6050 的初始化就完成了，可以正常工作了（其他未设置的寄存器全部采用默认值即可），接下来，我们就可以读取相关寄存器，得到加速度传感器、角速度传感器和温度传感器的数据了。

6）比较重要的寄存器

电源管理寄存器 1，该寄存器地址为 0X6B；陀螺仪配置寄存器，该寄存器地址为：0X1B；加速度传感器配置寄存器，寄存器地址为：0X1C；FIFO 使能寄存器，寄存器地址为：0X23；陀螺仪采样率分频寄存器，寄存器地址为：0X19；配置寄存器，寄存器地址为：0X1A；电源管理寄存器 2，寄存器地址为：0X6C

陀螺仪数据输出寄存器：有 6 个寄存器组成，地址为：0X43~0X48，通过读取这 6 个寄存器，就可以读到陀螺仪 x/y/z 轴的值，比如 x 轴的数据，可以通过读取 0X43（高 8 位）和 0X44（低 8 位）寄存器得到，其他轴以此类推。

加速度传感器数据输出寄存器：有 6 个，地址为：0X3B~0X40，通过读取这 6 个寄存器，就可以读到加速度传感器 x/y/z 轴的值，比如读 x 轴的数据，可以通过读取 0X3B（高8 位）和 0X3C（低 8 位）寄存器得到，其他轴以此类推。

温度传感器的值：可以通过读取 0X41（高 8 位）和 0X42（低 8 位）寄存器得到。温度换算公式为：Temperature = 36.53 + regval/340

2、DMP 使用简介

经过上面的介绍，我们可以读出 MPU6050 的加速度传感器和角速度传感器的原始数据。不过这些原始数据，对想搞四轴之类的初学者来说，用处不大，我们期望得到的是姿态数据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。有了这三个角，我们就可以得到当前四轴的姿态，这才是我们想要的结果。

要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，初学者不易掌握。而 MPU6050 自带了数字运动处理器，即 DMP，并且，InvenSense 提供了一个 MPU6050 的嵌入式运动驱动库，结合 MPU6050 的 DMP，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到 yaw、roll 和 pitch。

使用内置的 DMP，大大简化了四轴的代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。

InvenSense 提供的 MPU6050 运动驱动库是基于 MSP430 的，我们需要将其移植一下，才可以用到 STM32F1 上面。官方 DMP 驱动库移植起来，还是比较简单的，主要是实现这 4 个函数：i2c_write，i2c_read，delay_ms 和 get_ms

具体的移植就不在这里讲了，代码和工具大家在公众号后台回复【MPU6050】即可领取。

3、硬件设计

用正点原子战舰 STM32F1 开发板的 ATK MODULE 接口连接 ATK-MPU6050 模块，实验功能简介：程序先初始化 MPU6050 等外设，然后利用 DMP 库，初始化 MPU6050 及使能 DMP，最后，在死循环里面不停读取：温度传感器、加速度传感器、陀螺仪、DMP 姿态解算后的欧拉角等数据，通过串口上报给上位机（温度不上报），利用上位机软件（ANO_Tech匿名四轴上位机_V2.6.exe），可以实时显示 MPU6050 的传感器状态曲线，并显示 3D 姿态，可以通过 KEY0 按键开启/关闭数据上传功能。同时，在 LCD 模块上面显示温度和欧拉角等信息。DS0 来指示程序正在运行。

4、软件设计

代码和工具大家在公众号后台回复【MPU6050】即可领取。

为了高速上传数据，将串口 1 的波特率设置为 500Kbps 了，测试的时候要注意下，串口的波特率是 500000，其他的波特率会导致传输乱码。

接收串口数据的时候大家可以先用 XCOM 等串口工具测试一下有没有输出，然后再用 ANO_Tech 匿名四轴上位机_V2.6.exe 接收。

测试需要自备 ATK-MPU6050 模块。在代码编译成功之后，通过下载代码到 ALIENTEK 战舰 STM32F1 开发板上，可以看到 LCD 显示如图所示的内容（假定ATK-MPU6050 模块已经接到战舰 STM32 开发板的 ATK MODULE 接口）：

屏幕显示了 MPU6050 的温度、俯仰角（pitch）、横滚角（roll）和航向角（yaw）的数值。然后，我们可以晃动开发板，看看各角度的变化。

通过按 KEY0 可以开启或关闭数据上报，开启状态下，我们可以打开：ANO_Tech 匿名四轴上位机_V2.6.exe，这个软件，接收 STM32F1 上传的数据，从而图形化显示传感器数据以及飞行姿态：

上图就是波形化显示我们通过 mpu6050_send_data 函数发送的数据，采用 A1 功能帧发送，总共 6 条线（Series1~6）显示波形，全部来自 A1 功能帧，int16 数据格式，Series1~6 分别代表：加速度传感器 x/y/z 和角速度传感器（陀螺仪）x/y/z 方向的原始数据。

注意需要先连接右下角的串口，然后点击《波形显示》和《高级收码》按钮。

上图则 3D 显示了我们开发板的姿态，通过 usart1_report_imu 函数发送的数据显示，采用飞控显示帧格（AF）式上传，同时还显示了加速度陀螺仪等传感器的原始数据。