MPU-6050簡介

　　MPU-6000（6050）為全球首例整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時間軸之差的問題，減少了大量的封裝空間。

　　感測范圍

　　MPU-6000（6050）的角速度全格感測范圍為±250、±500、±1000與±2000°/sec （dps），可準(zhǔn)確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g。產(chǎn)品傳輸可透過最高至400kHz的IIC或最高達20MHz的SPI（MPU-6050沒有SPI）。MPU-6000可在不同電壓下工作，VDD供電電壓介為2.5V±5%、3.0V±5%或3.3V±5%，邏輯接口VDDIO供電為1.8V± 5%（MPU6000僅用VDD）。MPU-6000的包裝尺寸4x4x0.9mm（QFN），在業(yè)界是革命性的尺寸。其他的特征包含內(nèi)建的溫度感測器、包含在運作環(huán)境中僅有±1%變動的振蕩器。

MPU-6050百科

　　MPU-6000（6050）為全球首例整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時間軸之差的問題，減少了大量的封裝空間。

　　感測范圍

　　MPU-6000（6050）的角速度全格感測范圍為±250、±500、±1000與±2000°/sec （dps），可準(zhǔn)確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g。產(chǎn)品傳輸可透過最高至400kHz的IIC或最高達20MHz的SPI（MPU-6050沒有SPI）。MPU-6000可在不同電壓下工作，VDD供電電壓介為2.5V±5%、3.0V±5%或3.3V±5%，邏輯接口VDDIO供電為1.8V± 5%（MPU6000僅用VDD）。MPU-6000的包裝尺寸4x4x0.9mm（QFN），在業(yè)界是革命性的尺寸。其他的特征包含內(nèi)建的溫度感測器、包含在運作環(huán)境中僅有±1%變動的振蕩器。

　　應(yīng)用

　　運動感測游戲 現(xiàn)實增強 電子穩(wěn)像 （EIS： Electronic Image Stabilization） 光學(xué)穩(wěn)像（OIS： Optical Image Stabilization） 行人導(dǎo)航器 “零觸控”手勢用戶接口 姿勢快捷方式 認證

　　市場

　　智能型手機 平板裝置設(shè)備 手持型游戲產(chǎn)品 游戲機 3D遙控器 可攜式導(dǎo)航設(shè)備

　　特征

　　以數(shù)字輸出6軸或9軸的旋轉(zhuǎn)矩陣、四元數(shù)（quaternion）、歐拉角格式（Euler Angle forma）的融合演算數(shù)據(jù)。 具有131 LSBs/°/sec 敏感度與全格感測范圍為±250、±500、±1000與±2000°/sec 的3軸角速度感測器（陀螺儀）。 可程式控制，且程式控制范圍為±2g、±4g、±8g和±16g的3軸加速器。 移除加速器與陀螺儀軸間敏感度，降低設(shè)定給予的影響與感測器的飄移。 數(shù)字運動處理（DMP： Digital Motion Processing）引擎可減少復(fù)雜的融合演算數(shù)據(jù)、感測器同步化、姿勢感應(yīng)等的負荷。 運動處理數(shù)據(jù)庫支持Android、Linux與Windows 內(nèi)建之運作時間偏差與磁力感測器校正演算技術(shù)，免除了客戶須另外進行校正的需求。 以數(shù)位輸出的溫度傳感器 以數(shù)位輸入的同步引腳（Sync pin）支援視頻電子影相穩(wěn)定技術(shù)與GPS 可程式控制的中斷（interrupt）支援姿勢識別、搖攝、畫面放大縮小、滾動、快速下降中斷、high-G中斷、零動作感應(yīng)、觸擊感應(yīng)、搖動感應(yīng)功能。 VDD供電電壓為2.5V±5%、3.0V±5%、3.3V±5%；VDDIO為1.8V± 5% 陀螺儀運作電流：5mA，陀螺儀待命電流：5µA；加速器運作電流：350µA，加速器省電模式電流： 20µA@10Hz 高達400kHz快速模式的I2C，或最高至20MHz的SPI串行主機接口（serial host interface） 內(nèi)建頻率產(chǎn)生器在所有溫度范圍（full temperature range）僅有±1%頻率變化。 使用者親自測試 10，000 g 碰撞容忍度 為可攜式產(chǎn)品量身訂作的最小最薄包裝 （4x4x0.9mm QFN） 符合RoHS及環(huán)境標(biāo)準(zhǔn)

　　MPU6050的數(shù)據(jù)獲取、分析與處理

　　MPU6050是一種非常流行的空間運動傳感器芯片，可以獲取器件當(dāng)前的三個加速度分量和三個旋轉(zhuǎn)角速度。由于其體積小巧，功能強大，精度較高，不僅被廣泛應(yīng)用于工業(yè)，同時也是航模愛好者的神器，被安裝在各類飛行器上馳騁藍天。

　　隨著Arduino開發(fā)板的普及，許多朋友希望能夠自己制作基于MPU6050的控制系統(tǒng)，但由于缺乏專業(yè)知識而難以上手。此外，MPU6050的數(shù)據(jù)是有較大噪音的，若不進行濾波會對整個控制系統(tǒng)的精準(zhǔn)確帶來嚴重影響。

　　MPU6050芯片內(nèi)自帶了一個數(shù)據(jù)處理子模塊DMP，已經(jīng)內(nèi)置了濾波算法，在許多應(yīng)用中使用DMP輸出的數(shù)據(jù)已經(jīng)能夠很好的滿足要求。關(guān)于如何獲取DMP的輸出數(shù)據(jù)，我將在以后的文章中介紹。本文將直接面對原始測量數(shù)據(jù)，從連線、芯片通信開始一步一步教你如何利用Arduino獲取MPU6050的數(shù)據(jù)并進行卡爾曼濾波，最終獲得穩(wěn)定的系統(tǒng)運動狀態(tài)。

　　一、Arduino與MPU-6050的通信

　　為避免糾纏于電路細節(jié)，我們直接使用集成的MPU6050模塊。MPU6050的數(shù)據(jù)接口用的是I2C總線協(xié)議，因此我們需要Wire程序庫的幫助來實現(xiàn)Arduino與MPU6050之間的通信。請先確認你的Arduino編程環(huán)境中已安裝Wire庫。

　　Wire庫的官方文檔中指出：在UNO板子上，SDA接口對應(yīng)的是A4引腳，SCL對應(yīng)的是A5引腳。MPU6050需要5V的電源，可由UNO板直接供電。按照下圖連線。

　　（紫色線是中斷線，這里用不到，可以不接）

　　MPU6050的數(shù)據(jù)寫入和讀出均通過其芯片內(nèi)部的寄存器實現(xiàn)，這些寄存器的地址都是1個字節(jié)，也就是8位的尋址空間，其寄存器的詳細列表說明書請點擊下載：https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf

　　1.1 將數(shù)據(jù)寫入MPU-6050

　　在每次向器件寫入數(shù)據(jù)前要先打開Wire的傳輸模式，并指定器件的總線地址，MPU6050的總線地址是0x68（AD0引腳為高電平時地址為0x69）。然后寫入一個字節(jié)的寄存器起始地址，再寫入任意長度的數(shù)據(jù)。這些數(shù)據(jù)將被連續(xù)地寫入到指定的起始地址中，超過當(dāng)前寄存器長度的將寫入到后面地址的寄存器中。寫入完成后關(guān)閉Wire的傳輸模式。下面的示例代碼是向MPU6050的0x6B寄存器寫入一個字節(jié)0。

　　Wire.beginTransmission（0x68）; //開啟MPU6050的傳輸

　　Wire.write（0x6B）; //指定寄存器地址

　　Wire.write（0）; //寫入一個字節(jié)的數(shù)據(jù)

　　Wire.endTransmission（true）; //結(jié)束傳輸，true表示釋放總線

　　1.2 從MPU-6050讀出數(shù)據(jù)

　　讀出和寫入一樣，要先打開Wire的傳輸模式，然后寫一個字節(jié)的寄存器起始地址。接下來將指定地址的數(shù)據(jù)讀到Wire庫的緩存中，并關(guān)閉傳輸模式。最后從緩存中讀取數(shù)據(jù)。下面的示例代碼是從MPU6050的0x3B寄存器開始讀取2個字節(jié)的數(shù)據(jù)：

　　Wire.beginTransmission（0x68）; //開啟MPU6050的傳輸

　　Wire.write（0x3B）; //指定寄存器地址

　　Wire.requestFrom（0x68， 2， true）; //將輸據(jù)讀出到緩存

　　Wire.endTransmission（true）; //關(guān)閉傳輸模式

　　int val = Wire.read（） 《《 8 | Wire.read（）; //兩個字節(jié)組成一個16位整數(shù)

　　1.3 具體實現(xiàn)

　　通常應(yīng)當(dāng)在setup函數(shù)中對Wire庫進行初始化：

　　Wire.begin（）;

　　在對MPU6050進行各項操作前，必須啟動該器件，向它的0x6B寫入一個字節(jié)0即可啟動。通常也是在setup函數(shù)完成，代碼見1.1節(jié)。

　　二、 MPU6050的數(shù)據(jù)格式

　　我們感興趣的數(shù)據(jù)位于0x3B到0x48這14個字節(jié)的寄存器中。這些數(shù)據(jù)會被動態(tài)更新，更新頻率最高可達1000HZ。下面列出相關(guān)寄存器的地址，數(shù)據(jù)的名稱。注意，每個數(shù)據(jù)都是2個字節(jié)。

　　0x3B，加速度計的X軸分量ACC_X

　　0x3D，加速度計的Y軸分量ACC_Y

　　0x3F，加速度計的Z軸分量ACC_Z

　　0x41，當(dāng)前溫度TEMP

　　0x43，繞X軸旋轉(zhuǎn)的角速度GYR_X

　　0x45，繞Y軸旋轉(zhuǎn)的角速度GYR_Y

　　0x47，繞Z軸旋轉(zhuǎn)的角速度GYR_Z

　　MPU6050芯片的座標(biāo)系是這樣定義的：令芯片表面朝向自己，將其表面文字轉(zhuǎn)至正確角度，此時，以芯片內(nèi)部中心為原點，水平向右的為X軸，豎直向上的為Y軸，指向自己的為Z軸。見下圖：

　　我們只關(guān)心加速度計和角速度計數(shù)據(jù)的含義，下面分別介紹。

　　2.1 加速度計

　　加速度計的三軸分量ACC_X、ACC_Y和ACC_Z均為16位有符號整數(shù)，分別表示器件在三個軸向上的加速度，取負值時加速度沿座標(biāo)軸負向，取正值時沿正向。

　　三個加速度分量均以重力加速度g的倍數(shù)為單位，能夠表示的加速度范圍，即倍率可以統(tǒng)一設(shè)定，有4個可選倍率：2g、4g、8g、16g。以ACC_X為例，若倍率設(shè)定為2g（默認），則意味著ACC_X取最小值-32768時，當(dāng)前加速度為沿X軸正方向2倍的重力加速度；若設(shè)定為4g，取-32768時表示沿X軸正方向4倍的重力加速度，以此類推。顯然，倍率越低精度越好，倍率越高表示的范圍越大，這要根據(jù)具體的應(yīng)用來設(shè)定。

　　我們用f表示倍率，f=0為2g，f=3為16g，設(shè)定加速度倍率的代碼如下：

　　Wire.beginTransmission（0x68）; //開啟MPU-6050的傳輸

　　Wire.write（0x1C）; //加速度倍率寄存器的地址

　　Wire.requestFrom（0x68， 1， true）; //先讀出原配置

　　unsigned char acc_conf = Wire.read（）;

　　acc_conf = （（acc_conf & 0xE7） | （f 《《 3））;

　　Wire.write（acc_conf）;

　　Wire.endTransmission（true）; //結(jié)束傳輸，true表示釋放總線

　　再以ACC_X為例，若當(dāng)前設(shè)定的加速度倍率為4g，那么將ACC_X讀數(shù)換算為加速度的公式為：，g可取當(dāng)?shù)刂亓铀俣取?/p>

　　2.2 角速度計

　　繞X、Y和Z三個座標(biāo)軸旋轉(zhuǎn)的角速度分量GYR_X、GYR_Y和GYR_Z均為16位有符號整數(shù)。從原點向旋轉(zhuǎn)軸方向看去，取正值時為順時針旋轉(zhuǎn)，取負值時為逆時針旋轉(zhuǎn)。

　　三個角速度分量均以“度/秒”為單位，能夠表示的角速度范圍，即倍率可統(tǒng)一設(shè)定，有4個可選倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X為例，若倍率設(shè)定為250度/秒，則意味著GYR取正最大值32768時，當(dāng)前角速度為順時針250度/秒；若設(shè)定為500度/秒，取32768時表示當(dāng)前角速度為順時針500度/秒。顯然，倍率越低精度越好，倍率越高表示的范圍越大。

　　我們用f表示倍率，f=0為250度/秒，f=3為2000度/秒，除角速度倍率寄存器的地址為0x1B之外，設(shè)定加速度倍率的代碼與2.1節(jié)代碼一致。

　　以GYR_X為例，若當(dāng)前設(shè)定的角速度倍率為1000度/秒，那么將GRY_X讀數(shù)換算為角速度（順時針）的公式為：。

　　三、運動數(shù)據(jù)

　　在讀取加速度計和角速度計的數(shù)據(jù)并換算為物理值后，根據(jù)不同的應(yīng)用，數(shù)據(jù)有不同的解譯方式。本章將以飛行器運動模型為例，根據(jù)加速度和角速度來算出當(dāng)前的飛行姿態(tài)。

　　3.1 加速度計模型

　　我們可以把加速度計想象為一個正立方體盒子里放著一個球，這個球被彈簧固定在立方體的中心。當(dāng)盒子運動時，根據(jù)假想球的位置即可算出當(dāng)前加速度的值。想象如果在太空中，盒子沒有任何受力時，假想球?qū)⑻幱谡行牡奈恢茫齻€軸的加速度均為0。見下圖：

　　如果我們給盒子施加一個水平向左的力，那么顯然盒子就會有一個向左的加速度，此時盒內(nèi)的假想球會因為慣性作用貼向盒內(nèi)的右側(cè)面。如下圖所示：

　　為了保證數(shù)據(jù)的物理意義，MPU6050的加速度計是以假想球在三軸上座標(biāo)值的相反數(shù)作為三個軸的加速度值。當(dāng)假想球的位置偏向一個軸的正向時，該軸的加速度讀數(shù)為負值，當(dāng)假想球的位置偏向一個軸的負向時，該軸的加速度讀數(shù)為正值。

　　根據(jù)以上分析，當(dāng)我們把MPU6050芯片水平放于地方，芯片表面朝向天空，此時由于受到地球重力的作用， 假想球的位置偏向Z軸的負向，因此Z軸的加速度讀數(shù)應(yīng)為正，且在理想情況下應(yīng)為g。注意，此加速度的物理意義并不是重力加速度，而是自身運動的加速度，可以這樣理解：正因為其自身運動的加速度與重力加速度大小相等方向相反，芯片才能保持靜止。

　　3.2 Roll-pitch-yaw模型與姿態(tài)計算

　　表示飛行器當(dāng)前飛行姿態(tài)的一個通用模型就是建立下圖所示坐標(biāo)系，并用Roll表示繞X軸的旋轉(zhuǎn)，Pitch表示繞Y軸的旋轉(zhuǎn)，Yaw表示繞Z軸的旋轉(zhuǎn)。

　　由于MPU6050可以獲取三個軸向上的加速度，而地球重力則是長期存在且永遠豎直向下，因此我們可以根據(jù)重力加速度相對于芯片的指向為參考算得當(dāng)前姿態(tài)。

　　為方便起見，我們讓芯片正面朝下固定在上圖飛機上，且座標(biāo)系與飛機的坐標(biāo)系完全重合，以三個軸向上的加速度為分量，可構(gòu)成加速度向量。假設(shè)當(dāng)前芯片處于勻速直線運動狀態(tài)，那么應(yīng)垂直于地面上向，即指向Z軸負方向，模長為（與重力加速度大小相等，方向相反，見3.1節(jié)）。若芯片（座標(biāo)系）發(fā)生旋轉(zhuǎn)，由于加速度向量仍然豎直向上，所以Z軸負方向?qū)⒉辉倥c重合。見下圖。

　　為了方便表示，上圖坐標(biāo)系的Z軸正方向（機腹以及芯片正面）向下，X軸正方向（飛機前進方向）向右。此時芯片的Roll角（黃色）為加速度向量與其在XZ平面上投影的夾角，Pitch角（綠色）與其在YZ平面上投影的夾角。求兩個向量的夾角可用點乘公式： ，簡單推導(dǎo)可得：

　　，以及

　　注意，因為arccos函數(shù)只能返回正值角度，因此還需要根據(jù)不同情況來取角度的正負值。當(dāng)y值為正時，Roll角要取負值，當(dāng)x軸為負時，Pitch角要取負值。

　　3.4 Yaw角的問題

　　因為沒有參考量，所以無法求出當(dāng)前的Yaw角的絕對角度，只能得到Y(jié)aw的變化量，也就是角速度GYR_Z。當(dāng)然，我們可以通過對GYR_Z積分的方法來推算當(dāng)前Yaw角（以初始值為準(zhǔn)），但由于測量精度的問題，推算值會發(fā)生漂移，一段時間后就完全失去意義了。然而在大多數(shù)應(yīng)用中，比如無人機，只需要獲得GRY_Z就可以了。

　　如果必須要獲得絕對的Yaw角，那么應(yīng)當(dāng)選用MPU9250這款九軸運動跟蹤芯片，它可以提供額外的三軸羅盤數(shù)據(jù)，這樣我們就可以根據(jù)地球磁場方向來計算Yaw角了，具體方法此處不再贅述。

　　四、數(shù)據(jù)處理與實現(xiàn)

　　MPU6050芯片提供的數(shù)據(jù)夾雜有較嚴重的噪音，在芯片處理靜止?fàn)顟B(tài)時數(shù)據(jù)擺動都可能超過2%。除了噪音，各項數(shù)據(jù)還會有偏移的現(xiàn)象，也就是說數(shù)據(jù)并不是圍繞靜止工作點擺動，因此要先對數(shù)據(jù)偏移進行校準(zhǔn) ，再通過濾波算法消除噪音。

　　4.1 校準(zhǔn)

　　校準(zhǔn)是比較簡單的工作，我們只需要找出擺動的數(shù)據(jù)圍繞的中心點即可。我們以GRY_X為例，在芯片處理靜止?fàn)顟B(tài)時，這個讀數(shù)理論上講應(yīng)當(dāng)為0，但它往往會存在偏移量，比如我們以10ms的間隔讀取了10個值如下：

　　-158.4， -172.9， -134.2， -155.1， -131.2， -146.8， -173.1， -188.6， -142.7， -179.5

　　這10個值的均值，也就是這個讀數(shù)的偏移量為-158.25。在獲取偏移量后，每次的讀數(shù)都減去偏移量就可以得到校準(zhǔn)后的讀數(shù)了。當(dāng)然這個偏移量只是估計值，比較準(zhǔn)確的偏移量要對大量的數(shù)據(jù)進行統(tǒng)計才能獲知，數(shù)據(jù)量越大越準(zhǔn)，但統(tǒng)計的時間也就越慢。一般校準(zhǔn)可以在每次啟動系統(tǒng)時進行，那么你應(yīng)當(dāng)在準(zhǔn)確度和啟動時間之間做一個權(quán)衡。

　　三個角速度讀數(shù)GYR_X、GYR_Y和GYR_Z均可通過統(tǒng)計求平均的方法來獲得，但三個加速度分量就不能這樣簡單的完成了，因為芯片靜止時的加速度并不為0。

　　加速度值的偏移來自兩個方面，一是由于芯片的測量精度，導(dǎo)至它測得的加速度向量并不垂直于大地；二是芯片在整個系統(tǒng)（如無人機）上安裝的精度是有限的，系統(tǒng)與芯片的座標(biāo)系很難達到完美重合。前者我們稱為讀數(shù)偏移，后者我們稱為角度偏移。因為讀數(shù)和角度之間是非線性關(guān)系，所以要想以高精度進行校準(zhǔn)必須先單獨校準(zhǔn)讀數(shù)偏移，再把芯片固定在系統(tǒng)中后校準(zhǔn)角度偏移。然而，由于校準(zhǔn)角度偏移需要專業(yè)設(shè)備，且對于一般應(yīng)用來說，兩步校準(zhǔn)帶來的精度提升并不大，因此通常只進行讀數(shù)校準(zhǔn)即可。下面介紹讀數(shù)校準(zhǔn)的方法。我們還3.2節(jié)的飛機為例，分以下幾個步驟：

　　首先要確定飛機的坐標(biāo)系，對于多軸飛行器來說這非常重要。如果坐標(biāo)系原點的位置或坐標(biāo)軸的方向存在較大偏差，將會給后面的飛控造成不良影響。

　　在確定了飛機的坐標(biāo)系后，為了盡量避免讀數(shù)偏移帶來的影響，首先將MPU6050牢牢地固定在飛機上，并使二者座標(biāo)系盡可能的重合。當(dāng)然把Z軸反過來裝也是可以的，就是需要重新推算一套角度換算公式。

　　將飛機置于水平、堅固的平面上，并充分預(yù)熱。對于多軸無人機而言，空中懸停時的XY平面應(yīng)當(dāng)平行于校準(zhǔn)時的XY平面。此時，我們認為芯片的加速度方向應(yīng)當(dāng)與Z軸負方向重合，且加速度向量的模長為g，因此ACC_X和ACC_Y的理論值應(yīng)為0，ACC_Z的理論值應(yīng)為-16384（假設(shè)我們設(shè)定2g的倍率，1g的加速度的讀數(shù)應(yīng)為最大值-32768的一半）。

　　由于ACC_X和ACC_Y的理論值應(yīng)為0，與角速度量的校準(zhǔn)類似，這兩個讀數(shù)偏移量可用統(tǒng)計均值的方式校準(zhǔn)。ACC_Z則需要多一步處理，即在統(tǒng)計偏移量的過程中，每次讀數(shù)都要加上16384，再進行統(tǒng)計均值校準(zhǔn)。

　　4.2 卡爾曼濾波

　　對于夾雜了大量噪音的數(shù)據(jù)，卡爾曼濾波器的效果無疑是最好的。如果不想考慮算法細節(jié)，可以直接使用Arduino的Klaman Filter庫完成。在我們的模型中，一個卡爾曼濾波器接受一個軸上的角度值、角速度值以及時間增量，估計出一個消除噪音的角度值。跟據(jù)當(dāng)前的角度值和上一輪估計的角度值，以及這兩輪估計的間隔時間，我們還可以反推出消除噪音的角速度。

　　實現(xiàn)代碼見4.3節(jié)。下面介紹卡爾曼濾波算法細節(jié)，不感興趣的可跳過。

　　（想看的人多了再寫）

　　4.3 實現(xiàn)代碼

　　以下代碼在Arduino軟件1.65版本中編譯、燒寫以及測試通過。

　　// 本代碼版權(quán)歸Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0發(fā)布

　　// http://www.gnu.org/licenses/gpl-3.0.en.html

　　// 相關(guān)文檔參見作者于知乎專欄發(fā)表的原創(chuàng)文章：

　　// http://zhuanlan.zhihu.com/devymex/20082486

　　//連線方法

　　//MPU-UNO

　　//VCC-VCC

　　//GND-GND

　　//SCL-A5

　　//SDA-A4

　　//INT-2 （Optional）

　　#include 《Kalman.h》

　　#include 《Wire.h》

　　#include 《Math.h》

　　float fRad2Deg = 57.295779513f; //將弧度轉(zhuǎn)為角度的乘數(shù)

　　const int MPU = 0x68; //MPU-6050的I2C地址

　　const int nValCnt = 7; //一次讀取寄存器的數(shù)量

　　const int nCalibTimes = 1000; //校準(zhǔn)時讀數(shù)的次數(shù)

　　int calibData［nValCnt］; //校準(zhǔn)數(shù)據(jù)

　　unsigned long nLastTime = 0; //上一次讀數(shù)的時間

　　float fLastRoll = 0.0f; //上一次濾波得到的Roll角

　　float fLastPitch = 0.0f; //上一次濾波得到的Pitch角

　　Kalman kalmanRoll; //Roll角濾波器

　　Kalman kalmanPitch; //Pitch角濾波器

　　void setup（） {

　　Serial.begin（9600）; //初始化串口，指定波特率

　　Wire.begin（）; //初始化Wire庫

　　WriteMPUReg（0x6B， 0）; //啟動MPU6050設(shè)備

　　Calibration（）; //執(zhí)行校準(zhǔn)

　　nLastTime = micros（）; //記錄當(dāng)前時間

　　}

　　void loop（） {

　　int readouts［nValCnt］;

　　ReadAccGyr（readouts）; //讀出測量值

　　float realVals［7］;

　　Rectify（readouts， realVals）; //根據(jù)校準(zhǔn)的偏移量進行糾正

　　//計算加速度向量的模長，均以g為單位

　　float fNorm = sqrt（realVals［0］ * realVals［0］ + realVals［1］ * realVals［1］ + realVals［2］ * realVals［2］）;

　　float fRoll = GetRoll（realVals， fNorm）; //計算Roll角

　　if （realVals［1］ 》 0） {

　　fRoll = -fRoll;

　　}

　　float fPitch = GetPitch（realVals， fNorm）; //計算Pitch角

　　if （realVals［0］ 《 0） {

　　fPitch = -fPitch;

　　}

　　//計算兩次測量的時間間隔dt，以秒為單位

　　unsigned long nCurTime = micros（）;

　　float dt = （double）（nCurTime - nLastTime） / 1000000.0;

　　//對Roll角和Pitch角進行卡爾曼濾波

　　float fNewRoll = kalmanRoll.getAngle（fRoll， realVals［4］， dt）;

　　float fNewPitch = kalmanPitch.getAngle（fPitch， realVals［5］， dt）;

　　//跟據(jù)濾波值計算角度速

　　float fRollRate = （fNewRoll - fLastRoll） / dt;

　　float fPitchRate = （fNewPitch - fLastPitch） / dt;

　　//更新Roll角和Pitch角

　　fLastRoll = fNewRoll;

　　fLastPitch = fNewPitch;

　　//更新本次測的時間

　　nLastTime = nCurTime;

　　//向串口打印輸出Roll角和Pitch角，運行時在Arduino的串口監(jiān)視器中查看

　　Serial.print（“Roll：”）;

　　Serial.print（fNewRoll）; Serial.print（‘（’）;

　　Serial.print（fRollRate）; Serial.print（“），\tPitch：”）;

　　Serial.print（fNewPitch）; Serial.print（‘（’）;

　　Serial.print（fPitchRate）; Serial.print（“）\n”）;

　　delay（10）;

　　}

　　//向MPU6050寫入一個字節(jié)的數(shù)據(jù)

　　//指定寄存器地址與一個字節(jié)的值

　　void WriteMPUReg（int nReg， unsigned char nVal） {

　　Wire.beginTransmission（MPU）;

　　Wire.write（nReg）;

　　Wire.write（nVal）;

　　Wire.endTransmission（true）;

　　}

　　//從MPU6050讀出一個字節(jié)的數(shù)據(jù)

　　//指定寄存器地址，返回讀出的值

　　unsigned char ReadMPUReg（int nReg） {

　　Wire.beginTransmission（MPU）;

　　Wire.write（nReg）;

　　Wire.requestFrom（MPU， 1， true）;

　　Wire.endTransmission（true）;

　　return Wire.read（）;

　　}

　　//從MPU6050讀出加速度計三個分量、溫度和三個角速度計

　　//保存在指定的數(shù)組中

　　void ReadAccGyr（int *pVals） {

　　Wire.beginTransmission（MPU）;

　　Wire.write（0x3B）;

　　Wire.requestFrom（MPU， nValCnt * 2， true）;

　　Wire.endTransmission（true）;

　　for （long i = 0; i 《 nValCnt; ++i） {

　　pVals［i］ = Wire.read（） 《《 8 | Wire.read（）;

　　}

　　}

　　//對大量讀數(shù)進行統(tǒng)計，校準(zhǔn)平均偏移量

　　void Calibration（）

　　{

　　float valSums［7］ = {0.0f， 0.0f， 0.0f， 0.0f， 0.0f， 0.0f， 0.0};

　　//先求和

　　for （int i = 0; i 《 nCalibTimes; ++i） {

　　int mpuVals［nValCnt］;

　　ReadAccGyr（mpuVals）;

　　for （int j = 0; j 《 nValCnt; ++j） {

　　valSums［j］ += mpuVals［j］;

　　}

　　}

　　//再求平均

　　for （int i = 0; i 《 nValCnt; ++i） {

　　calibData［i］ = int（valSums［i］ / nCalibTimes）;

　　}

　　calibData［2］ += 16384; //設(shè)芯片Z軸豎直向下，設(shè)定靜態(tài)工作點。

　　}

　　//算得Roll角。算法見文檔。

　　float GetRoll（float *pRealVals， float fNorm） {

　　float fNormXZ = sqrt（pRealVals［0］ * pRealVals［0］ + pRealVals［2］ * pRealVals［2］）;

　　float fCos = fNormXZ / fNorm;

　　return acos（fCos） * fRad2Deg;

　　}

　　//算得Pitch角。算法見文檔。

　　float GetPitch（float *pRealVals， float fNorm） {

　　float fNormYZ = sqrt（pRealVals［1］ * pRealVals［1］ + pRealVals［2］ * pRealVals［2］）;

　　float fCos = fNormYZ / fNorm;

　　return acos（fCos） * fRad2Deg;

　　}

　　//對讀數(shù)進行糾正，消除偏移，并轉(zhuǎn)換為物理量。公式見文檔。

　　void Rectify（int *pReadout， float *pRealVals） {

　　for （int i = 0; i 《 3; ++i） {

　　pRealVals［i］ = （float）（pReadout［i］ - calibData［i］） / 16384.0f;

　　}

　　pRealVals［3］ = pReadout［3］ / 340.0f + 36.53;

　　for （int i = 4; i 《 7; ++i） {

　　pRealVals［i］ = （float）（pReadout［i］ - calibData［i］） / 131.0f;

　　}

　　}