MPU6050傳感器解析實驗

19.1 MPU6050簡介

19.1.1 芯片概述

MPU6050是InvenSense公司推出的一款6軸運動處理芯片，內(nèi)置3軸陀螺儀及3軸速度傳感器，內(nèi)置兩組I2C接口，其中一組用于通信，另一組則用于連接外部磁力傳感器，采用自帶的數(shù)字運動處理器DMP（Digital Motion Processor），通過主I2C接口，直接讀取完整的9軸融合演算數(shù)據(jù)。MPU6050檢測軸及其檢測方向如下圖所示。

19.1.2 引腳介紹

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

19.1.3 硬件電路

由于MPU6050內(nèi)部是可以自動計算X，Y和Z軸的方向及加速度的，使用者可以不考慮實際的數(shù)據(jù)轉換問題，但是為了詳細的了解MPU6050的計算過程，使用者最好還是應該具備了解原始數(shù)據(jù)如何轉換為我們需要的角度與加速度值。

19.2 姿態(tài)解算與融合算法基礎概念

19.2.1 方向矩陣

設有一個三位直角坐標系Oxyz，如下圖所示。

19.2.2 方向余弦矩陣

19.2.3 歐拉角

歐拉角是用于確定定點轉動缸體位置的3個1組的獨立角參量，由章動角θ，旋轉角（進動角）ψ和自轉角φ組成，歐拉角有多種取法，下面是比較常見的一種。

如上圖所示，由定點O做出固定坐標系Oxyz以及固定連在剛體的坐標系Ox’y’z’，以軸Oz和Oz’為基本軸。其垂直面Oxy和Ox’y’為基本平面，由軸Oz量到Oz’的角度θ稱為章動角，平面zOz’的垂線ON稱為節(jié)線，同時ON又是基本平面Ox’y’和Oxy的交線，在右手坐標系中，由ON的正端看，角θ應按照逆時針方向計算，由固定軸Ox到節(jié)線ON的角度ψ稱為進動角，也叫作旋轉角，由節(jié)線ON到動軸Ox’的角度φ稱為自轉角，有Oz和Oz’正端看，進動角ψ與自轉角φ也應該按照逆時針方向計算。

從上面的描述過程可以發(fā)現(xiàn)，歐拉角實際是可以分解成三步來計算的：

第1步：繞z軸旋轉α，使得x軸與N軸重合

第2步：繞x軸旋轉β，使z軸與旋轉后的z軸重合

第3步：繞z軸旋轉y，是坐標系與旋轉后的完全重合

根據(jù)上面的三個步驟，我們來通過以下實例來說明歐拉角與方向余弦矩陣的轉換過程。

19.2.4 四元數(shù)與歐拉角的轉換

四元數(shù)是一個簡單的超復數(shù)，是由實數(shù)加上三個虛數(shù)單位i，j，k組成，每個四元數(shù)都是1，i，j，k的線性組合，四元數(shù)是愛爾蘭數(shù)學家哈密頓在1843年發(fā)明的數(shù)學概念，四元數(shù)的乘法不符合交換律。

四元數(shù)姿態(tài)表達式是一個四參數(shù)的表達式，它的基本思路是一個坐標系轉換到另一個坐標系可以通過繞一個定義在參考系中的矢量μ的單次轉動來實現(xiàn)，四元數(shù)用符號q表示，是一個具有4個元素的矢量，這些元素是該矢量方向和轉動大小的函數(shù)。定義四元數(shù)如下所示。

這里直接給出結論，不作證明。會用即可。四元數(shù)與歐拉角的轉換公式為：

用方向余弦表示歐拉角，這里歐拉角不允許等于90度。

用四元數(shù)表示歐拉角

在姿態(tài)解算中常用的算法由歐拉角法，方向余弦法和四元數(shù)法，歐拉角在求解姿態(tài)時存在奇點，無法用于全姿態(tài)結算，方向余弦沒有奇點，但是計算量大，無法滿足實時性要求，四元數(shù)法，計算量小，無奇點可以滿足飛行器運動過程中姿態(tài)的實時解算，姿態(tài)解算的原理是對于一個確定的向量，用不同的坐標系表示時，他們所表示的大小和方向一定是相同的。但是由于這兩個坐標系的旋轉矩陣存在誤差，那么當一個向量經(jīng)過一個有誤差存在的旋轉矩陣后，在另一個坐標系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉矩陣。這個旋轉矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，以此來修正姿態(tài)。

19.3 實驗例程

實驗內(nèi)容：利用MPU6050采集到數(shù)據(jù)獲取歐拉角顯示在TFTLCD上。

19.3.1 MPU6050內(nèi)部相關寄存器

（1） 電源管理寄存器1 （地址0x6B）

Bit 7：軟件復位

0：不復位MPU6050

1：復位MPU6050

Bit 6：休眠模式

0：正常工作模式

1：睡眠模式

Bit 5：循環(huán)模式

0：默認狀態(tài)

1：睡眠模式與喚醒模式交替運行

Bit 3：溫度傳感器使能

0：使能溫度傳感器

1：禁用溫度傳感器

Bit 2~Bit 0：選擇系統(tǒng)時鐘源

000：內(nèi)部8M RC時鐘源

001：PLL，使用X軸陀螺作為參考

010：PLL，使用Y軸陀螺作為參考

011：PLL，使用Z軸陀螺作為參考

100：PLL，使用外部32.768kHz作為參考

101：PLL，使用外部19.2MHz作為參考

110：保留

111：關閉時鐘，保持時序產(chǎn)生電路復位狀態(tài)

（2） 陀螺儀配置寄存器 （地址：0x1B）

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）

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）

Bit 7：TEMP_OUT_H和TEMP_OUT_L緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 6：GYRO_XOUT_H和GYRO_XOUT_L緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 5：GYRO_YOUT_H和GYRO_YOUT_L緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 4：GYRO_ZOUT_H和GYRO_ZOUT_L緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 3：ACCEL_XOUT_H/ L，ACCEL_YOUT_H/L，ACCEL_ZOUT_H/ L緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 2：EXT_SENS_DATA寄存器和從機2緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 1：EXT_SENS_DATA寄存器和從機1緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

Bit 0：EXT_SENS_DATA寄存器和從機0緩沖區(qū)激活

0：關閉該緩沖區(qū)

1：激活該FIFO緩沖區(qū)

（5） 陀螺儀采樣率分頻寄存器 （地址：0x19）

采樣頻率=陀螺儀輸出頻率/（1+SMPLRT_DIV）

（6） 配置寄存器 （地址：0x1A）

Bit 5~Bit 3：該段內(nèi)的值確定采樣的值將代替?zhèn)鞲衅鲾?shù)據(jù)寄存器中的最低有效位

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：低通濾波器設置

（7） 電源管理寄存器2 （地址：0x6C）

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）創(chuàng)建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地址
/*********************************************************************************************************
                數(shù)    據(jù)    結    構    定    義
*********************************************************************************************************/
//如果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數(shù)據(jù)地址寄存器
#define MPU_I2CSLV0_CTRL_REG  0X27  //IIC從機0控制寄存器
#define MPU_I2CSLV1_ADDR_REG  0X28  //IIC從機1器件地址寄存器
#define MPU_I2CSLV1_REG      0X29  //IIC從機1數(shù)據(jù)地址寄存器
#define MPU_I2CSLV1_CTRL_REG  0X2A  //IIC從機1控制寄存器
#define MPU_I2CSLV2_ADDR_REG  0X2B  //IIC從機2器件地址寄存器
#define MPU_I2CSLV2_REG      0X2C  //IIC從機2數(shù)據(jù)地址寄存器
#define MPU_I2CSLV2_CTRL_REG  0X2D  //IIC從機2控制寄存器
#define MPU_I2CSLV3_ADDR_REG  0X2E  //IIC從機3器件地址寄存器
#define MPU_I2CSLV3_REG      0X2F  //IIC從機3數(shù)據(jù)地址寄存器
#define MPU_I2CSLV3_CTRL_REG  0X30  //IIC從機3控制寄存器
#define MPU_I2CSLV4_ADDR_REG  0X31  //IIC從機4器件地址寄存器
#define MPU_I2CSLV4_REG      0X32  //IIC從機4數(shù)據(jù)地址寄存器
#define MPU_I2CSLV4_DO_REG    0X33  //IIC從機4寫數(shù)據(jù)寄存器
#define MPU_I2CSLV4_CTRL_REG  0X34  //IIC從機4控制寄存器
#define MPU_I2CSLV4_DI_REG    0X35  //IIC從機4讀數(shù)據(jù)寄存器


#define MPU_I2CMST_STA_REG    0X36  //IIC主機狀態(tài)寄存器
#define MPU_INTBP_CFG_REG    0X37  //中斷/旁路設置寄存器
#define MPU_INT_EN_REG      0X38  //中斷使能寄存器
#define MPU_INT_STA_REG      0X3A  //中斷狀態(tài)寄存器


#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數(shù)據(jù)寄存器
#define MPU_I2CSLV1_DO_REG    0X64  //IIC從機1數(shù)據(jù)寄存器
#define MPU_I2CSLV2_DO_REG    0X65  //IIC從機2數(shù)據(jù)寄存器
#define MPU_I2CSLV3_DO_REG    0X66  //IIC從機3數(shù)據(jù)寄存器


#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計數(shù)寄存器高八位
#define MPU_FIFO_CNTL_REG    0X73  //FIFO計數(shù)寄存器低八位
#define MPU_FIFO_RW_REG      0X74  //FIFO讀寫寄存器
#define MPU_DEVICE_ID_REG    0X75  //器件ID寄存器
/*********************************************************************************************************
                    函    數(shù)    列    表
*********************************************************************************************************/
void MPU_IIC_Init( void ) ;                                        //初始化IIC
u8 MPU_Init( void ) ;                                          //初始化MPU6050
u8 MPU_Read_Len( u8 addr, u8 reg, u8 len, u8 *buf ) ;                          //IIC連續(xù)讀
u8 MPU_Write_Len( u8 addr, u8 reg, u8 len, u8 *buf ) ;                          //IIC連續(xù)寫
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）創(chuàng)建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發(fā)送1個字節(jié)
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 ;                                          //拉低時鐘開始數(shù)據(jù)傳輸
  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個字節(jié)
Paramater  :
      Ack:應答控制
        0:不應答
        1:應答
Return    :讀取的字節(jié)
***************************************************/
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寫一個字節(jié)
Paramater  :
      reg:寄存器地址
      data:數(shù)據(jù)
Return    :
      0:正常
      其他:錯誤代碼
***************************************************/
u8 MPU_Write_Byte( u8 reg, u8 data )
{ 
    MPU_IIC_Start() ; 
  MPU_IIC_Send_Byte( MPU_ADDR<<1 ) ;                                  //發(fā)送器件地址+寫命令
  //等待應答
  if( MPU_IIC_Wait_Ack() )
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Send_Byte( reg ) ;                                      //寫寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待應答
  MPU_IIC_Send_Byte( data ) ;                                      //發(fā)送數(shù)據(jù)
  //等待ACK
  if( MPU_IIC_Wait_Ack() )
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Stop() ;
  return 0 ;
}
/***************************************************
Name    :MPU_Read_Byte
Function  :IIC讀一個字節(jié)
Paramater  :
      reg:寄存器地址
Return    :讀到的數(shù)據(jù)
***************************************************/
u8 MPU_Read_Byte( u8 reg )
{
  u8 res ;
    MPU_IIC_Start() ;
  MPU_IIC_Send_Byte( MPU_ADDR<<1 ) ;                                  //發(fā)送器件地址+寫命令
  MPU_IIC_Wait_Ack() ;                                        //等待應答
    MPU_IIC_Send_Byte( reg ) ;                                      //寫寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待應答
    MPU_IIC_Start() ;
  MPU_IIC_Send_Byte( ( MPU_ADDR<<1 )|1 ) ;                              //發(fā)送器件地址+讀命令
    MPU_IIC_Wait_Ack() ;                                        //等待應答
  res = MPU_IIC_Read_Byte( 0 ) ;                                    //讀取數(shù)據(jù),發(fā)送nACK
    MPU_IIC_Stop() ;                                          //產(chǎn)生一個停止條件
  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 ) ;                        //設置數(shù)字低通濾波器
  //自動設置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 ) ;                            //設置數(shù)字低通濾波器
}
/***************************************************
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連續(xù)寫
Paramater  :
      addr:器件地址
      reg:寄存器地址
      len:寫入長度
      buf:數(shù)據(jù)區(qū)
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 ) ;                                    //發(fā)送器件地址+寫命令
  if( MPU_IIC_Wait_Ack() )                                      //等待應答
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Send_Byte( reg ) ;                                      //寫寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待應答
  for( i=0; i

（3）創(chuàng)建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 ;                                      //加速度傳感器原始數(shù)據(jù)
  short gyrox, gyroy, gyroz ;                                      //陀螺儀原始數(shù)據(jù)
  float temp ;                                            //溫度
   STM32_Clock_Init( 9 ) ;                                        //系統(tǒng)時鐘設置
  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 ) ;                        //得到加速度傳感器數(shù)據(jù)
      MPU_Get_Gyroscope( &gyrox, &gyroy, &gyroz ) ;                        //得到陀螺儀數(shù)據(jù)
      //轉換溫度
      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 ) ;
    }
  }
}

注：例程使用了網(wǎng)上已經(jīng)移植成功的DMP源碼，直接調用即可。