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

传感器

282人已加入

描述

  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所示:

  MPU6050

  图1  MPU6050检测轴及其方向

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

 MPU6050

  图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所示:

  MPU6050

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

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

  表1.1.1CLKSEL选择列表

  MPU6050

  MPU6050

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

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

  MPU6050

  图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所示:

  MPU6050

  图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所示:

  MPU6050

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

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

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

  MPU6050

  图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配置表

  MPU6050

  这里的加速度传感器,输出速率(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格式四元数转换成欧拉角。

打开APP阅读更多精彩内容
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
评论(0)
发评论
莫比乌斯环 2019-07-11
0 回复 举报
使用这个函数,把欧拉角放在Oled上显示,只有Yaw存在正负值,roll只存在负值,pitch只存在正值,这种情况该如何处理 收起回复
饶杰151 2019-04-01
0 回复 举报
不错不错 收起回复

全部0条评论

快来发表一下你的评论吧 !

×
20
完善资料,
赚取积分