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格式四元數轉換成歐拉角。