MPU6050传感器解析实验

描述

19.1 MPU6050简介

19.1.1 芯片概述

MPU6050是InvenSense公司推出的一款6轴运动处理芯片,内置3轴陀螺仪及3轴速度传感器,内置两组I2C接口,其中一组用于通信,另一组则用于连接外部磁力传感器,采用自带的数字运动处理器DMP(Digital Motion Processor),通过主I2C接口,直接读取完整的9轴融合演算数据。MPU6050检测轴及其检测方向如下图所示。

DMP

19.1.2 引脚介绍

DMP

MPU6050采用QFN-24封装,端口描述如下表所示。

引脚编号 引脚名称 功能
1 CLKIN 外部参考时钟输入,如果不使用直接接地
2 NC 空引脚
3 NC 空引脚
4 NC 空引脚
5 NC 空引脚
6 AUX_DA 从I2C接口数据口,用于连接磁传感器的SDA组成九轴传感器
7 AUX_CL 从I2C接口时钟口,用于连接磁传感器的SCL组成九轴传感器
8 VLOGIC IO口逻辑电平,最低可以设置1.8V,默认连接VDD
9 AD0 I2C接口地址控制端,端口为高电平默认地址0x69,端口为低电平默认地址0x68
10 REGOUT 外接稳压器的滤波电容
11 FSYNC 帧同步数字输入,如果不使用直接接GND
12 INT 中断信号输出(可以配置为开漏输出)
13 VDD 电源正极,供电范围0.5V~6VDC
14 NC 空引脚
15 NC 空引脚
16 NC 空引脚
17 NC 空引脚
18 GND 电源地
19 RESV 保留
20 CPOUT 外部电荷泵电容
21 RESV 保留
22 RESV 保留
23 SCL 主I2C接口时钟
24 SDA 主I2C接口数据

19.1.3 硬件电路

DMP

由于MPU6050内部是可以自动计算X,Y和Z轴的方向及加速度的,使用者可以不考虑实际的数据转换问题,但是为了详细的了解MPU6050的计算过程,使用者最好还是应该具备了解原始数据如何转换为我们需要的角度与加速度值。

19.2 姿态解算与融合算法基础概念

19.2.1 方向矩阵

设有一个三位直角坐标系Oxyz,如下图所示。

DMP

19.2.2 方向余弦矩阵

DMP

19.2.3 欧拉角

欧拉角是用于确定定点转动缸体位置的3个1组的独立角参量,由章动角θ,旋转角(进动角)ψ和自转角φ组成,欧拉角有多种取法,下面是比较常见的一种。

DMP

如上图所示,由定点O做出固定坐标系Oxyz以及固定连在刚体的坐标系Ox’y’z’,以轴Oz和Oz’为基本轴。其垂直面Oxy和Ox’y’为基本平面,由轴Oz量到Oz’的角度θ称为章动角,平面zOz’的垂线ON称为节线,同时ON又是基本平面Ox’y’和Oxy的交线,在右手坐标系中,由ON的正端看,角θ应按照逆时针方向计算,由固定轴Ox到节线ON的角度ψ称为进动角,也叫作旋转角,由节线ON到动轴Ox’的角度φ称为自转角,有Oz和Oz’正端看,进动角ψ与自转角φ也应该按照逆时针方向计算。

从上面的描述过程可以发现,欧拉角实际是可以分解成三步来计算的:

第1步:绕z轴旋转α,使得x轴与N轴重合

第2步:绕x轴旋转β,使z轴与旋转后的z轴重合

第3步:绕z轴旋转y,是坐标系与旋转后的完全重合

根据上面的三个步骤,我们来通过以下实例来说明欧拉角与方向余弦矩阵的转换过程。

DMP

19.2.4 四元数与欧拉角的转换

四元数是一个简单的超复数,是由实数加上三个虚数单位i,j,k组成,每个四元数都是1,i,j,k的线性组合,四元数是爱尔兰数学家哈密顿在1843年发明的数学概念,四元数的乘法不符合交换律。

四元数姿态表达式是一个四参数的表达式,它的基本思路是一个坐标系转换到另一个坐标系可以通过绕一个定义在参考系中的矢量μ的单次转动来实现,四元数用符号q表示,是一个具有4个元素的矢量,这些元素是该矢量方向和转动大小的函数。定义四元数如下所示。

DMP

这里直接给出结论,不作证明。会用即可。四元数与欧拉角的转换公式为:

DMP

用方向余弦表示欧拉角,这里欧拉角不允许等于90度。

DMP

用四元数表示欧拉角

DMP

在姿态解算中常用的算法由欧拉角法,方向余弦法和四元数法,欧拉角在求解姿态时存在奇点,无法用于全姿态结算,方向余弦没有奇点,但是计算量大,无法满足实时性要求,四元数法,计算量小,无奇点可以满足飞行器运动过程中姿态的实时解算,姿态解算的原理是对于一个确定的向量,用不同的坐标系表示时,他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差,那么当一个向量经过一个有误差存在的旋转矩阵后,在另一个坐标系中肯定和理论值是有偏差的,我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数,我们修正的就是四元数,以此来修正姿态。

19.3 实验例程

实验内容:利用MPU6050采集到数据获取欧拉角显示在TFTLCD上。

19.3.1 MPU6050内部相关寄存器

(1) 电源管理寄存器1 (地址0x6B)

7 6 5 4 3 2 1 0
DEVICE_RST SLEEP CYCLE - TEMP_DIS CLKSEL[2:0]

Bit 7:软件复位

0:不复位MPU6050

1:复位MPU6050

Bit 6:休眠模式

0:正常工作模式

1:睡眠模式

Bit 5:循环模式

0:默认状态

1:睡眠模式与唤醒模式交替运行

Bit 3:温度传感器使能

0:使能温度传感器

1:禁用温度传感器

Bit 2~Bit 0:选择系统时钟源

000:内部8M RC时钟源

001:PLL,使用X轴陀螺作为参考

010:PLL,使用Y轴陀螺作为参考

011:PLL,使用Z轴陀螺作为参考

100:PLL,使用外部32.768kHz作为参考

101:PLL,使用外部19.2MHz作为参考

110:保留

111:关闭时钟,保持时序产生电路复位状态

(2) 陀螺仪配置寄存器 (地址:0x1B)

7 6 5 4 3 2 1 0
XG_ST YG_ST ZG_ST FS_SEL[1:0] - - -

Bit 7:陀螺仪X轴自检

0:禁用

1:启用

Bit 6:陀螺仪Y轴自检

0:禁用

1:启用

Bit 5:陀螺仪Z轴自检

0:禁用

1:启用

Bit 4~Bit 3:陀螺仪满量程

0:±250°/s

1:±500°/s

2:±1000°/s

3:±2000°/s

(3) 加速度传感器配置寄存器 (地址:0x1C)

7 6 5 4 3 2 1 0
XA_ST YA_ST ZA_ST AFS_SEL[1:0] - - -

Bit 7:加速度计X轴自检

0:禁用

1:启用

Bit 6:加速度计Y轴自检

0:禁用

1:启用

Bit 5:加速度计Z轴自检

0:禁用

1:启用

Bit 4~Bit 3:加速度传感器满量程

0:±2g

1:±4g

2:±8g

3:±16g

(4) FIFO使能寄存器 (地址:0x23)

7 6 5 4 3 2 1 0
TEMP XG YG ZG ACCEL SLV2 SLV1 SLV0

Bit 7:TEMP_OUT_H和TEMP_OUT_L缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 6:GYRO_XOUT_H和GYRO_XOUT_L缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 5:GYRO_YOUT_H和GYRO_YOUT_L缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 4:GYRO_ZOUT_H和GYRO_ZOUT_L缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 3:ACCEL_XOUT_H/ L,ACCEL_YOUT_H/L,ACCEL_ZOUT_H/ L缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 2:EXT_SENS_DATA寄存器和从机2缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 1:EXT_SENS_DATA寄存器和从机1缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

Bit 0:EXT_SENS_DATA寄存器和从机0缓冲区激活

0:关闭该缓冲区

1:激活该FIFO缓冲区

(5) 陀螺仪采样率分频寄存器 (地址:0x19)

7 6 5 4 3 2 1 0
SMPLRT_DIV[7:0]

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

(6) 配置寄存器 (地址:0x1A)

7 6 5 4 3 2 1 0
- - EXT_SYNC_SET[2:0] DLPF_CFG[2:0]

Bit 5~Bit 3:该段内的值确定采样的值将代替传感器数据寄存器中的最低有效位

0:输入禁用

1:TEMP_OUT_L寄存器第0位

2:GYRO_XOUT_L寄存器第0位

3:GYRO_YOUT_L寄存器第0位

4:GYRO_ZOUT_L寄存器第0位

5:ACCEL_XOUT_L寄存器第0位

6:ACCEL _YOUT_L寄存器第0位

7:ACCEL _ZOUT_L寄存器第0位

Bit 2~Bit 0:低通滤波器设置

加速度传感器(Fs=1kHz) 角速度传感器
带宽(Hz) 延迟(ms) 带宽(Hz)
000 260 0
001 184 2.0
010 94 3.0
011 44 4.9
100 21 8.5
101 10 13.8
110 5 19.0
111 保留 保留

(7) 电源管理寄存器2 (地址:0x6C)

7 6 5 4 3 2 1 0
LP_WAKE_CTRL[1:0] STBY_XA STBY_YA STBY_ZA STBY_XG STBY_YG XTBY_ZG

Bit 7~Bit 6:低功耗模式下的唤醒频率

0:1.25Hz

1:5Hz

2:20Hz

3:40Hz

Bit 5:X轴加速度待机模式

0:禁用

1:启用

Bit 4:Y轴加速度待机模式

0:禁用

1:启用

Bit 3:Z轴加速度待机模式

0:禁用

1:启用

Bit 2:X轴陀螺仪待机模式

0:禁用

1:启用

Bit 1:Y轴陀螺仪待机模式

0:禁用

1:启用

Bit 0:Z轴陀螺仪待机模式

0:禁用

1:启用

19.3.2 源代码

(1)创建mpu6050.h文件,输入以下代码。

/*********************************************************************************************************
                MUP6050    驱    动    文    件
*********************************************************************************************************/
#ifndef _MPU6050_H_
#define _MPU6050_H_


#include "sys.h"
/*********************************************************************************************************
                硬    件    端    口    定    义
*********************************************************************************************************/
#define MPU_IIC_SCL    PBout( 10)                                    //SCL
#define MPU_IIC_SDA    PBout( 11 )                                    //SDA
#define MPU_READ_SDA  PBin( 11 )                                    //输入SDA
#define MPU_AD0_CTRL  PAout( 15 )                                    //控制AD0电平,从而控制MPU地址
/*********************************************************************************************************
                数    据    结    构    定    义
*********************************************************************************************************/
//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位)
//如果接V3.3,则IIC地址为0X69(不包含最低位)
#define MPU_ADDR        0X68


#define MPU_ACCEL_OFFS_REG    0X06  //accel_offs寄存器,可读取版本号,寄存器手册未提到
#define MPU_PROD_ID_REG      0X0C  //prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG    0X0D  //自检寄存器X
#define MPU_SELF_TESTY_REG    0X0E  //自检寄存器Y
#define MPU_SELF_TESTZ_REG    0X0F  //自检寄存器Z
#define MPU_SELF_TESTA_REG    0X10  //自检寄存器A
#define MPU_SAMPLE_RATE_REG    0X19  //采样频率分频器
#define MPU_CFG_REG        0X1A  //配置寄存器
#define MPU_GYRO_CFG_REG    0X1B  //陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG    0X1C  //加速度计配置寄存器
#define MPU_MOTION_DET_REG    0X1F  //运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG      0X23  //FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG    0X24  //IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG  0X25  //IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG      0X26  //IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG  0X27  //IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG  0X28  //IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG      0X29  //IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG  0X2A  //IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG  0X2B  //IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG      0X2C  //IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG  0X2D  //IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG  0X2E  //IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG      0X2F  //IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG  0X30  //IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG  0X31  //IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG      0X32  //IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG    0X33  //IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG  0X34  //IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG    0X35  //IIC从机4读数据寄存器


#define MPU_I2CMST_STA_REG    0X36  //IIC主机状态寄存器
#define MPU_INTBP_CFG_REG    0X37  //中断/旁路设置寄存器
#define MPU_INT_EN_REG      0X38  //中断使能寄存器
#define MPU_INT_STA_REG      0X3A  //中断状态寄存器


#define MPU_ACCEL_XOUTH_REG    0X3B  //加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG    0X3C  //加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG    0X3D  //加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG    0X3E  //加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG    0X3F  //加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG    0X40  //加速度值,Z轴低8位寄存器


#define MPU_TEMP_OUTH_REG    0X41  //温度值高八位寄存器
#define MPU_TEMP_OUTL_REG    0X42  //温度值低8位寄存器


#define MPU_GYRO_XOUTH_REG    0X43  //陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG    0X44  //陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG    0X45  //陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG    0X46  //陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG    0X47  //陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG    0X48  //陀螺仪值,Z轴低8位寄存器


#define MPU_I2CSLV0_DO_REG    0X63  //IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG    0X64  //IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG    0X65  //IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG    0X66  //IIC从机3数据寄存器


#define MPU_I2CMST_DELAY_REG  0X67  //IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG    0X68  //信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG  0X69  //运动检测控制寄存器
#define MPU_USER_CTRL_REG    0X6A  //用户控制寄存器
#define MPU_PWR_MGMT1_REG    0X6B  //电源管理寄存器1
#define MPU_PWR_MGMT2_REG    0X6C  //电源管理寄存器2 
#define MPU_FIFO_CNTH_REG    0X72  //FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG    0X73  //FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG      0X74  //FIFO读写寄存器
#define MPU_DEVICE_ID_REG    0X75  //器件ID寄存器
/*********************************************************************************************************
                    函    数    列    表
*********************************************************************************************************/
void MPU_IIC_Init( void ) ;                                        //初始化IIC
u8 MPU_Init( void ) ;                                          //初始化MPU6050
u8 MPU_Read_Len( u8 addr, u8 reg, u8 len, u8 *buf ) ;                          //IIC连续读
u8 MPU_Write_Len( u8 addr, u8 reg, u8 len, u8 *buf ) ;                          //IIC连续写
short MPU_Get_Temperature( void ) ;                                    //获取温度
u8 MPU_Get_Gyroscope( short *gx, short *gy, short *gz ) ;                        //获取陀螺仪值
u8 MPU_Get_Accelerometer( short *ax, short *ay, short *az ) ;                      //获取加速度值


#endif

(2)创建mpu6050.c文件,输入以下代码。

/*********************************************************************************************************
                MUP6050    驱    动    程    序
*********************************************************************************************************/
#include "mpu6050.h"
#include "delay.h"
/***************************************************
Name    :MPU_IIC_Init
Function  :初始化IIC
Paramater  :None
Return    :None
***************************************************/
void MPU_IIC_Init()
{
   RCC->APB2ENR |= 1<<3 ;                                        //先使能PB时钟
  GPIOB->CRH &= 0xFFFF00FF ;                                      //PB10/11 推挽输出
  GPIOB->CRH |= 0x00003300 ;
  GPIOB->ODR |= 3<<10 ;                                        //PB10,11 输出高
}
/***************************************************
Name    :MPU_IIC_Wait_Ack
Function  :开始时序
Paramater  :None
Return    :None
***************************************************/
void MPU_IIC_Start()
{
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00003000 ;
  MPU_IIC_SDA = 1 ;
  MPU_IIC_SCL = 1 ;
  delay_us( 2 ) ;
  MPU_IIC_SDA = 0 ;
  delay_us( 2 ) ;
  MPU_IIC_SCL = 0 ;
}
/***************************************************
Name    :MPU_IIC_Wait_Ack
Function  :停止时序
Paramater  :None
Return    :None
***************************************************/
void MPU_IIC_Stop()
{
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00003000 ;
  MPU_IIC_SCL = 0 ;
  MPU_IIC_SDA = 0 ;
  delay_us( 2 ) ;
  MPU_IIC_SCL = 1 ;
  MPU_IIC_SDA = 1 ;
  delay_us( 2 ) ;
}
/***************************************************
Name    :MPU_IIC_Wait_Ack
Function  :应答时序
Paramater  :None
Return    :
      0:成功
      1:失败
***************************************************/
u8 MPU_IIC_Wait_Ack()
{
  u8 ucErrTime=0 ;
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00008000 ;
  MPU_IIC_SDA = 1 ;
  delay_us( 2 ) ;
  MPU_IIC_SCL = 1 ;
  delay_us( 2 ) ;
  while( MPU_READ_SDA )
  {
    ucErrTime ++ ;
    if( ucErrTime>250 )
    {
      MPU_IIC_Stop() ;
      return 1 ;
    }
  }
  MPU_IIC_SCL = 0 ;                                          //时钟输出0
  return 0 ;
}
/***************************************************
Name    :MPU_IIC_Send_Byte
Function  :IIC发送1个字节
Paramater  :
      Ack:应答控制
        0:不应答
        1:应答
Return    :None
***************************************************/
void MPU_IIC_Send_Byte( u8 Byte )
{
  u8 i ;
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00003000 ;
  MPU_IIC_SCL = 0 ;                                          //拉低时钟开始数据传输
  for( i=0; i<8; i++ )
  {
    if( ( Byte&0x80 )==0x80 )
      MPU_IIC_SDA = 1 ;
    else
      MPU_IIC_SDA = 0 ;
        Byte <<= 1 ;
    MPU_IIC_SCL = 1 ;
    delay_us( 2 ) ;
    MPU_IIC_SCL = 0 ;
    delay_us( 2 ) ;
  }
}
/***************************************************
Name    :MPU_IIC_Read_Byte
Function  :IIC读取1个字节
Paramater  :
      Ack:应答控制
        0:不应答
        1:应答
Return    :读取的字节
***************************************************/
u8 MPU_IIC_Read_Byte( u8 Ack )
{
  u8 i, Byte=0;
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00008000 ;
    for( i=0; i<8; i++ )
  {
    MPU_IIC_SCL = 0 ;
    delay_us( 2 ) ;
    MPU_IIC_SCL = 1 ;
    Byte <<= 1 ;
    if( MPU_READ_SDA )
      Byte ++ ;
    delay_us( 2 ) ;
  }
  MPU_IIC_SCL = 0 ;
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00003000 ;
  MPU_IIC_SDA = 1-Ack ;
  delay_us( 2 ) ;
  MPU_IIC_SCL = 1 ;
  delay_us( 2 ) ;
  MPU_IIC_SCL = 0 ;
  return Byte ;
}
/***************************************************
Name    :MPU_Write_Byte
Function  :IIC写一个字节
Paramater  :
      reg:寄存器地址
      data:数据
Return    :
      0:正常
      其他:错误代码
***************************************************/
u8 MPU_Write_Byte( u8 reg, u8 data )
{ 
    MPU_IIC_Start() ; 
  MPU_IIC_Send_Byte( MPU_ADDR<<1 ) ;                                  //发送器件地址+写命令
  //等待应答
  if( MPU_IIC_Wait_Ack() )
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Send_Byte( reg ) ;                                      //写寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待应答
  MPU_IIC_Send_Byte( data ) ;                                      //发送数据
  //等待ACK
  if( MPU_IIC_Wait_Ack() )
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Stop() ;
  return 0 ;
}
/***************************************************
Name    :MPU_Read_Byte
Function  :IIC读一个字节
Paramater  :
      reg:寄存器地址
Return    :读到的数据
***************************************************/
u8 MPU_Read_Byte( u8 reg )
{
  u8 res ;
    MPU_IIC_Start() ;
  MPU_IIC_Send_Byte( MPU_ADDR<<1 ) ;                                  //发送器件地址+写命令
  MPU_IIC_Wait_Ack() ;                                        //等待应答
    MPU_IIC_Send_Byte( reg ) ;                                      //写寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待应答
    MPU_IIC_Start() ;
  MPU_IIC_Send_Byte( ( MPU_ADDR<<1 )|1 ) ;                              //发送器件地址+读命令
    MPU_IIC_Wait_Ack() ;                                        //等待应答
  res = MPU_IIC_Read_Byte( 0 ) ;                                    //读取数据,发送nACK
    MPU_IIC_Stop() ;                                          //产生一个停止条件
  return res ;
}
/***************************************************
Name    :MPU_Read_Byte
Function  :设置MPU6050的采样率(假定Fs=1KHz)
Paramater  :
      rate:4~1000(Hz)
Return    :
      0:成功
      其他:失败
***************************************************/
u8 MPU_Set_Rate( u16 rate )
{
  u8 data ;
  if( rate>1000 )
    rate=1000 ;
  if( rate<4 )
    rate = 4 ;
  data = 1000/rate-1 ;
  data = MPU_Write_Byte( MPU_SAMPLE_RATE_REG, data ) ;                        //设置数字低通滤波器
  //自动设置LPF为采样率的一半
  if( ( rate/2 )>=188 )
    data = 1 ;
  else if( ( rate/2 )>=98 )
    data = 2 ;
  else if( ( rate/2 )>=42 )
    data = 3 ;
  else if( ( rate/2 )>=20 )
    data = 4;
  else if( ( rate/2 )>=10 )
    data = 5 ;
  else
    data = 6 ;
  return MPU_Write_Byte( MPU_CFG_REG, data ) ;                            //设置数字低通滤波器
}
/***************************************************
Name    :MPU_Init
Function  :初始化MPU6050
Paramater  :None
Return    :
      0:成功
      其他:错误代码
***************************************************/
u8 MPU_Init()
{ 
  u8 res ;
  RCC->APB2ENR |= 1<<2 ;                                        //使能PORTA时钟 
  GPIOA->CRH &= 0x0FFFFFFF ;                                      //PA15设置成推挽输出    
  GPIOA->CRH |= 0x30000000 ; 
  JTAG_Set( 1 ) ;                                            //禁止JTAG,从而PA15可以做普通IO使用,否则PA15不能做普通IO
  MPU_AD0_CTRL = 0 ;                                          //控制MPU6050的AD0脚为低电平,从机地址为:0X68
  //初始化IIC总线
  RCC->APB2ENR |= 1<<3 ;                                        //先使能PB时钟
  GPIOB->CRH &= 0xFFFF00FF ;                                      //PB10/11 推挽输出
  GPIOB->CRH |= 0x00003300 ;
  GPIOB->ODR |= 3<<10 ;                                        //PB10,11 输出高
  MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0x80 ) ;                              //复位MPU6050
    delay_ms( 100 ) ;
  MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0x00 ) ;                              //唤醒MPU6050
  MPU_Write_Byte( MPU_GYRO_CFG_REG, 3<<3 ) ;                              //陀螺仪传感器,±2000dps
  MPU_Write_Byte( MPU_ACCEL_CFG_REG, 0<<3 ) ;                              //加速度传感器,±2g
  MPU_Set_Rate( 50 ) ;                                        //设置采样率50Hz
  MPU_Write_Byte( MPU_INT_EN_REG, 0x00 ) ;                              //关闭所有中断
  MPU_Write_Byte( MPU_USER_CTRL_REG, 0x00 ) ;                              //I2C主模式关闭
  MPU_Write_Byte( MPU_FIFO_EN_REG, 0x00 ) ;                              //关闭FIFO
  MPU_Write_Byte( MPU_INTBP_CFG_REG, 0x80 ) ;                              //INT引脚低电平有效
  res = MPU_Read_Byte( MPU_DEVICE_ID_REG ) ;
  //器件ID正确
  if( res==MPU_ADDR )
  {
    MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0x01 ) ;                            //设置CLKSEL,PLL X轴为参考
    MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0x00 ) ;                            //加速度与陀螺仪都工作
    MPU_Set_Rate( 50 ) ;                                      //设置采样率为50Hz
   }
  else
    return 1 ;
  return 0 ;
}
/***************************************************
Name    :MPU_Write_Len
Function  :IIC连续写
Paramater  :
      addr:器件地址
      reg:寄存器地址
      len:写入长度
      buf:数据区
Return    :
      0:成功
      其他:错误代码
***************************************************/
u8 MPU_Write_Len( u8 addr, u8 reg, u8 len, u8 *buf )
{
  u8 i ;
    MPU_IIC_Start() ;
  MPU_IIC_Send_Byte( addr<<1 ) ;                                    //发送器件地址+写命令
  if( MPU_IIC_Wait_Ack() )                                      //等待应答
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Send_Byte( reg ) ;                                      //写寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待应答
  for( i=0; i

(3)创建1.c文件,输入以下代码。

#include "sys.h"
#include "delay.h"
#include "usart1.h"
#include "lcd.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"


int main()
{
  u8 t, Str[ 20 ] ;
  float pitch, roll, yaw ;                                      //欧拉角
  short aacx, aacy, aacz ;                                      //加速度传感器原始数据
  short gyrox, gyroy, gyroz ;                                      //陀螺仪原始数据
  float temp ;                                            //温度
   STM32_Clock_Init( 9 ) ;                                        //系统时钟设置
  SysTick_Init( 72 ) ;                                        //延时初始化
  USART1_Init( 72, 500000 ) ;                                      //串口初始化为500000
  LCD_Init() ;                                            //初始化LCD
  MPU_Init() ;                                            //初始化MPU6050
  while( mpu_dmp_init() ) ;
  POINT_COLOR = RED ;                                          //设置字体为蓝色
   while(1)
  {
    if( mpu_dmp_get_data( &pitch, &roll, &yaw )==0 )
    {
      temp = ( float )MPU_Get_Temperature()/100 ;                          //得到温度值
      MPU_Get_Accelerometer( &aacx, &aacy, &aacz ) ;                        //得到加速度传感器数据
      MPU_Get_Gyroscope( &gyrox, &gyroy, &gyroz ) ;                        //得到陀螺仪数据
      //转换温度
      sprintf( ( char* )Str, "Temp: %.2f C", temp ) ;
      for( t=0; t<20; t++ )
      {
        if( Str[ t ]=='.' )
        {
          t += 4 ;
          while( t<20 )
          {
            t ++ ;
            Str[ t ] = ' ' ;
          }
        }
      }
      LCD_ShowString( 10, 0, Str ) ;
      //自转角
      sprintf( ( char* )Str, "Pitch: %.1f C", pitch  ) ;
      for( t=0; t<20; t++ )
      {
        if( Str[ t ]=='.' )
        {
          t += 3 ;
          while( t<20 )
          {
            t ++ ;
            Str[ t ] = ' ' ;
          }
        }
      }
      LCD_ShowString( 10, 30, Str ) ;


      //章动角
      sprintf( ( char* )Str, "Roll: %.1f C", roll  ) ;
      for( t=0; t<20; t++ )
      {
        if( Str[ t ]=='.' )
        {
          t += 3 ;
          while( t<20 )
          {
            t ++ ;
            Str[ t ] = ' ' ;
          }
        }
      }
      LCD_ShowString( 10, 60, Str ) ;


      //旋转角
      sprintf( ( char* )Str, "Yaw: %.1f C", yaw  ) ;
      for( t=0; t<20; t++ )
      {
        if( Str[ t ]=='.' )
        {
          t += 3 ;
          while( t<20 )
          {
            t ++ ;
            Str[ t ] = ' ' ;
          }
        }
      }
      LCD_ShowString( 10, 90, Str ) ;
    }
  }
}

注:例程使用了网上已经移植成功的DMP源码,直接调用即可。

打开APP阅读更多精彩内容
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉

全部0条评论

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

×
20
完善资料,
赚取积分