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次方，然后再進行計算，計算公式為：
1 q0=quat［0］/q30;//q30格式轉換為浮點數 2 3 q1=quat［1］/q30; 4 5 q2=quat［2］/q30; 6 7 q3=quat［3］/q30; 8 9 //計算得到俯仰角/橫滾角/航向角 10 11 pitch=asin（-2*q1*q3+2*q0*q2）*57.3;//俯仰角 12 13 roll=atan2（2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1）*57.3;//橫滾角 14 15 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初始化函數，該函數代碼如下：
1 //mpu6050，dmp初始化 2 3 //返回值：0，正常 4 5 //其他，失敗 6 7 u8mpu_dmp_init（void） 8 9 { 10 11 u8res=0; 12 13 IIC_Init（）;//初始化IIC總線 14 15 if（mpu_init（）==0）//初始化MPU6050 16 17 { 18 19 res=mpu_set_sensors（INV_XYZ_GYRO|INV_XYZ_ACCEL）;//需要的傳感器 20 21 if（res）return1; 22 23 res=mpu_configure_fifo（INV_XYZ_GYRO|INV_XYZ_ACCEL）;//設置FIFO 24 25 if（res）return2; 26 27 res=mpu_set_sample_rate（DEFAULT_MPU_HZ）;//設置采樣率 28 29 if（res）return3; 30 31 res=dmp_load_moTIon_driver_firmware（）;//加載dmp固件 32 33 if（res）return4; 34 35 res=dmp_set_orientaTIon（inv_orientation_matrix_to_scalar（gyro_orientation））; 36 37 //設置陀螺儀方向 38 39 if（res）return5; 40 41 res=dmp_enable_feature（DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP 42 43 |DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL 44 45 |DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL）; 46 47 //設置dmp功能 48 49 if（res）return6; 50 51 res=dmp_set_fifo_rate（DEFAULT_MPU_HZ）;//設置DMP輸出速率（最大200Hz） 52 53 if（res）return7; 54 55 res=run_self_test（）;//自檢 56 57 if（res）return8; 58 59 res=mpu_set_dmp_state（1）;//使能DMP 60 61 if（res）return9; 62 63 } 64 65 return0; 66 67 }
此函數首先通過IIC_Init（需外部提供）初始化與MPU6050連接的IIC接口，然后調用mpu_init函數，初始化MPU6050，之后就是設置DMP所用傳感器、FIFO、采樣率和加載固件等一系列操作，在所有操作都正常之后，最后通過mpu_set_dmp_state（1）使能DMP功能，在使能成功以后，我們便可以通過mpu_dmp_get_data來讀取姿態解算后的數據了。
mpu_dmp_get_data函數代碼如下：
1 //得到dmp處理后的數據（注意，本函數需要比較多堆棧，局部變量有點多） 2 3 //pitch：俯仰角精度：0.1°范圍：-90.0°《---》+90.0° 4 5 //roll：橫滾角精度：0.1°范圍：-180.0°《---》+180.0° 6 7 //yaw：航向角精度：0.1°范圍：-180.0°《---》+180.0° 8 9 //返回值：0，正常 10 11 //其他，失敗 12 13 u8mpu_dmp_get_data（float*pitch，float*roll，float*yaw） 14 15 { 16 17 floatq0=1.0f，q1=0.0f，q2=0.0f，q3=0.0f; 18 19 unsignedlongsensor_timestamp; 20 21 shortgyro［3］，accel［3］，sensors; 22 23 unsignedcharmore; 24 25 longquat［4］; 26 27 if（dmp_read_fifo（gyro，accel，quat，&sensor_timestamp，&sensors，&more））return1; 28 29 if（sensors&INV_WXYZ_QUAT） 30 31 { 32 33 q0=quat［0］/q30;//q30格式轉換為浮點數 34 35 q1=quat［1］/q30; 36 37 q2=quat［2］/q30; 38 39 q3=quat［3］/q30; 40 41 //計算得到俯仰角/橫滾角/航向角 42 43 *pitch=asin（-2*q1*q3+2*q0*q2）*57.3;//pitch 44 45 *roll=atan2（2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1）*57.3;//roll 46 47 *yaw=atan2（2*（q1*q2+q0*q3），q0*q0+q1*q1-q2*q2-q3*q3）*57.3;//yaw 48 49 }elsereturn2; 50 51 return0; 52 53 }
此函數用於得到DMP姿態解算后的俯仰角、橫滾角和航向角。不過本函數局部變量有點多，大家在使用的時候，如果死機，那么請設置堆棧大一點（在startup_stm32f10x_hd.s里面設置，默認是400）。這里就用到了我們前面介紹的四元數轉歐拉角公式，將dmp_read_fifo函數讀到的q30格式四元數轉換成歐拉角。