OpenEdv-开源电子网»首页 › 单片机/嵌入式 › STM32-F3/F4/F7/H7专区 › 【正点原子探索者STM32F407开发板例程连载+教学】第37章 ...

12 / 2 页下一页

返回列表

查看: 56939|回复: 94

【正点原子探索者STM32F407开发板例程连载+教学】第37章 MPU6050六轴传感器实验

[复制链接]

Admin

230 主题	1950 帖子	10 精华

论坛元老

Rank: 8 Rank: 8

积分: 4562
金钱: 4562
注册时间: 2010-12-14
在线时间: 32 小时

发消息

发表于 2014-12-6 18:10:25 | 显示全部楼层 |阅读模式

正点原子逻辑分析仪DL16热卖中，最多16通道，采样率最高可达1GHz，存储深度可达3.5Gbit

第三十七章 MPU6050六轴传感器实验

1.硬件平台：正点原子探索者STM32F407开发板

2.软件平台：MDK5.1

3.固件库版本：V1.4.0

本章，我们介绍当下最流行的一款六轴（三轴加速度+三轴角速度(陀螺仪)）传感器：MPU6050，该传感器广泛用于四轴、平衡车和空中鼠标等设计，具有非常广泛的应用范围。ALIENTEK探索者STM32F4开发板自带了MPU6050传感器。本章我们将使用STM32F4来驱动MPU6050，读取其原始数据，并利用其自带的DMP实现姿态解算，结合匿名四轴上位机软件和LCD显示，教大家如何使用这款功能强大的六轴传感器。本章分为如下几个部分：

37.1 MPU6050简介

37.2 硬件设计

37.3 软件设计

37.4 下载验证

37.1 MPU6050简介

本节，我们将分2个部分介绍：1，MPU6050基础介绍。2，DMP使用简介。

37.1.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传感器的检测轴如图37.1.1.1所示：

图37.1.1.1 MPU6050检测轴及其方向

MPU6050的内部框图如图37.1.1.2所示：

图37.1.1.2 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，注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）！！

在探索者STM32F4开发板上，AD0是接GND的，所以MPU6050的IIC地址是0X68（不含最低位），IIC通信的时序我们在之前已经介绍过（第二十九章，IIC实验），这里就不再细说了。

接下来，我们介绍一下利用STM32F4读取MPU6050的加速度和角度传感器数据（非中断方式），需要哪些初始化步骤：

1）初始化IIC接口

MPU6050采用IIC与STM32F4通信，所以我们需要先初始化与MPU6050连接的SDA和SCL数据线。这个在前面的IIC实验章节已经介绍过了，这里MPU6050与24C02共用一个IIC，所以初始化IIC完全一模一样。

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（即禁止FIFO），所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器（0X19）控制，这个采样率我们一般设置为50即可。数字低通滤波器(DLPF)则通过配置寄存器（0X1A）设置，一般设置DLPF为带宽的1/2即可。

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

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

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

首先，我们介绍电源管理寄存器1，该寄存器地址为0X6B，各位描述如图37.1.1.3所示：

图37.1.1.3 电源管理寄存器1各位描述

其中，DEVICE_RESET位用来控制复位，设置为1，复位MPU6050，复位结束后，MPU硬件自动清零该位。SLEEEP位用于控制MPU6050的工作模式，复位后，该位为1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式。TEMP_DIS用于设置是否使能温度传感器，设置为0，则使能。最后CLKSEL[2:0]用于选择系统时钟源，选择关系如表37.1.1.1所示：

CLKSEL[2:0]	时钟源
000	内部8M RC晶振
001	PLL，使用X轴陀螺作为参考
010	PLL，使用Y轴陀螺作为参考
011	PLL，使用Z轴陀螺作为参考
100	PLL，使用外部32.768Khz作为参考
101	PLL，使用外部19.2Mhz作为参考
110	保留
111	关闭时钟，保持时序产生电路复位状态

图37.1.1.1 CLKSEL选择列表

默认是使用内部8M RC晶振的，精度不高，所以我们一般选择X/Y/Z轴陀螺作为参考的PLL作为时钟源，一般设置CLKSEL=001即可。

接着，我们看陀螺仪配置寄存器，该寄存器地址为：0X1B，各位描述如图37.1.4所示：

图37.1.1.4 陀螺仪配置寄存器各位描述

该寄存器我们只关心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)。

接下来，我们看加速度传感器配置寄存器，寄存器地址为：0X1C，各位描述如图37.1.1.5所示：

图37.1.1.5 加速度传感器配置寄存器各位描述

该寄存器我们只关心AFS_SEL[1:0]这两个位，用于设置加速度传感器的满量程范围：0，±2g；1，±4g；2，±8g；3，±16g；我们一般设置为0，即±2g，因为加速度传感器的ADC也是16位，所以得到灵敏度为：65536/4=16384LSB/g。

接下来，我看看FIFO使能寄存器，寄存器地址为：0X1C，各位描述如图37.1.1.6所示：

图37.1.1.6 FIFO使能寄存器各位描述

该寄存器用于控制FIFO使能，在简单读取传感器数据的时候，可以不用FIFO，设置对应位为0即可禁止FIFO，设置为1，则使能FIFO。注意加速度传感器的3个轴，全由1个位（ACCEL_FIFO_EN）控制，只要该位置1，则加速度传感器的三个通道都开启FIFO了。

接下来，我们看陀螺仪采样率分频寄存器，寄存器地址为：0X19，各位描述如图37.1.1.7所示：

图37.1.1.7 陀螺仪采样率分频寄存器各位描述

该寄存器用于设置MPU6050的陀螺仪采样频率，计算公式为：

采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)

这里陀螺仪的输出频率，是1Khz或者8Khz，与数字低通滤波器（DLPF）的设置有关，当DLPF_CFG=0/7的时候，频率为8Khz，其他情况是1Khz。而且DLPF滤波频率一般设置为采样率的一半。采样率，我们假定设置为50Hz，那么SMPLRT_DIV=1000/50-1=19。

接下来，我们看配置寄存器，寄存器地址为：0X1A，各位描述如图37.1.1.8所示：

图37.1.1.8 配置寄存器各位描述

这里，我们主要关心数字低通滤波器（DLPF）的设置位，即：DLPF_CFG[2:0]，加速度计和陀螺仪，都是根据这三个位的配置进行过滤的。DLPF_CFG不同配置对应的过滤情况如表37.1. 1. 2所示：

DLPF_CFG[2:0]	加速度传感器 Fs=1Khz		角速度传感器（陀螺仪）
DLPF_CFG[2:0]	带宽(Hz)	延迟（ms）	带宽(Hz)	延迟（ms）	Fs(Khz)
000	260	0	256	0.98	8
001	184	2.0	188	1.9	1
010	94	3.0	98	2.8	1
011	44	4.9	42	4.8	1
100	21	8.5	20	8.3	1
101	10	13.8	10	13.4	1
110	5	19.0	5	18.6	1
111	保留		保留		8

图37.1.1.2 DLPF_CFG配置表

这里的加速度传感器，输出速率（Fs）固定是1Khz，而角速度传感器的输出速率（Fs），则根据DLPF_CFG的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半，如前面所说的，如果设置采样率为50Hz，那么带宽就应该设置为25Hz，取近似值20Hz，就应该设置DLPF_CFG=100。

接下来，我们看电源管理寄存器2，寄存器地址为：0X6C，各位描述如图37.1.1.9所示：

图37.1.1.9 电源管理寄存器2各位描述

该寄存器的LP_WAKE_CTRL用于控制低功耗时的唤醒频率，本章用不到。剩下的6位，分别控制加速度和陀螺仪的x/y/z轴是否进入待机模式，这里我们全部都不进入待机模式，所以全部设置为0即可。

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

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

最后，温度传感器的值，可以通过读取0X41（高8位）和0X42（低8位）寄存器得到，温度换算公式为：

Temperature = 36.53 + regval/340

其中，Temperature为计算得到的温度值，单位为℃，regval为从0X41和0X42读到的温度传感器值。

关于MPU6050的基础介绍，我们就介绍到这。MPU6050的详细资料和相关寄存器介绍，请参考光盘：7，硬件资料àMPU6050资料àMPU-6000 and MPU-6050 Product Specification.pdf和MPU-6000 and MPU-6050 Register Map and Descriptions.pdf这两个文档，另外该目录还提供了部分MPU6050的中文资料，供大家参考学习。

37.1.2 DMP使用简介

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

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

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

使用MPU6050的DMP输出的四元数是q30格式的，也就是浮点数放大了2的30次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以2的30次方，然后再进行计算，计算公式为：

q0=quat[0] / q30; //q30格式转换为浮点数

q1=quat[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / q30;

//计算得到俯仰角/横滚角/航向角

pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角

roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //横滚角

yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //航向角

其中quat[0]~ quat[3]是MPU6050的DMP解算后的四元数，q30格式，所以要除以一个2的30次方，其中q30是一个常量：1073741824，即2的30次方，然后带入公式，计算出欧拉角。上述计算公式的57.3是弧度转换为角度，即180/π，这样得到的结果就是以度（°）为单位的。关于四元数与欧拉角的公式推导，这里我们不进行讲解，感兴趣的朋友，可以自行查阅相关资料学习。

InvenSense提供的MPU6050运动驱动库是基于MSP430的，我们需要将其移植一下，才可以用到STM32F4上面，官方原版驱动在光盘：7，硬件资料àMPU6050资料àDMP资料àEmbedded_MotionDriver_5.1.rar，这就是官方原版的驱动，代码比较多，不过官方提供了两个资料供大家学习：Embedded Motion Driver V5.1.1 API 说明.pdf和Embedded Motion Driver V5.1.1 教程.pdf，这两个文件都在DMP资料文件夹里面，大家可以阅读这两个文件，来熟悉官方驱动库的使用。

官方DMP驱动库移植起来，还是比较简单的，主要是实现这4个函数：i2c_write，i2c_read，delay_ms和get_ms，具体细节，我们就不详细介绍了，移植后的驱动代码，我们放在本例程àHARDWAREàMPU6050àeMPL文件夹内，总共6个文件，如图37.1.2.1所示：

图37.1.2.1 移植后的驱动库代码

该驱动库，重点就是两个c文件：inv_mpu.c和inv_mpu_dmp_motion_driver.c。其中我们在inv_mpu.c添加了几个函数，方便我们使用，重点是两个函数：mpu_dmp_init和mpu_dmp_get_data这两个函数，这里我们简单介绍下这两个函数。

mpu_dmp_init，是MPU6050 DMP初始化函数，该函数代码如下：

//mpu6050,dmp初始化

//返回值:0,正常

// 其他,失败

u8 mpu_dmp_init(void)

{

u8 res=0;

IIC_Init(); //初始化IIC总线

if(mpu_init()==0) //初始化MPU6050

{

res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置需要的传感器

if(res)return 1;

res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO

if(res)return 2;

res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率

if(res)return 3;

res=dmp_load_motion_driver_firmware(); //加载dmp固件

if(res)return 4;

res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));

//设置陀螺仪方向

if(res)return 5;

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 6;

res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);//设置DMP输出速率(最大200Hz)

if(res)return 7;

res=run_self_test(); //自检

if(res)return 8;

res=mpu_set_dmp_state(1); //使能DMP

if(res)return 9;

}

return 0;

}

此函数首先通过IIC_Init(需外部提供)初始化与MPU6050连接的IIC接口，然后调用mpu_init函数，初始化MPU6050，之后就是设置DMP所用传感器、FIFO、采样率和加载固件等一些列操作，在所有操作都正常之后，最后通过mpu_set_dmp_state(1)使能DMP功能，在使能成功以后，我们便可以通过mpu_dmp_get_data来读取姿态解算后的数据了。

mpu_dmp_get_data函数代码如下：

//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)

//pitch:俯仰角精度:0.1° 范围:-90.0° <---> +90.0°

//roll:横滚角精度:0.1° 范围:-180.0°<---> +180.0°

//yaw:航向角精度:0.1° 范围:-180.0°<---> +180.0°

//返回值:0,正常其他,失败

u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)

{

float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;

unsigned long sensor_timestamp;

short gyro[3], accel[3], sensors;

unsigned char more;

long quat[4];

if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;

if(sensors&INV_WXYZ_QUAT)

{

q0 = quat[0] / q30; //q30格式转换为浮点数

q1 = quat[1] / q30;

q2 = quat[2] / q30;

q3 = quat[3] / q30;

//计算得到俯仰角/横滚角/航向角

*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch

*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;// roll

*yaw= atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw

}else return 2;

return 0;

}

此函数用于得到DMP姿态解算后的俯仰角、横滚角和航向角。不过本函数局部变量有点多，大家在使用的时候，如果死机，那么请设置堆栈大一点(在startup_stm32f40_41xxx.s里面设置，默认是400)。这里就用到了我们前面介绍的四元数转欧拉角公式，将dmp_read_fifo函数读到的q30格式四元数转换成欧拉角。

利用这两个函数，我们就可以读取到姿态解算后的欧拉角，使用非常方便。DMP部分，我们就介绍到这。

37.2 硬件设计

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

所要用到的硬件资源如下：

1）指示灯DS0

2） KEY0按键

3） TFTLCD模块

4）串口

5） MPU6050

前4个，在之前的实例已经介绍过了，这里我们仅介绍MPU6050与探索者STM32F4开发板的连接。该接口与MCU的连接原理图如37.2.1所示：

图37.2.1 MPU6050与STM32F4的连接电路图

从上图可以看出，MPU6050通过三根线与STM32F4开发板连接，其中IIC总线时和24C02以及WM8978共用，接在PB8和PB9上面。MPU6050的中断输出，连接在STM32F4的PC0脚，不过本例程我们并没有用到中断。另外，AD0接的GND，所以MPU6050的器件地址是：0X68。

37.3 软件设计

软件设计部分请直接下载附件的pdf和实验源码学习

37.4 下载验证

在代码编译成功之后，我们通过下载代码到ALIENTEK探索者STM32F4开发板上，可以看到LCD显示如图37.4.1所示的内容：

图37.4.1 程序运行时LCD显示内容

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

另外，通过按KEY0可以开启或关闭数据上报，开启状态下，我们可以打开：ANO_Tech匿名四轴上位机_V2.6.exe（该软件双击后，会弹出一个蓝色的小界面，直接关闭即可。然后才会进入主界面），这个软件，接收STM32F4上传的数据，从而图形化显示传感器数据以及飞行姿态，如图37.4.2和图37.4.3所示：