19.1 MPU6050簡介

19.1.1 芯片概述

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

19.1.2 引腳介紹

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

19.1.3 硬件電路

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

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

19.2.1 方向矩陣

設(shè)有一個(gè)三位直角坐標(biāo)系Oxyz，如下圖所示。

19.2.2 方向余弦矩陣

19.2.3 歐拉角

歐拉角是用于確定定點(diǎn)轉(zhuǎn)動(dòng)缸體位置的3個(gè)1組的獨(dú)立角參量，由章動(dòng)角θ，旋轉(zhuǎn)角（進(jìn)動(dòng)角）ψ和自轉(zhuǎn)角φ組成，歐拉角有多種取法，下面是比較常見的一種。

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

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

第1步：繞z軸旋轉(zhuǎn)α，使得x軸與N軸重合

第2步：繞x軸旋轉(zhuǎn)β，使z軸與旋轉(zhuǎn)后的z軸重合

第3步：繞z軸旋轉(zhuǎn)y，是坐標(biāo)系與旋轉(zhuǎn)后的完全重合

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

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

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

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

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

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

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

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

19.3 實(shí)驗(yàn)例程

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

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

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

Bit 7：軟件復(fù)位

0：不復(fù)位MPU6050

1：復(fù)位MPU6050

Bit 6：休眠模式

0：正常工作模式

1：睡眠模式

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

0：默認(rèn)狀態(tài)

1：睡眠模式與喚醒模式交替運(yùn)行

Bit 3：溫度傳感器使能

0：使能溫度傳感器

1：禁用溫度傳感器

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

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

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

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

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

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

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

110：保留

111：關(guān)閉時(shí)鐘，保持時(shí)序產(chǎn)生電路復(fù)位狀態(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：加速度計(jì)X軸自檢

0：禁用

1：啟用

Bit 6：加速度計(jì)Y軸自檢

0：禁用

1：啟用

Bit 5：加速度計(jì)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：關(guān)閉該緩沖區(qū)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

0：關(guān)閉該緩沖區(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：低通濾波器設(shè)置

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

Bit 7~Bit 6：低功耗模式下的喚醒頻率

0：1.25Hz

1：5Hz

2：20Hz

3：40Hz

Bit 5：X軸加速度待機(jī)模式

0：禁用

1：啟用

Bit 4：Y軸加速度待機(jī)模式

0：禁用

1：啟用

Bit 3：Z軸加速度待機(jī)模式

0：禁用

1：啟用

Bit 2：X軸陀螺儀待機(jī)模式

0：禁用

1：啟用

Bit 1：Y軸陀螺儀待機(jī)模式

0：禁用

1：啟用

Bit 0：Z軸陀螺儀待機(jī)模式

0：禁用

1：啟用

19.3.2 源代碼

（1）創(chuàng)建mpu6050.h文件，輸入以下代碼。

/*********************************************************************************************************
                MUP6050    驅(qū)    動(dòng)    文    件
*********************************************************************************************************/
#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ù)    結(jié)    構(gòu)    定    義
*********************************************************************************************************/
//如果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  //加速度計(jì)配置寄存器
#define MPU_MOTION_DET_REG    0X1F  //運(yùn)動(dòng)檢測閥值設(shè)置寄存器
#define MPU_FIFO_EN_REG      0X23  //FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG    0X24  //IIC主機(jī)控制寄存器
#define MPU_I2CSLV0_ADDR_REG  0X25  //IIC從機(jī)0器件地址寄存器
#define MPU_I2CSLV0_REG      0X26  //IIC從機(jī)0數(shù)據(jù)地址寄存器
#define MPU_I2CSLV0_CTRL_REG  0X27  //IIC從機(jī)0控制寄存器
#define MPU_I2CSLV1_ADDR_REG  0X28  //IIC從機(jī)1器件地址寄存器
#define MPU_I2CSLV1_REG      0X29  //IIC從機(jī)1數(shù)據(jù)地址寄存器
#define MPU_I2CSLV1_CTRL_REG  0X2A  //IIC從機(jī)1控制寄存器
#define MPU_I2CSLV2_ADDR_REG  0X2B  //IIC從機(jī)2器件地址寄存器
#define MPU_I2CSLV2_REG      0X2C  //IIC從機(jī)2數(shù)據(jù)地址寄存器
#define MPU_I2CSLV2_CTRL_REG  0X2D  //IIC從機(jī)2控制寄存器
#define MPU_I2CSLV3_ADDR_REG  0X2E  //IIC從機(jī)3器件地址寄存器
#define MPU_I2CSLV3_REG      0X2F  //IIC從機(jī)3數(shù)據(jù)地址寄存器
#define MPU_I2CSLV3_CTRL_REG  0X30  //IIC從機(jī)3控制寄存器
#define MPU_I2CSLV4_ADDR_REG  0X31  //IIC從機(jī)4器件地址寄存器
#define MPU_I2CSLV4_REG      0X32  //IIC從機(jī)4數(shù)據(jù)地址寄存器
#define MPU_I2CSLV4_DO_REG    0X33  //IIC從機(jī)4寫數(shù)據(jù)寄存器
#define MPU_I2CSLV4_CTRL_REG  0X34  //IIC從機(jī)4控制寄存器
#define MPU_I2CSLV4_DI_REG    0X35  //IIC從機(jī)4讀數(shù)據(jù)寄存器


#define MPU_I2CMST_STA_REG    0X36  //IIC主機(jī)狀態(tài)寄存器
#define MPU_INTBP_CFG_REG    0X37  //中斷/旁路設(shè)置寄存器
#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從機(jī)0數(shù)據(jù)寄存器
#define MPU_I2CSLV1_DO_REG    0X64  //IIC從機(jī)1數(shù)據(jù)寄存器
#define MPU_I2CSLV2_DO_REG    0X65  //IIC從機(jī)2數(shù)據(jù)寄存器
#define MPU_I2CSLV3_DO_REG    0X66  //IIC從機(jī)3數(shù)據(jù)寄存器


#define MPU_I2CMST_DELAY_REG  0X67  //IIC主機(jī)延時(shí)管理寄存器
#define MPU_SIGPATH_RST_REG    0X68  //信號通道復(fù)位寄存器
#define MPU_MDETECT_CTRL_REG  0X69  //運(yùn)動(dòng)檢測控制寄存器
#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計(jì)數(shù)寄存器高八位
#define MPU_FIFO_CNTL_REG    0X73  //FIFO計(jì)數(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    驅(qū)    動(dòng)    程    序
*********************************************************************************************************/
#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時(shí)鐘
  GPIOB->CRH &= 0xFFFF00FF ;                                      //PB10/11 推挽輸出
  GPIOB->CRH |= 0x00003300 ;
  GPIOB->ODR |= 3<<10 ;                                        //PB10,11 輸出高
}
/***************************************************
Name    :MPU_IIC_Wait_Ack
Function  :開始時(shí)序
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  :停止時(shí)序
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  :應(yīng)答時(shí)序
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 ;                                          //時(shí)鐘輸出0
  return 0 ;
}
/***************************************************
Name    :MPU_IIC_Send_Byte
Function  :IIC發(fā)送1個(gè)字節(jié)
Paramater  :
      Ack:應(yīng)答控制
        0:不應(yīng)答
        1:應(yīng)答
Return    :None
***************************************************/
void MPU_IIC_Send_Byte( u8 Byte )
{
  u8 i ;
  GPIOB->CRH &= 0xFFFF0FFF ;
  GPIOB->CRH |= 0x00003000 ;
  MPU_IIC_SCL = 0 ;                                          //拉低時(shí)鐘開始數(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個(gè)字節(jié)
Paramater  :
      Ack:應(yīng)答控制
        0:不應(yīng)答
        1:應(yīng)答
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寫一個(gè)字節(jié)
Paramater  :
      reg:寄存器地址
      data:數(shù)據(jù)
Return    :
      0:正常
      其他:錯(cuò)誤代碼
***************************************************/
u8 MPU_Write_Byte( u8 reg, u8 data )
{ 
    MPU_IIC_Start() ; 
  MPU_IIC_Send_Byte( MPU_ADDR<<1 ) ;                                  //發(fā)送器件地址+寫命令
  //等待應(yīng)答
  if( MPU_IIC_Wait_Ack() )
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Send_Byte( reg ) ;                                      //寫寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待應(yīng)答
  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讀一個(gè)字節(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() ;                                        //等待應(yīng)答
    MPU_IIC_Send_Byte( reg ) ;                                      //寫寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待應(yīng)答
    MPU_IIC_Start() ;
  MPU_IIC_Send_Byte( ( MPU_ADDR<<1 )|1 ) ;                              //發(fā)送器件地址+讀命令
    MPU_IIC_Wait_Ack() ;                                        //等待應(yīng)答
  res = MPU_IIC_Read_Byte( 0 ) ;                                    //讀取數(shù)據(jù),發(fā)送nACK
    MPU_IIC_Stop() ;                                          //產(chǎn)生一個(gè)停止條件
  return res ;
}
/***************************************************
Name    :MPU_Read_Byte
Function  :設(shè)置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è)置數(shù)字低通濾波器
  //自動(dòng)設(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è)置數(shù)字低通濾波器
}
/***************************************************
Name    :MPU_Init
Function  :初始化MPU6050
Paramater  :None
Return    :
      0:成功
      其他:錯(cuò)誤代碼
***************************************************/
u8 MPU_Init()
{ 
  u8 res ;
  RCC->APB2ENR |= 1<<2 ;                                        //使能PORTA時(shí)鐘 
  GPIOA->CRH &= 0x0FFFFFFF ;                                      //PA15設(shè)置成推挽輸出    
  GPIOA->CRH |= 0x30000000 ; 
  JTAG_Set( 1 ) ;                                            //禁止JTAG,從而PA15可以做普通IO使用,否則PA15不能做普通IO
  MPU_AD0_CTRL = 0 ;                                          //控制MPU6050的AD0腳為低電平,從機(jī)地址為:0X68
  //初始化IIC總線
  RCC->APB2ENR |= 1<<3 ;                                        //先使能PB時(shí)鐘
  GPIOB->CRH &= 0xFFFF00FF ;                                      //PB10/11 推挽輸出
  GPIOB->CRH |= 0x00003300 ;
  GPIOB->ODR |= 3<<10 ;                                        //PB10,11 輸出高
  MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0x80 ) ;                              //復(fù)位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 ) ;                                        //設(shè)置采樣率50Hz
  MPU_Write_Byte( MPU_INT_EN_REG, 0x00 ) ;                              //關(guān)閉所有中斷
  MPU_Write_Byte( MPU_USER_CTRL_REG, 0x00 ) ;                              //I2C主模式關(guān)閉
  MPU_Write_Byte( MPU_FIFO_EN_REG, 0x00 ) ;                              //關(guān)閉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 ) ;                            //設(shè)置CLKSEL,PLL X軸為參考
    MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0x00 ) ;                            //加速度與陀螺儀都工作
    MPU_Set_Rate( 50 ) ;                                      //設(shè)置采樣率為50Hz
   }
  else
    return 1 ;
  return 0 ;
}
/***************************************************
Name    :MPU_Write_Len
Function  :IIC連續(xù)寫
Paramater  :
      addr:器件地址
      reg:寄存器地址
      len:寫入長度
      buf:數(shù)據(jù)區(qū)
Return    :
      0:成功
      其他:錯(cuò)誤代碼
***************************************************/
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() )                                      //等待應(yīng)答
  {
    MPU_IIC_Stop() ;
    return 1 ;
  }
    MPU_IIC_Send_Byte( reg ) ;                                      //寫寄存器地址
    MPU_IIC_Wait_Ack() ;                                        //等待應(yīng)答
  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)時(shí)鐘設(shè)置
  SysTick_Init( 72 ) ;                                        //延時(shí)初始化
  USART1_Init( 72, 500000 ) ;                                      //串口初始化為500000
  LCD_Init() ;                                            //初始化LCD
  MPU_Init() ;                                            //初始化MPU6050
  while( mpu_dmp_init() ) ;
  POINT_COLOR = RED ;                                          //設(shè)置字體為藍(lán)色
   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ù)
      //轉(zhuǎn)換溫度
      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 ) ;
      //自轉(zhuǎn)角
      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 ) ;


      //章動(dòng)角
      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 ) ;


      //旋轉(zhuǎn)角
      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源碼，直接調(diào)用即可。