mpu6050六轴传感器模块驱动程序源代码分享

　　1、MPU6050基础介绍

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

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

　　MPU6050的特点包括：

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

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

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

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

　　⑤自带数字运动处理（DMP：DigitalMotionProcessing）引擎可减少MCU复杂的融合演算数据、感测器同步化、姿势感应等的负荷

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

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

　　⑧带数字输入同步引脚（Syncpin）支持视频电子影相稳定技术与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传感器的检测轴如图1所示：

　　

　　图1  MPU6050检测轴及其方向

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

　

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

　　接下来，我们介绍一下利用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）设置其他参数

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

　　（0X23）控制，默认都是0（即禁止FIFO），所以用默认值就可以了。陀螺仪采样率通过采

　　样率分频寄存器（0X19）控制，这个采样率我们一般设置为50即可。数字低通滤波器（DLPF）则通过配置寄存器（0X1A）设置，一般设置DLPF为带宽的1/2即可。

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

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

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

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

　　

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

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

　　表1.1.1CLKSEL选择列表

　　

　　

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

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

　　

　　图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，各位描述如图1.1.5所示：

　　

　　图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，各位描述如图1.1.6所示：

　　

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

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

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

　　

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

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

　　采样频率=陀螺仪输出频率/（1+SMPLRT_DIV）

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

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

　　图1.1.2DLPF_CFG配置表

　　

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

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

　　Temperature=36.53+regval/340

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

　　2、DMP使用简介

　　使用内置的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/π，这样得到的结果就是以（°）为单位的。

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

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

　　//mpu6050，dmp初始化

　　//返回值：0，正常

　　//其他，失败

　　u8mpu_dmp_init（void）

　　{

　　u8res=0;

　　IIC_Init（）;//初始化IIC总线

　　if（mpu_init（）==0）//初始化MPU6050

　　{

　　res=mpu_set_sensors（INV_XYZ_GYRO|INV_XYZ_ACCEL）;//需要的传感器

　　if（res）return1;

　　res=mpu_configure_fifo（INV_XYZ_GYRO|INV_XYZ_ACCEL）;//设置FIFO

　　if（res）return2;

　　res=mpu_set_sample_rate（DEFAULT_MPU_HZ）;//设置采样率

　　if（res）return3;

　　res=dmp_load_motion_driver_firmware（）;//加载dmp固件

　　if（res）return4;

　　res=dmp_set_orientation（inv_orientation_matrix_to_scalar（gyro_orientation））;

　　//设置陀螺仪方向

　　if（res）return5;

　　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）return6;

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

　　if（res）return7;

　　res=run_self_test（）;//自检

　　if（res）return8;

　　res=mpu_set_dmp_state（1）;//使能DMP

　　if（res）return9;

　　}

　　return0;

　　}

　　此函数首先通过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，正常

　　//其他，失败

　　u8mpu_dmp_get_data（float*pitch，float*roll，float*yaw）

　　{

　　floatq0=1.0f，q1=0.0f，q2=0.0f，q3=0.0f;

　　unsignedlongsensor_timestamp;

　　shortgyro［3］，accel［3］，sensors;

　　unsignedcharmore;

　　longquat［4］;

　　if（dmp_read_fifo（gyro，accel，quat，&sensor_timestamp，&sensors，&more））return1;

　　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

　　}elsereturn2;

　　return0;

　　}

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