MPU-6050簡介

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

　　感測范圍

　　MPU-6000（6050）的角速度全格感測范圍為±250、±500、±1000與±2000°/sec （dps），可準確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g。產品傳輸可透過最高至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），在業界是革命性的尺寸。其他的特征包含內建的溫度感測器、包含在運作環境中僅有±1%變動的振蕩器。

MPU-6050百科

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

　　感測范圍

　　MPU-6000（6050）的角速度全格感測范圍為±250、±500、±1000與±2000°/sec （dps），可準確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g。產品傳輸可透過最高至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），在業界是革命性的尺寸。其他的特征包含內建的溫度感測器、包含在運作環境中僅有±1%變動的振蕩器。

　　應用

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

　　市場

　　智能型手機 平板裝置設備 手持型游戲產品 游戲機 3D遙控器 可攜式導航設備

　　特征

　　以數字輸出6軸或9軸的旋轉矩陣、四元數（quaternion）、歐拉角格式（Euler Angle forma）的融合演算數據。 具有131 LSBs/°/sec 敏感度與全格感測范圍為±250、±500、±1000與±2000°/sec 的3軸角速度感測器（陀螺儀）。 可程式控制，且程式控制范圍為±2g、±4g、±8g和±16g的3軸加速器。 移除加速器與陀螺儀軸間敏感度，降低設定給予的影響與感測器的飄移。 數字運動處理（DMP： Digital Motion Processing）引擎可減少復雜的融合演算數據、感測器同步化、姿勢感應等的負荷。 運動處理數據庫支持Android、Linux與Windows 內建之運作時間偏差與磁力感測器校正演算技術，免除了客戶須另外進行校正的需求。 以數位輸出的溫度傳感器 以數位輸入的同步引腳（Sync pin）支援視頻電子影相穩定技術與GPS 可程式控制的中斷（interrupt）支援姿勢識別、搖攝、畫面放大縮小、滾動、快速下降中斷、high-G中斷、零動作感應、觸擊感應、搖動感應功能。 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） 內建頻率產生器在所有溫度范圍（full temperature range）僅有±1%頻率變化。 使用者親自測試 10，000 g 碰撞容忍度 為可攜式產品量身訂作的最小最薄包裝 （4x4x0.9mm QFN） 符合RoHS及環境標準

　　MPU6050的數據獲取、分析與處理

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

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

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

　　一、Arduino與MPU-6050的通信

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

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

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

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

　　1.1 將數據寫入MPU-6050

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

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

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

　　Wire.write（0）; //寫入一個字節的數據

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

　　1.2 從MPU-6050讀出數據

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

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

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

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

　　Wire.endTransmission（true）; //關閉傳輸模式

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

　　1.3 具體實現

　　通常應當在setup函數中對Wire庫進行初始化：

　　Wire.begin（）;

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

　　二、 MPU6050的數據格式

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

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

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

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

　　0x41，當前溫度TEMP

　　0x43，繞X軸旋轉的角速度GYR_X

　　0x45，繞Y軸旋轉的角速度GYR_Y

　　0x47，繞Z軸旋轉的角速度GYR_Z

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

　　我們只關心加速度計和角速度計數據的含義，下面分別介紹。

　　2.1 加速度計

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

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

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

　　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）; //結束傳輸，true表示釋放總線

　　再以ACC_X為例，若當前設定的加速度倍率為4g，那么將ACC_X讀數換算為加速度的公式為：，g可取當地重力加速度。

　　2.2 角速度計

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

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

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

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

　　三、運動數據

　　在讀取加速度計和角速度計的數據并換算為物理值后，根據不同的應用，數據有不同的解譯方式。本章將以飛行器運動模型為例，根據加速度和角速度來算出當前的飛行姿態。

　　3.1 加速度計模型

　　我們可以把加速度計想象為一個正立方體盒子里放著一個球，這個球被彈簧固定在立方體的中心。當盒子運動時，根據假想球的位置即可算出當前加速度的值。想象如果在太空中，盒子沒有任何受力時，假想球將處于正中心的位置，三個軸的加速度均為0。見下圖：

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

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

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

　　3.2 Roll-pitch-yaw模型與姿態計算

　　表示飛行器當前飛行姿態的一個通用模型就是建立下圖所示坐標系，并用Roll表示繞X軸的旋轉，Pitch表示繞Y軸的旋轉，Yaw表示繞Z軸的旋轉。

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

　　為方便起見，我們讓芯片正面朝下固定在上圖飛機上，且座標系與飛機的坐標系完全重合，以三個軸向上的加速度為分量，可構成加速度向量。假設當前芯片處于勻速直線運動狀態，那么應垂直于地面上向，即指向Z軸負方向，模長為（與重力加速度大小相等，方向相反，見3.1節）。若芯片（座標系）發生旋轉，由于加速度向量仍然豎直向上，所以Z軸負方向將不再與重合。見下圖。

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

　　，以及

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

　　3.4 Yaw角的問題

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

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

　　四、數據處理與實現

　　MPU6050芯片提供的數據夾雜有較嚴重的噪音，在芯片處理靜止狀態時數據擺動都可能超過2%。除了噪音，各項數據還會有偏移的現象，也就是說數據并不是圍繞靜止工作點擺動，因此要先對數據偏移進行校準 ，再通過濾波算法消除噪音。

　　4.1 校準

　　校準是比較簡單的工作，我們只需要找出擺動的數據圍繞的中心點即可。我們以GRY_X為例，在芯片處理靜止狀態時，這個讀數理論上講應當為0，但它往往會存在偏移量，比如我們以10ms的間隔讀取了10個值如下：

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

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

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

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

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

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

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

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

　　4.2 卡爾曼濾波

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

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

　　（想看的人多了再寫）

　　4.3 實現代碼

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

　　// 本代碼版權歸Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0發布

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

　　// 相關文檔參見作者于知乎專欄發表的原創文章：

　　// 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; //將弧度轉為角度的乘數

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

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

　　const int nCalibTimes = 1000; //校準時讀數的次數

　　int calibData［nValCnt］; //校準數據

　　unsigned long nLastTime = 0; //上一次讀數的時間

　　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設備

　　Calibration（）; //執行校準

　　nLastTime = micros（）; //記錄當前時間

　　}

　　void loop（） {

　　int readouts［nValCnt］;

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

　　float realVals［7］;

　　Rectify（readouts， realVals）; //根據校準的偏移量進行糾正

　　//計算加速度向量的模長，均以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）;

　　//跟據濾波值計算角度速

　　float fRollRate = （fNewRoll - fLastRoll） / dt;

　　float fPitchRate = （fNewPitch - fLastPitch） / dt;

　　//更新Roll角和Pitch角

　　fLastRoll = fNewRoll;

　　fLastPitch = fNewPitch;

　　//更新本次測的時間

　　nLastTime = nCurTime;

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

　　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寫入一個字節的數據

　　//指定寄存器地址與一個字節的值

　　void WriteMPUReg（int nReg， unsigned char nVal） {

　　Wire.beginTransmission（MPU）;

　　Wire.write（nReg）;

　　Wire.write（nVal）;

　　Wire.endTransmission（true）;

　　}

　　//從MPU6050讀出一個字節的數據

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

　　unsigned char ReadMPUReg（int nReg） {

　　Wire.beginTransmission（MPU）;

　　Wire.write（nReg）;

　　Wire.requestFrom（MPU， 1， true）;

　　Wire.endTransmission（true）;

　　return Wire.read（）;

　　}

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

　　//保存在指定的數組中

　　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（）;

　　}

　　}

　　//對大量讀數進行統計，校準平均偏移量

　　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; //設芯片Z軸豎直向下，設定靜態工作點。

　　}

　　//算得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;

　　}

　　//對讀數進行糾正，消除偏移，并轉換為物理量。公式見文檔。

　　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;

　　}

　　}