「ALIENTEK 阿波罗 STM32F767 开发板资料连载」第四十章 MPU9250 九轴传感器实验

1）实验平台：alientek 阿波罗 STM32F767 开发板
2）摘自《STM32F7 开发指南(HAL 库版)》关注官方微信号公众号，获取更多资料：正点原子


第四十章 MPU9250 九轴传感器实验
本章，我们介绍一款主流的九轴（三轴加速度+三轴角速度(陀螺仪)+三轴磁力计）传感器：
MPU9250，该传感器广泛用于四轴、平衡车和空中鼠标等设计，具有非常广泛的应用范围。
ALIENTEK 阿波罗 STM32F767 开发板自带了 MPU9250 传感器。本章我们将使用 STM32F767
来驱动 MPU9250，读取其原始数据，并利用其自带的 DMP 结合 MPL 库实现姿态解算，结合
匿名四轴上位机软件和 LCD 显示，教大家如何使用这款功能强大的九轴传感器。本章分为如下
几个部分：
40.1 MPU9250 简介
40.2 硬件设计
40.3 软件设计
40.4 下载验证
40.1 MPU9250 简介
本节，我们将分 2 个部分介绍：1，MPU9250 基础介绍。2，DMP 使用简介。另外，所有
MPU9250 的相关资料，都在光盘：A 盘7，硬件资料MPU9250 资料 文件夹里面。
40.1.1 MPU9250 基础介绍
MPU9250 是 InvenSense 公司推出的全球首款整合性 9 轴运动处理组件，相较于多组件方
案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了体积和功耗。
MPU9250 内部集成有 3 轴陀螺仪、
3 轴加速度计和 3 轴磁力计，输出都是 16 位的数字量; 可
以通过集成电路总线( IIC) 接口和单片机进行数据交互，传输速率可达 400 kHz /s。陀螺仪的角
速度测量范围最高达±2000（°/s），具有良好的动态响应特性。加速度计的测量范围最大为±
16g( g 为重力加速度)，静态测量精度高。磁力计采用高灵度霍尔型传感器进行数据采集，磁感
应强度测量范围为±4800μT，可用于对偏航角的辅助测量。
MPU9250 自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，可以整
合九轴传感器数据，向应用端输出完整的 9 轴融合演算数据。有了 DMP，我们可以使用
InvenSense 公司提供的运动处理库（MPL：Motion Process Library），非常方便的实现姿态解算，
降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。
MPU9250 的特点包括：
① 以数字形式输出 9 轴旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的
融合演算数据（需 DMP 支持）
② 集成 16 位分辨率，量程为：±250、±500、±1000°与±2000°/sec 的 3 轴角速度
传感器(陀螺仪)
③ 集成 16 位分辨率，量程为：±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
④ 集成 16 位分辨率，量程为：±4800uT 的磁场传感器（磁力计）
⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算
数据、感测器同步化、姿势感应等的负荷
⑥ 自带一个数字温度传感器
⑦ 可编程数字滤波器
⑧ 支持 SPI 接口，通信速度高达 20Mhz
⑨ 自带 512 字节 FIFO 缓冲区
⑩ 高达 400Khz 的 IIC 通信接口
⑪ 超小封装尺寸：3x3x1mm（QFN）
MPU9250 传感器的检测轴如图 40.1.1.1 所示：



图 40.1.1.1 MPU9250 检测轴及其方向
MPU9250 的内部框图如图 40.1.1.2 所示：



图 40.1.1.2 MPU9250 框图
其中，SCL 和 SDA 可以连接 MCU 的 IIC 接口，MCU 通过这个 IIC 接口来控制 MPU9250，
另外还有一个 IIC 接口：AUX_CL 和 AUX_DA，这个接口可用来连接外部从设备，比气压传感
器。VDDIO 是 IO 口电压，该引脚最低可以到 1.8V，我们一般直接接 VDD 即可。AD0 是从 IIC
接口（接 MCU）的地址控制引脚，该引脚控制 IIC 地址的最低位。如果接 GND，则 MPU9250
的 IIC 地址是：0X68，如果接 VDD，则是 0X69，注意：这里的地址是不包含数据传输的最低
位的（最低位用来表示读写）！！注意：当使用 SPI 接口的时候，使用：SCLK、SDO、SDI 和
nCS 脚来传输数据。
这里需要和大家说明一下的是：MPU9250，实际上是内部集成了一个 MPU6500 六轴传感
器和一个 AK8963 三轴磁力计，他们共用一个 IIC 接口，这样组合成一个九轴传感器。前面说
了我们开发板上MPU9250的IIC地址是0X68，实际上是指MPU6500的地址是0X68，而AK8963
磁力计的 IIC 地址，则是：0X0C（不包含最低位）。
在阿波罗 STM32 开发板上，AD0 是接 GND 的，所以 MPU9250 的 IIC 地址是 0X68（不含
最低位），IIC 通信的时序我们在之前已经介绍过（第二十九章，IIC 实验），这里就不再细说了。
接下来，我们介绍一下利用 STM32F767 读取 MPU9250 的加速度和角度传感器数据（非
中断方式），需要哪些初始化步骤：
1）初始化 IIC 接口
MPU9250 采用 IIC 与 STM32F767 通信，所以我们需要先初始化与 MPU9250 连接的 SDA
和 SCL 数据线。这个在前面的 IIC 实验章节已经介绍过了，这里 MPU9250 与 24C02 共用一个
IIC，所以初始化 IIC 完全一模一样。
2）复位 MPU9250
这一步让 MPU9250 内部所有寄存器恢复默认值，通过对电源管理寄存器 1（0X6B）的 bit7
写 1 实现。 复位后，电源管理寄存器 1 恢复默认值(0X40)，然后必须设置该寄存器为 0X00，
以唤醒 MPU9250，进入正常工作状态。
3）设置角速度传感器（陀螺仪）和加速度传感器的满量程范围
这一步，我们设置两个传感器的满量程范围(FSR)，分别通过陀螺仪配置寄存器（0X1B）
和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为±2000dps，
加速度传感器的满量程范围为±2g。
4）设置其他参数
这里，我们还需要配置的参数有：关闭中断、关闭 AUX IIC 接口、禁止 FIFO、设置陀螺
仪采样率和设置数字低通滤波器（DLPF）等。本章我们不用中断方式读取数据，所以关闭中断，
然后也没用到 AUX IIC 接口外接其他传感器，所以也关闭这个接口。分别通过中断使能寄存器
（0X38）和用户控制寄存器（0X6A）控制。MPU9250 可以使用 FIFO 存储传感器数据，不过
本章我们没有用到，所以关闭所有 FIFO 通道，这个通过 FIFO 使能寄存器（0X23）控制，默
认都是 0（即禁止 FIFO），所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）
控制，这个采样率我们一般设置为 50 即可。数字低通滤波器(DLPF)则通过配置寄存器（0X1A）
设置，一般设置 DLPF 为带宽的 1/2 即可。
5）配置系统时钟源并使能角速度传感器和加速度传感器
系统时钟源同样是通过电源管理寄存器 1（0X6B）来设置，该寄存器的最低三位用于设置
系统时钟源选择，默认值是 0（内部 8M RC 震荡），不过我们一般设置为 1，选择 x 轴陀螺 PLL
作为时钟源，以获得更高精度的时钟。同时，使能角速度传感器和加速度传感器，这两个操作
通过电源管理寄存器 2（0X6C）来设置，设置对应位为 0 即可开启。
6）配置 AK8963 磁场传感器（磁力计）
经过前面 5 步配置，我们完成了对 MPU6500 的配置，此步需要对 AK8963 进行配置。首
先设置控制寄存器 2（0X0B）的最低位为 1，对 AK8963 进行软复位。随后设置控制寄存器 1
（0X0A）为 0X11，选择 16 位输出，单次测量模式。随后就可以读取磁力计数据了。
至此，MPU9250 的初始化就完成了，可以正常工作了（其他未设置的寄存器全部采用默认值即可），接下来，我们就可以读取相关寄存器，得到加速度传感器、角速度传感器和温度传感
器的数据了。不过，我们先简单介绍几个重要的寄存器。
首先，我们介绍电源管理寄存器 1，该寄存器地址为 0X6B，各位描述如表 40.1.1.1 所示：


图 40.1.1.1 电源管理寄存器 1 各位描述
其中，H_RESET 位用来控制复位，设置为 1，复位 MPU9250，复位结束后，MPU 硬件自
动清零该位。SLEEEP 位用于控制 MPU9250 的工作模式，复位后，该位为 1，即进入了睡眠模
式（低功耗），所以我们要清零该位，以进入正常工作模式。最后 CLKSEL[2:0]用于选择系统时
钟源，选择关系如表 40.1.1.2 所示：


图 40.1.1.2 CLKSEL 选择列表
默认是使用内部 20M RC 晶振的，精度不高，我们一般设置其自动选择最有效的时钟源，
一般设置 CLKSEL=001 即可。
接着，我们看陀螺仪配置寄存器，该寄存器地址为：0X1B，各位描述如表 40.1.3 所示：


表 40.1.1.3 陀螺仪配置寄存器各位描述
该寄存器我们只关心 GYRO_FS_SEL[1:0]和 FCHOICE[1:0]这四个位，GYRO_FS_SEL[1:0]用于
设置陀螺仪的满量程范围：0，±250°/S；1，±500°/S；2，±1000°/S；3，±2000°/S；我
们一般设置为 3，即±2000°/S，因为陀螺仪的 ADC 为 16 位分辨率，所以得到灵敏度为：
65536/4000=16.4LSB/(°/S)。FCHOICE[1:0]用于控制 DLPF 旁路，我们一般设置为 3，不旁路
DLPF。
接下来，我们看加速度传感器配置寄存器，寄存器地址为：0X1C，各位描述如表 40.1.1.4
所示：


表 40.1.1.4 加速度传感器配置寄存器各位描述
该寄存器我们只关心 ACCEL_FS_SEL[1:0]这两个位，用于设置加速度传感器的满量程范围：
0，±2g；1，±4g；2，±8g；3，±16g；我们一般设置为 0，即±2g，因为加速度传感器的
ADC 也是 16 位，所以得到灵敏度为：65536/4=16384LSB/g。
接下来，我看看 FIFO 使能寄存器，寄存器地址为：0X23，各位描述如表 40.1.1.5 所示：


表 40.1.1.5 FIFO 使能寄存器各位描述
该寄存器用于控制 FIFO 使能，在简单读取传感器数据的时候，可以不用 FIFO，设置对应
位为 0即可禁止 FIFO，设置为 1，则使能 FIFO。注意加速度传感器的 3个轴，全由1 个位（ACCEL）
控制，只要该位置 1，则加速度传感器的三个通道都开启 FIFO 了。
接下来，我们看陀螺仪采样率分频寄存器，寄存器地址为：0X19，各位描述如表 40.1.1.6
所示：


表 40.1.1.6 陀螺仪采样率分频寄存器各位描述
该寄存器用于设置 MPU9250 的陀螺仪采样频率，计算公式为：
采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
这里陀螺仪的输出频率，是 1Khz、8Khz 或 32Khz，与数字低通滤波器（DLPF）的设置有
关，当 FCHOICE[1:0]不为 11 的时候，频率为 32Khz，其他情况：当 DLPF_CFG=0/7 的时候，
频率为 8Khz，否则是 1Khz。而且 DLPF 滤波频率一般设置为采样率的一半。采样率，我们假
定设置为 50Hz，那么 SMPLRT_DIV=1000/50-1=19。
接下来，我们看配置寄存器，寄存器地址为：0X1A，各位描述如表 40.1.1.7 所示：


表 40.1.1.7 配置寄存器各位描述
这里，我们主要关心数字低通滤波器（DLPF）的设置位，即：DLPF_CFG[2:0]，陀螺仪根
据这三个位的配置进行过滤。DLPF_CFG 不同配置对应的过滤情况如表 40.1. 1. 8 所示：


表 40.1.1.8 DLPF_CFG 配置表
一般我们设置角速度传感器的带宽为其采样率的一半，如前面所说的，如果设置采样率为
50Hz，那么带宽就应该设置为 25Hz，取近似值 20Hz，就应该设置 DLPF_CFG=100。需要注意：
FCHOICE[1:0]（通过 0X1B 寄存器配置）必须设置为 11，否则固定 32K 频率，且 DLPF_CFG
的配置无效！
接下来，我们看电源管理寄存器 2，寄存器地址为：0X6C，各位描述如表 40.1.1.9 所示：


表 40.1.1.9 电源管理寄存器 2 各位描述
该寄存器低六位有效，分别控制加速度和陀螺仪的 x/y/z 轴是否开启，这里我们设置全部都
开启，所以全部设置为 0 即可。
接下来，我们看看陀螺仪数据输出寄存器，总共由 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 = 21 + regval/338.87
其中，Temperature 为计算得到的温度值，单位为℃，regval 为从 0X41 和 0X42 读到的温度
传感器值。
接下来，我们看 AK8963 的控制寄存器 1 ，寄存器地址为：0X0A，各位描述如表 40.1.1.10
所示：


表 40.1.1.10 AK8963 控制寄存器 1
其中，BIT 位控制 AK8963 输出位数，0，表示 14 位；1，表示 16 位；我们一般设置为 1。
MODE[3:0]用于控制 AK8963 的工作模式：0000，掉电模式；0001，单次测量模式；0010，
连续测量模式 1；0110，连续测量模式 2；0100，外部触发测量模式；1000，自测试模式；
1111，Fuse ROM 访问模式；我们一般设置 MODE[3:0]=0001，即单次测量模式。
接下来，我们看 AK8963 的控制寄存器 2，寄存器地址为：0X0B，各位描述如表 40.1.1.11
所示：


表 40.1.1.11 AK8963 控制寄存器 2
该寄存器仅最低位有效，用于控制 AK8963 的软复位，我们在初始化的时候，设置 SRST=1
即可让 AK8963 进行一次软复位，复位结束后，自动设置为 0。
最后，我们看看磁力计数据输出寄存器，总共由 6 个寄存器组成，地址为：0X03~0X08，
通过读取这 6 个寄存器，就可以读到磁力计 x/y/z 轴的值，比如 x 轴的数据，可以通过读取 0X03
（低 8 位）和 0X04（高 8 位）寄存器得到，其他轴以此类推。
关于 MPU9250 的基础介绍，我们就介绍到这。MPU9250 的详细资料和相关寄存器介绍，
请参考光盘：7，硬件资料MPU9250 资料 PS-MPU-9250A-01.pdf 和 RM-MPU-9250A-00.pdf
这两个文档，另外该目录还提供了部分 MPU9250 的中文资料，供大家参考学习。
40.1.2 DMP 使用简介
经过 40.1.1 节的介绍，我们可以读出 MPU9250 的加速度传感器和角速度传感器的原始数
据。不过这些原始数据，对想搞四轴之类的初学者来说，用处不大，我们期望得到的是姿态数
据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。有了这三个角，我们就
可以得到当前四轴的姿态，这才是我们想要的结果。
要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识
点比较多，初学者 不易掌握。而 MPU9250 自带了数字运动处理器，即 DMP，并且，InvenSense
提供了一个 MPU9250 的嵌入式运动处理库（MPL），结合 MPU9250 的 DMP，可以将我们的传
感器原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，
从而得到 yaw、roll 和 pitch。
使用内置的 DMP，大大简化了四轴的代码设计，且 MCU 不用进行姿态解算过程，大大降
低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
InvenSense 提供的最新 MPL 库版本为：6.12 版本，它提供了基于 STM32F4 Discovery 板的
参考例程（IAR 工程），我们只需要将它移植到我们的开发板上即可。官方原版驱动在光盘：7，
硬件资料MPU9250 资料 motion_driver_6.12.zip。解压之后，里面有 MPL 的参考例程
（arm/msp430）、LIB 库（mpl libraries）和说明文档（documentation）等资料，大家可以参考
documentation 文件夹下的几个 PDF 教程来学习 MPL 的使用。
官方 MPL 库移植起来，还是比较简单的，主要是实现这 4 个函数：i2c_write，i2c_read，
delay_ms 和 get_ms，具体细节，我们就不详细介绍了，移植后的驱动代码，我们放在本例程
HARDWAREMPU9250MPL 文件夹内，包含 4 个文件夹，如图 40.1.2.1 所示：



图 40.1.2.1 移植后的驱动库代码
为了方便大家使用该驱动库（MPL），我们在inv_mpu.c里面添加了两个函数：
mpu_dmp_init
和 mpu_mpl_get_data 这两个函数，这里我们简单介绍下这两个函数。
mpu_dmp_init，是 MPU9250 DMP 初始化函数，该函数代码如下：
//MPU9250,dmp 初始化
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_init(void)
{
u8 res=0;
struct int_param_s int_param;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned short compass_fsr;
IIC_Init();
//初始化 IIC 总线
if(mpu_init(&int_param)==0)
//初始化 MPU9250
{
res=inv_init_mpl();
//初始化 MPL
if(res)return 1;
inv_enable_quaternion();
inv_enable_9x_sensor_fusion();
inv_enable_fast_nomot();
inv_enable_gyro_tc();
inv_enable_vector_compass_cal();
inv_enable_magnetic_disturbance();
inv_enable_eMPL_outputs();
res=inv_start_mpl(); //开启 MPL
if(res)return 1;
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|
INV_XYZ_COMPASS);//设置所需要的传感器
if(res)return 2;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //设置 FIFO
if(res)return 3;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ);
//设置采样率
if(res)return 4;
res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS); //磁力计采样率
if(res)return 5;
mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_compass_fsr(&compass_fsr);
inv_set_gyro_sample_rate(1000000L/gyro_rate);
inv_set_accel_sample_rate(1000000L/gyro_rate);
inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);
inv_set_gyro_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);
inv_set_accel_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);
inv_set_compass_orientation_and_scale(
inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);
res=dmp_load_motion_driver_firmware(); //加载 dmp 固件
if(res)return 6;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//设置陀螺仪方向
if(res)return 7;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|
DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
//设置 dmp 功能
if(res)return 8;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);//设置 DMP 输出速率(不超过 200Hz)
if(res)return 9;
res=run_self_test();
//自检
if(res)return 10;
res=mpu_set_dmp_state(1); //使能 DMP
if(res)return 11;
}
return 0;
}
此函数首先通过 IIC_Init(需外部提供)初始化与 MPU9250 连接的 IIC 接口，然后调用
mpu_init 函数，初始化 MPU9250，之后就是设置 DMP 所用传感器、FIFO、采样率和加载固件
等一系列操作，在所有操作都正常之后，最后通过 mpu_set_dmp_state(1)使能 DMP 功能，在使
能成功以后，我们便可以通过 mpu_mpl_get_data 来读取姿态解算后的数据了。
mpu_mpl_get_data 函数代码如下：
//得到 mpl 处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw)
{
unsigned long sensor_timestamp,timestamp;
short gyro[3], accel_short[3],compass_short[3],sensors;
unsigned char more;
long compass[3],accel[3],quat[4],temperature;
long data[9];
int8_t accuracy;
if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;
if(sensors&INV_XYZ_GYRO)
{
inv_build_gyro(gyro,sensor_timestamp);
//把新数据发送给 MPL
mpu_get_temperature(&temperature,&sensor_timestamp);
inv_build_temp(temperature,sensor_timestamp); //发温度值给 MPL,仅陀螺仪需要
}
if(sensors&INV_XYZ_ACCEL)
{
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel,0,sensor_timestamp);
//把加速度值发给 MPL
}
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp))
{
compass[0]=(long)compass_short[0];
compass[1]=(long)compass_short[1];
compass[2]=(long)compass_short[2];
inv_build_compass(compass,0,sensor_timestamp); //把磁力计值发给 MPL
}
inv_execute_on_data();
inv_get_sensor_type_euler(data,&accuracy,×tamp);
*roll = (data[0]/q16);
*pitch = -(data[1]/q16);
*yaw = -data[2] / q16;
return 0;
}
此函数用于得到 DMP 姿态解算后的俯仰角、横滚角和航向角。不过本函数局部变量有点
多，大家在使用的时候，如果死机，那么请设置堆栈大一点(在 startup_stm32f767xx.s 里面设置，
默认是 800)。
利用这两个函数，我们就可以读取到姿态解算后的欧拉角，使用非常方便。DMP 部分，我
们就介绍到这。
40.2 硬件设计
本实验采用 STM32F767 的 2 个普通 IO 连接 MPU9250（IIC），本章实验功能简介：程序先
初始化 MPU9250 等外设，然后利用 MPL 库，初始化 MPU9250 及使能 DMP，最后，在死循
环里面不停读取：温度传感器、加速度传感器、陀螺仪、磁力计、MPL 姿态解算后的欧拉角等
数据，通过串口上报给上位机（温度不上报），利用上位机软件（ANO_TC 匿名科创地面站
v4.exe），可以实时显示 MPU9250 的传感器状态曲线，并显示 3D 姿态，可以通过 KEY0 按
键开启/关闭数据上传功能。同时，在 LCD 模块上面显示温度和欧拉角等信息。DS0 来指示程
序正在运行。
所要用到的硬件资源如下：
1） 指示灯 DS0
2） KEY0 按键
3） LCD 模块
4） 串口
5） MPU9250
前 4 个，在之前的实例已经介绍过了，这里我们仅介绍 MPU9250 与阿波罗 STM32F767 开
发板的连接。该接口与 MCU 的连接原理图如 40.2.1 所示：



图 40.2.1 MPU9250 与 STM32F767 的连接电路图
从上图可以看出，MPU9250 的 SCL 和 SDA 与 STM32F767 开发板的 PH4 和 PH5 连接，与
24C02 等共用 IIC 总线。图中，AD0 接的 GND，所以 MPU9250 的器件地址是：0X68。
注意：9D_INT 信号，是连接在 PCF8574T 的 P5 脚上的，并没有直接连接到 MCU，所以，
在需要读取 9D_INT 的时候，需要先初始化 PCF8574T。不过，本例程用不到 9D_INT，所以，
可以不初始化 PCF8574T，直接通过 IIC 总线读取数据即可。
40.3 软件设计
打开本章实验工程可以看到，我们在 HARDWARE 分组之下添加了 MPU9250 驱动源文件
mpu9250.c，并且包含了其对应的头文件 mpu9250.h。同时，将 MPL 驱动库代码（见光盘例程
源码：实验 35 MPU9250 九轴传感器实验HARDWAREMPU9250MPL 目录）添加到新建的
MPL 分组之下，工程结构如图 39.3.1 所示：


图 39.3.1 MPU9250 工程结构图
注意：MPL 代码，要求在 MDK Options for Target 的 C/C++选项卡里面，要勾选 C99 模
式，否则编译出错。
由于篇幅所限，MPL 部分的代码，我们就不详细介绍了，请大家参考 motion_driver_6.12.zip
里面的相关教程进行学习。我们仅介绍 mpu9250.c 里面的部分函数，首先是：MPU_Init，该函
数代码如下：
//初始化 MPU9250
//返回值:0,成功
// 其他,错误代码
u8 MPU9250_Init(void)
{
u8 res=0;
IIC_Init(); //初始化 IIC 总线
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//复位 MPU9250
delay_ms(100); //延时 100ms
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒 MPU9250
MPU_Set_Gyro_Fsr(3);
//陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0);
//加速度传感器,±2g
MPU_Set_Rate(50);
//设置采样率 50Hz
MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//主模式关闭
MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00); //关闭 FIFO
MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);
//INT 引脚低电平有效，开启 bypass 模式，可以直接读取磁力计
res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG);
//读取 MPU6500 的 ID
if(res==MPU6500_ID) //器件 ID 正确
{
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01);
//设置 CLKSEL,PLL X 轴为参考
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00);
//加速度与陀螺仪都工作
MPU_Set_Rate(50);
//设置采样率为 50Hz
}else return 1;
res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA); //读取 AK8963 ID
if(res==AK8963_ID)
{
MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //设置 AK8963 为单次测量模式
}else return 1;
return 0;
}
该函数就是按我们在 39.1.1 节介绍的方法，对 MPU9250 进行初始化，该函数执行成功后，
便可以读取传感器数据了。
然后，我们再看 MPU_Get_Temperature、MPU_Get_Gyroscope 、MPU_Get_Accelerometer
和 MPU_Get_Magnetometer 等四个函数，源码如下：
//得到温度值
//返回值:温度值(扩大了 100 倍)
short MPU_Get_Temperature(void)
{
u8 buf[2];
short raw;
float temp;
MPU_Read_Len(MPU9250_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((u16)buf[0]<<8)|buf[1];
temp=21+((double)raw)/333.87;
return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪 x,y,z 轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪 x,y,z 轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到磁力计值(原始值)
//mx,my,mz:磁力计 x,y,z 轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Magnetometer(short *mx,short *my,short *mz)
{
u8 buf[6],res;
res=MPU_Read_Len(AK8963_ADDR,MAG_XOUT_L,6,buf);
if(res==0)
{
*mx=((u16)buf[1]<<8)|buf[0];
*my=((u16)buf[3]<<8)|buf[2];
*mz=((u16)buf[5]<<8)|buf[4];
}
MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11);
//AK8963 每次读完以后都需要重新设置为单次测量模式
return res;;
}
其中 MPU_Get_Temperature 用于获取 MPU9250 自带温度传感器的温度值，然后
MPU_Get_Gyroscope、MPU_Get_Accelerometer 和 MPU_Get_Magnetometer 分别用于读取
陀螺仪、加速度传感器和磁力计的原始数据。
最后看 MPU_Write_Len 和 MPU_Read_Len 这两个函数，代码如下：
//IIC 连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
IIC_Start();
IIC_Send_Byte((addr<<1)|0);
//发送器件地址+写命令
if(IIC_Wait_Ack()) { IIC_Stop();return 1;}
//等待应答
IIC_Send_Byte(reg);
//写寄存器地址
IIC_Wait_Ack();
//等待应答
for(i=0;i {
IIC_Send_Byte(buf);
//发送数据
if(IIC_Wait_Ack()){ IIC_Stop();return 1;} //等待应答
}
IIC_Stop();
return 0;
}
//IIC 连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0);
//发送器件地址+写命令
if(IIC_Wait_Ack()){ IIC_Stop();return 1;}
//等待应答
IIC_Send_Byte(reg);
//写寄存器地址
IIC_Wait_Ack();
//等待应答
IIC_Start();
IIC_Send_Byte((addr<<1)|1);
//发送器件地址+读命令
IIC_Wait_Ack();
//等待应答
while(len)
{
if(len==1)*buf=IIC_Read_Byte(0);
//读数据,发送 nACK
else *buf=IIC_Read_Byte(1);
//读数据,发送 ACK
len--;
buf++;
}
IIC_Stop();
//产生一个停止条件
return 0;
}
MPU_Write_Len 用于指定器件和地址，连续写数据，可用于实现 MPL 部分的：i2c_write
函数。而 MPU_Read_Len 用于指定器件和地址，连续读数据，可用于实现 MPL 部分的：i2c_read
函数。MPL 移植部分的 4 个函数，这里就实现了 2 个，剩下的 delay_ms 就直接采用我们 delay.c
里面的 delay_ms 实现，get_ms 则直接提供一个空函数即可。
关于 mpu9250.c 我们就介绍到这， mpu9250.h 的代码，我们这里就不再贴出了，大家看光
盘源码即可。
最后看看 main.c 内容，代码如下：
//串口 1 发送 1 个字符
//c:要发送的字符
void usart1_send_char(u8 c)
{
while(__HAL_UART_GET_FLAG(&UART1_Handler,UART_FLAG_TC)==RESET){};
USART1->TDR=c;
}
//传送数据给匿名四轴地面站(V4 版本)
//fun:功能字. 0X01~0X1C
//data:数据缓存区,最多 28 字节!!
//len:data 区有效数据个数
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32];
u8 i;
if(len>28)return;
//最多 28 字节数据
send_buf[len+3]=0 ;
//校验数置零
send_buf[0]=0XAA;
//帧头
send_buf[1]=0XAA;
//帧头
send_buf[2]=fun;
//功能字
send_buf[3]=len;
//数据长度
for(i=0;i;
//复制数据
for(i=0;i; //计算校验和
for(i=0;i); //发送数据到串口 1
}
//发送加速度传感器数据+陀螺仪数据(传感器帧)
//aacx,aacy,aacz:x,y,z 三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z 三个方向上面的陀螺仪值
void mpu9250_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
{
u8 tbuf[18];
tbuf[0]=(aacx>>8)&0XFF;
tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF;
tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF;
tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF;
tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF;
tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF;
tbuf[11]=gyroz&0XFF;
tbuf[12]=0;//开启 MPL 后,无法直接读取磁力计数据,所以这里直接屏蔽掉.用 0 替代.
tbuf[13]=0;
tbuf[14]=0;
tbuf[15]=0;
tbuf[16]=0;
tbuf[17]=0;
usart1_niming_report(0X02,tbuf,18);//传感器帧,0X02
}
//通过串口 1 上报结算后的姿态数据给电脑(状态帧)
//roll:横滚角.单位 0.01 度。 -18000 -> 18000 对应 -180.00 -> 180.00 度
//pitch:俯仰角.单位 0.01 度。-9000 - 9000 对应 -90.00 -> 90.00 度
//yaw:航向角.单位为 0.1 度 0 -> 3600 对应 0 -> 360.0 度
//c***:超声波高度,单位:cm
//prs:气压计高度,单位:mm
void usart1_report_imu(short roll,short pitch,short yaw,short c***,int prs)
{
u8 tbuf[12];
tbuf[0]=(roll>>8)&0XFF;
tbuf[1]=roll&0XFF;
tbuf[2]=(pitch>>8)&0XFF;
tbuf[3]=pitch&0XFF;
tbuf[4]=(yaw>>8)&0XFF;
tbuf[5]=yaw&0XFF;
tbuf[6]=(c***>>8)&0XFF;
tbuf[7]=c***&0XFF;
tbuf[8]=(prs>>24)&0XFF;
tbuf[9]=(prs>>16)&0XFF;
tbuf[10]=(prs>>8)&0XFF;
tbuf[11]=prs&0XFF;
usart1_niming_report(0X01,tbuf,12);//状态帧,0X01
}
int main(void)
{
u8 t=0,report=1;
//默认开启上报
u8 key;
float pitch,roll,yaw;
//欧拉角
short aacx,aacy,aacz;
//加速度传感器原始数据
short gyrox,gyroy,gyroz;
//陀螺仪原始数据
short temp;
//温度
Cache_Enable(); //打开 L1-Cache
MPU_Memory_Protection(); //保护相关存储区域
HAL_Init();
//初始化 HAL 库
Stm32_Clock_Init(432,25,2,9); //设置时钟,216Mhz
delay_init(180);
//初始化延时函数
…//此处省略部分初始化代码
while(mpu_dmp_init())
{
LCD_ShowString(30,130,200,16,16,"MPU9250 Error"); delay_ms(200);
LCD_Fill(30,130,239,130+16,WHITE); delay_ms(200);
LED0_Toggle;//DS0 闪烁
}
LCD_ShowString(30,130,200,16,16,"MPU9250 OK");
LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");
POINT_COLOR=BLUE; //设置字体为蓝色
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
LCD_ShowString(30,200,200,16,16," Temp: . C");
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
while(1)
{
key=KEY_Scan(0);
if(key==KEY0_PRES)
{
report=!report;
if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
}
if(mpu_mpl_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(report)mpu9250_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);
//发送加速度+陀螺仪原始数据
if(report)usart1_report_imu((int)(roll*100),(int)(pitch*100),(int)(yaw*100),0,0);
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0);
//显示负号
temp=-temp;
//转为正数
}else LCD_ShowChar(30+48,200,' ',16,0);
//去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16);
//显示小数部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0);
//显示负号
temp=-temp;
//转为正数
}else LCD_ShowChar(30+48,220,' ',16,0);
//去掉负号
LCD_ShowNum(30+48+8,220,temp/10,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,220,temp%10,1,16);
//显示小数部分
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0);
//显示负号
temp=-temp;
//转为正数
}else LCD_ShowChar(30+48,240,' ',16,0);
//去掉负号
LCD_ShowNum(30+48+8,240,temp/10,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,240,temp%10,1,16);
//显示小数部分
temp=yaw*10;
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0);
//显示负号
temp=-temp;
//转为正数
}else LCD_ShowChar(30+48,260,' ',16,0);
//去掉负号
LCD_ShowNum(30+48+8,260,temp/10,3,16);
//显示整数部分
LCD_ShowNum(30+48+40,260,temp%10,1,16);
//显示小数部分
t=0;
LED0_Toggle;//DS0 闪烁
}
}
t++;
}
}
此部分代码除了 main 函数，还有几个函数，用于上报数据给上位机软件，利用上位机软件
显示传感器波形，以及 3D 姿态显示，有助于更好的调试 MPU9250。上位机软件使用：ANO_TC
匿名科创地面站 v4.exe，该软件在：开发板光盘 6，软件资料软件匿名地面站 文件夹里
面可以找到，该软件的使用方法，见该文件夹下的：飞控通信协议 v1.3-0720.pdf，这里我们
不做介绍。其中，usart1_niming_report 函数用于将数据打包、计算校验和，然后上报给匿名地
面站软件。MPU9250_send_data 函数用于上报加速度和陀螺仪的原始数据，可用于波形显示传
感器数据，通过传感器帧（02H）发送。而 usart1_report_imu 函数，则用于上报飞控显示帧，
可以实时 3D 显示 MPU9250 的姿态，传感器数据等，通过状态帧（01H）发送。
这里，main 函数是比较简单的，大家看代码即可，不过需要注意的是，为了高速上传数据，
这里我们将串口 1 的波特率设置为 500Kbps 了，测试的时候要注意下。
至此，我们的软件设计部分就结束了。
40.4 下载验证
在代码编译成功之后，我们通过下载代码到 ALIENTEK 阿波罗 STM32 开发板上，可以看
到 LCD 显示如图 40.4.1 所示的内容：


图 40.4.1 程序运行时 LCD 显示内容
屏幕显示了 MPU9250 的温度、俯仰角（pitch）、横滚角（roll）和航向角（yaw）的数值。
然后，我们可以晃动开发板，看看各角度的变化。
另外，通过按 KEY0 可以开启或关闭数据上报，开启状态下，我们可以打开：ANO_TC 匿
名科创地面站 v4.exe，这个软件，接收 STM32F767 上传的数据，从而图形化显示传感器数据以
及飞行姿态，如图 40.4.2 和图 40.4.3 所示：



图 40.4.2 传感器数据波形显示



图 40.4.3 飞控状态显示
图 40.4.2 就是波形化显示我们通过 MPU9250_send_data 函数发送的数据，采用传感器帧（02）
发送，总共 6 条线（ACC_X、ACC_Y、ACC_Z、GYRO_X、GYRO_Y 和 GYRO_Z）显示波形，
全部来自传感器帧，分别代表：加速度传感器 x/y/z 和角速度传感器（陀螺仪）x/y/z 方向的原
始数据（请注意把选项“程序设置->上位机设置->数据校验”设置为 Off，否则可能看不到数
据和飞控状态变化）。
图图 40.4.3 则 3D 显示了我们开发板的姿态，通过 usart1_report_imu 函数发送的数据显示，
采用状态帧（01）上传，同时还显示了加速度陀螺仪等传感器的原始数据。