1、MPU6050基礎(chǔ)介紹
MPU6050是InvenSense公司推出的全球首款整合性6軸運(yùn)動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了安裝空間。
MPU6050內(nèi)部整合了3軸陀螺儀和3軸加速度傳感器，并且含有一個第二IIC接口，可用于連接外部磁力傳感器，并利用自帶的數(shù)字運(yùn)動處理器（DMP：DigitalMotionProcessor）硬件加速引擎，通過主IIC接口，向應(yīng)用端輸出完整的9軸融合演算數(shù)據(jù)。有了DMP，我們可以使用InvenSense公司提供的運(yùn)動處理資料庫，非常方便的實(shí)現(xiàn)姿態(tài)解算，降低了運(yùn)動處理運(yùn)算對操作系統(tǒng)的負(fù)荷，同時大大降低了開發(fā)難度。
MPU6050的特點(diǎn)包括：
?、僖詳?shù)字形式輸出6軸或9軸（需外接磁傳感器）的旋轉(zhuǎn)矩陣、四元數(shù)（quaternion）、歐拉角格式（EulerAngleforma）的融合演算數(shù)據(jù)（需DMP支持）
?、诰哂?31LSBs/°/sec敏感度與全格感測范圍為±250、±500、±1000與±2000°/sec的3軸角速度感測器（陀螺儀）
?、奂煽沙绦蚩刂?，范圍為±2g、±4g、±8g和±16g的3軸加速度傳感器
?、芤瞥铀倨髋c陀螺儀軸間敏感度，降低設(shè)定給予的影響與感測器的飄移
?、葑詭?shù)字運(yùn)動處理（DMP：DigitalMotionProcessing）引擎可減少MCU復(fù)雜的融合演算數(shù)據(jù)、感測器同步化、姿勢感應(yīng)等的負(fù)荷
?、迌?nèi)建運(yùn)作時間偏差與磁力感測器校正演算技術(shù)，免除了客戶須另外進(jìn)行校正的需求
?、咦詭б粋€數(shù)字溫度傳感器
?、鄮?shù)字輸入同步引腳（Syncpin）支持視頻電子影相穩(wěn)定技術(shù)與GPS
?、峥沙绦蚩刂频闹袛啵╥nterrupt），支持姿勢識別、搖攝、畫面放大縮小、滾動、快速下降中斷、high-G中斷、零動作感應(yīng)、觸擊感應(yīng)、搖動感應(yīng)功能
?、釼DD供電電壓為2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC可低至1.8V±5%
?陀螺儀工作電流：5mA，陀螺儀待機(jī)電流：5uA；加速器工作電流：500uA，加速器省電模式電流：40uA@10Hz
?自帶1024字節(jié)FIFO，有助于降低系統(tǒng)功耗
?高達(dá)400Khz的IIC通信接口
?超小封裝尺寸：4x4x0.9mm（QFN）MPU6050傳感器的檢測軸如圖1所示：
圖1 ?MPU6050檢測軸及其方向
MPU6050的內(nèi)部框圖如圖2所示：
圖2 MPU6050框圖
其中，SCL和SDA是連接MCU的IIC接口，MCU通過這個IIC接口來控制MPU6050，另外還有一個IIC接口：AUX_CL和AUX_DA，這個接口可用來連接外部從設(shè)備，比如磁傳感器，這樣就可以組成一個九軸傳感器。VLOGIC是IO口電壓，該引腳最低可以到1.8V，我們一般直接接VDD即可。AD0是從IIC接口（接MCU）的地址控制引腳，該引腳控制IIC地址的最低位。如果接GND，則MPU6050的IIC地址是：0X68，如果接VDD，則是0X69，注意：這里的地址是不包含數(shù)據(jù)傳輸?shù)淖畹臀坏模ㄗ畹臀挥脕肀硎咀x寫）??！
接下來，我們介紹一下利用STM32F1讀取MPU6050的加速度和角度傳感器數(shù)據(jù)（非中斷方式），需要哪些初始化步驟：
1）初始化IIC接口
MPU6050采用IIC與STM32F1通信，所以我們需要先初始化與MPU6050連接的SDA和SCL數(shù)據(jù)線。
2）復(fù)位MPU6050
這一步讓MPU6050內(nèi)部所有寄存器恢復(fù)默認(rèn)值，通過對電源管理寄存器1（0X6B）的bit7寫1實(shí)現(xiàn)。復(fù)位后，電源管理寄存器1恢復(fù)默認(rèn)值（0X40），然后必須設(shè)置該寄存器為0X00，以喚醒MPU6050，進(jìn)入正常工作狀態(tài)。
3）設(shè)置角速度傳感器（陀螺儀）和加速度傳感器的滿量程范圍
這一步，我們設(shè)置兩個傳感器的滿量程范圍（FSR），分別通過陀螺儀配置寄存器（0X1B）和加速度傳感器配置寄存器（0X1C）設(shè)置。我們一般設(shè)置陀螺儀的滿量程范圍為±2000dps，加速度傳感器的滿量程范圍為±2g。
4）設(shè)置其他參數(shù)
這里，我們還需要配置的參數(shù)有：關(guān)閉中斷、關(guān)閉AUXIIC接口、禁止FIFO、設(shè)置陀螺儀采樣率和設(shè)置數(shù)字低通濾波器（DLPF）等。本章我們不用中斷方式讀取數(shù)據(jù)，所以關(guān)閉中斷，然后也沒用到AUXIIC接口外接其他傳感器，所以也關(guān)閉這個接口。分別通過中斷使能寄存器（0X38）和用戶控制寄存器（0X6A）控制。MPU6050可以使用FIFO存儲傳感器數(shù)據(jù)，不過本章我們沒有用到，所以關(guān)閉所有FIFO通道，這個通過FIFO使能寄存器
?。?X23）控制，默認(rèn)都是0（即禁止FIFO），所以用默認(rèn)值就可以了。陀螺儀采樣率通過采
樣率分頻寄存器（0X19）控制，這個采樣率我們一般設(shè)置為50即可。數(shù)字低通濾波器（DLPF）則通過配置寄存器（0X1A）設(shè)置，一般設(shè)置DLPF為帶寬的1/2即可。
5）配置系統(tǒng)時鐘源并使能角速度傳感器和加速度傳感器
系統(tǒng)時鐘源同樣是通過電源管理寄存器1（0X1B）來設(shè)置，該寄存器的最低三位用于設(shè)置系統(tǒng)時鐘源選擇，默認(rèn)值是0（內(nèi)部8MRC震蕩），不過我們一般設(shè)置為1，選擇x軸陀螺PLL作為時鐘源，以獲得更高精度的時鐘。同時，使能角速度傳感器和加速度傳感器，這兩個操作通過電源管理寄存器2（0X6C）來設(shè)置，設(shè)置對應(yīng)位為0即可開啟。
至此，MPU6050的初始化就完成了，可以正常工作了（其他未設(shè)置的寄存器全部采用默認(rèn)值即可），接下來，我們就可以讀取相關(guān)寄存器，得到加速度傳感器、角速度傳感器和溫度傳感器的數(shù)據(jù)了。
首先，我們介紹電源管理寄存器1，該寄存器地址為0X6B，各位描述如圖1.1.3所示：
圖1.1.3電源管理寄存器1各位描述
其中，DEVICE_RESET位用來控制復(fù)位，設(shè)置為1，復(fù)位MPU6050，復(fù)位結(jié)束后，MPU硬件自動清零該位。SLEEEP位用于控制MPU6050的工作模式，復(fù)位后，該位為1，即進(jìn)入了睡眠模式（低功耗），所以我們要清零該位，以進(jìn)入正常工作模式。TEMP_DIS用于設(shè)置是否使能溫度傳感器，設(shè)置為0，則使能。最后CLKSEL［2:0］用于選擇系統(tǒng)時鐘源，選擇關(guān)系如表1.1.1所示：
表1.1.1CLKSEL選擇列表
默認(rèn)是使用內(nèi)部8MRC晶振的，精度不高，所以我們一般選擇X/Y/Z軸陀螺作為參考的PLL作為時鐘源，一般設(shè)置CLKSEL=001即可。
接著，我們看陀螺儀配置寄存器，該寄存器地址為：0X1B，各位描述如圖1.1.4所示：
圖1.1.4陀螺儀配置寄存器各位描述
該寄存器我們只關(guān)心FS_SEL［1:0］這兩個位，用于設(shè)置陀螺儀的滿量程范圍：0，±250°/S；1，±500°/S；2，±1000°/S；3，±2000°/S；我們一般設(shè)置為3，即±2000°/S，因?yàn)橥勇輧x的ADC為16位分辨率，所以得到靈敏度為：65536/4000=16.4LSB/（°/S）。
接下來，我們看加速度傳感器配置寄存器，寄存器地址為：0X1C，各位描述如圖1.1.5所示：
圖1.1.5加速度傳感器配置寄存器各位描述
該寄存器我們只關(guān)心AFS_SEL［1:0］這兩個位，用于設(shè)置加速度傳感器的滿量程范圍：0，±2g；1，±4g；2，±8g；3，±16g；我們一般設(shè)置為0，即±2g，因?yàn)榧铀俣葌鞲衅鞯腁DC也是16位，所以得到靈敏度為：65536/4=16384LSB/g。
接下來，我看看FIFO使能寄存器，寄存器地址為：0X1C，各位描述如圖1.1.6所示：
圖1.1.6FIFO使能寄存器各位描述
該寄存器用于控制FIFO使能，在簡單讀取傳感器數(shù)據(jù)的時候，可以不用FIFO，設(shè)置對應(yīng)位為0即可禁止FIFO，設(shè)置為1，則使能FIFO。注意加速度傳感器的3個軸，全由1個位（ACCEL_FIFO_EN）控制，只要該位置1，則加速度傳感器的三個通道都開啟FIFO了。
接下來，我們看陀螺儀采樣率分頻寄存器，寄存器地址為：0X19，各位描述如圖1.1.7所示：
圖1.1.7陀螺儀采樣率分頻寄存器各位描述
該寄存器用于設(shè)置MPU6050的陀螺儀采樣頻率，計算公式為：
采樣頻率=陀螺儀輸出頻率/（1+SMPLRT_DIV）
這里陀螺儀的輸出頻率，是1Khz或者8Khz，與數(shù)字低通濾波器（DLPF）的設(shè)置有關(guān)，當(dāng)DLPF_CFG=0/7的時候，頻率為8Khz，其他情況是1Khz。而且DLPF濾波頻率一般設(shè)置為采樣率的一半。采樣率，我們假定設(shè)置為50Hz，那么SMPLRT_DIV=1000/50-1=19。
這里，我們主要關(guān)心數(shù)字低通濾波器（DLPF）的設(shè)置位，即：DLPF_CFG［2:0］，加速度計和陀螺儀，都是根據(jù)這三個位的配置進(jìn)行過濾的。DLPF_CFG不同配置對應(yīng)的過濾情況如表1.1.2所示：
圖1.1.2DLPF_CFG配置表
這里的加速度傳感器，輸出速率（Fs）固定是1Khz，而角速度傳感器的輸出速率（Fs），則根據(jù)DLPF_CFG的配置有所不同。一般我們設(shè)置角速度傳感器的帶寬為其采樣率的一半，如前面所說的，如果設(shè)置采樣率為50Hz，那么帶寬就應(yīng)該設(shè)置為25Hz，取近似值20Hz，就應(yīng)該設(shè)置DLPF_CFG=100。
最后，溫度傳感器的值，可以通過讀取0X41（高8位）和0X42（低8位）寄存器得到，溫度換算公式為：
Temperature=36.53+regval/340
其中，Temperature為計算得到的溫度值，單位為℃，regval為從0X41和0X42讀到的溫度傳感器值。
2、DMP使用簡介
使用內(nèi)置的DMP，大大簡化了四軸的代碼設(shè)計，且MCU不用進(jìn)行姿態(tài)解算過程，大大降低了MCU的負(fù)擔(dān)，從而有更多的時間去處理其他事件，提高系統(tǒng)實(shí)時性。
使用MPU6050的DMP輸出的四元數(shù)是q30格式的，也就是浮點(diǎn)數(shù)放大了2的30次方倍。在換算成歐拉角之前，必須先將其轉(zhuǎn)換為浮點(diǎn)數(shù)，也就是除以2的30次方，然后再進(jìn)行計算，計算公式為：
q0=quat［0］/q30;//q30格式轉(zhuǎn)換為浮點(diǎn)數(shù)
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解算后的四元數(shù)，q30格式，所以要除以一個2的30次方，其中q30是一個常量：1073741824，即2的30次方，然后帶入公式，計算出歐拉角。上述計算公式的57.3是弧度轉(zhuǎn)換為角度，即180/π，這樣得到的結(jié)果就是以（°）為單位的。
其中我們在inv_mpu.c添加了幾個函數(shù)，方便我們使用，重點(diǎn)是兩個函數(shù)：mpu_dmp_init和mpu_dmp_get_data這兩個函數(shù)，這里我們簡單介紹下這兩個函數(shù)。
mpu_dmp_init，是MPU6050DMP初始化函數(shù)，該函數(shù)代碼如下：
//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）;//設(shè)置FIFO
if（res）return2;
res=mpu_set_sample_rate（DEFAULT_MPU_HZ）;//設(shè)置采樣率
if（res）return3;
res=dmp_load_motion_driver_firmware（）;//加載dmp固件
if（res）return4;
res=dmp_set_orientation（inv_orientation_matrix_to_scalar（gyro_orientation））;
//設(shè)置陀螺儀方向
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）;
//設(shè)置dmp功能
if（res）return6;
res=dmp_set_fifo_rate（DEFAULT_MPU_HZ）;//設(shè)置DMP輸出速率（最大200Hz）
if（res）return7;
res=run_self_test（）;//自檢
if（res）return8;
res=mpu_set_dmp_state（1）;//使能DMP
if（res）return9;
}
return0;
}
此函數(shù)首先通過IIC_Init（需外部提供）初始化與MPU6050連接的IIC接口，然后調(diào)用mpu_init函數(shù)，初始化MPU6050，之后就是設(shè)置DMP所用傳感器、FIFO、采樣率和加載固件等一系列操作，在所有操作都正常之后，最后通過mpu_set_dmp_state（1）使能DMP功能，在使能成功以后，我們便可以通過mpu_dmp_get_data來讀取姿態(tài)解算后的數(shù)據(jù)了。
mpu_dmp_get_data函數(shù)代碼如下：
//得到dmp處理后的數(shù)據(jù)（注意，本函數(shù)需要比較多堆棧，局部變量有點(diǎn)多）
//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格式轉(zhuǎn)換為浮點(diǎn)數(shù)
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;
}
此函數(shù)用于得到DMP姿態(tài)解算后的俯仰角、橫滾角和航向角。不過本函數(shù)局部變量有點(diǎn)多，大家在使用的時候，如果死機(jī)，那么請設(shè)置堆棧大一點(diǎn)（在startup_stm32f10x_hd.s里面設(shè)置，默認(rèn)是400）。這里就用到了我們前面介紹的四元數(shù)轉(zhuǎn)歐拉角公式，將dmp_read_fifo函數(shù)讀到的q30格式四元數(shù)轉(zhuǎn)換成歐拉角。