MPU6050陀螺儀與Processing和上位機飛控聯動實錄

簡而言之，MPU6050 = 三軸MEMS陀螺儀 + 三軸MEMS加速度計 + 可擴展數字運動處理器DMP ，它可進行姿態解算（Pitch、Yaw、Roll角），我們還可以外接Processing IDE，或外接匿名上位機(V7)，實時繪製系統的飛行姿態，下面講一下整個聯調過程以及遇到的坑。

 

圖0 單片機與上位機(V7)飛行姿態聯動

一，MPU6050簡介

MPU6050是InvenSense推出的集成6軸運動處理組件，即三軸MEMS（注1）陀螺儀傳感器和三軸MEMS加速度傳感器，相較於多組件方案，集成模塊可以免除各個組件時間軸之差的問題，還能大大減小封裝的空間。它含有一個副IIC接口，可用於連接外部磁力傳感器，利用自帶數字運動處理器（DMP，Digital Motion Processor的縮寫）硬件加速引擎，通過主IIC接口，可以嚮應用端輸出完整的9軸姿態融合演算數據。

注1：MEMS是微機電系統（Micro Electro Mechanical System）的英文縮寫。 它是指可批量製作的，集微型機構、微型傳感器、微型執行器以及信號處理和控制電路、通信和電源於一體的系統。 比較成熟的MEMS傳感器有三種：加速度計、壓力傳感器和陀螺儀。

有了DMP，我們可以使用InvenSense提供的運動處理資料庫，非常方便地實現姿態解算，降低了運動處理運算對操作系統的負荷，同時大大降低了開發難度 。

 

二，MUP6050特點

MPU6050 的特點有：

① 以數字形式輸出 6 軸或 9 軸（需外接磁傳感器）（注2）的旋轉矩陣、四元數(quaternion)、歐拉角格式(Euler Angle forma)的融合演算數據（需 DMP 支持）。

② 具有 131 LSBs/°/sec 敏感度與全格感測範圍爲±250、±500、±1000 與±2000°/sec 的 3 軸角速度感測器(陀螺儀)。

③ 集成可程序控制，範圍爲±2g、±4g、±8g 和±16g 的 3 軸加速度傳感器。

④ 移除加速器與陀螺儀軸間敏感度，降低設定給予的影響與感測器的飄移。

⑤ 自帶數字運動處理引擎可減少MCU複雜的融合演算數據、感測器同步化、姿勢感應等的負荷。

⑥ 內建運作時間偏差與磁力感測器校正演算技術，免除了客戶須另外進行校正的需求。

⑦ 自帶一個數字溫度傳感器。

⑧ 帶數字輸入同步引腳(Sync pin)支持視頻電子影相穩定技術與 GPS。

⑨ 可程序控制的中斷(interrupt)，支持姿勢識別、搖攝、畫面放大縮小、滾動、快速下降中斷、high-G 中斷、零動作感應、觸擊感應、搖動感應功能。

⑩ VDD 供電電壓爲 2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC 可低至 1.8V± 5%。

⑪ 陀螺儀工作電流：5mA。

⑫ 自帶 1024 字節 FIFO，有助於降低系統功率。

⑬ 400Khz 的 IIC 通信接口。

注2：三軸 = 3軸陀螺儀

六軸 = 3軸加速度計 + 3軸陀螺儀

九軸 = 3軸加速度計 + 3軸陀螺儀 + 3軸磁力計

 

三，原理說明

3.1.MEMS陀螺儀

MEMS陀螺儀與傳統的陀螺儀原理不同。 傳統陀螺儀是一個不停轉動的物體即轉子，由於慣性其旋轉軸的指向不會隨着承載它的支架旋轉而改變方向，如下圖所示，三軸即橫滾軸、俯仰軸和航向軸，通俗點說也就是X Y Z三軸，當三軸發生旋轉時，旋轉軸是不會隨之變化的。 顯然，將這樣一個不停旋轉的裝置用微機技術在硅片襯底上加工是非常難的事情。

圖1 傳統陀螺儀示意

爲此，MEMS陀螺儀基於陀螺儀的特性利用科里奧利力來實現。 科里奧利力即科氏力，它是對旋轉體系中進行直線運動的質點由於慣性相對於旋轉體系產生的直線運動的偏移的一種描述。

以MEMS陀螺儀常見的微機械音叉式陀螺結構爲例，陀螺儀中間有一個懸在空中的質量塊，它可以在兩個互相垂直的平面內振動。在一般狀態下，陀螺儀通過加震盪電壓迫使質量塊在其中一個平面做振動，如下圖藍色箭頭所示，也就是提供了科里奧利力中的徑向運動。在陀螺儀發生旋轉時，質量塊受到與它運動方向垂直的科里奧利力，產生了另一個平面的振動，如下圖黃色箭頭所示，根據振動產生的電容變化便可以測量科里奧利力的大小，又因爲科里奧利力正比於角速度，所以我們可以通過電容變化計算得到角速度。

 

圖2 科里奧利力示意

3.2.MEMS加速度計

技術成熟的MEMS加速度計分爲三種：壓電式、容感式和熱感式。

壓電式：在其內部有一個剛體支撐的質量塊，在運動的情況下質量塊會產生壓力，剛體產生應變，從而轉換爲電信號輸出。

容感式：它是標準的平行板電容器，加速度的變化帶來質量塊的移動從而改變電容兩級的間距以及面積，通過計算即可得到相應的加速度。

熱感式：內部無任何質量塊，它的中央有加熱體，周邊是溫度傳感器，裏面是密閉的氣腔。工作時在加熱體的作用下，氣體在內部形成一個熱氣團，熱氣團的比重和周圍的冷氣是有差異的，通過慣性熱氣團的移動形成的質量塊，熱場變化讓感應器感應到加速度值。

附錄A：

陀螺儀歷史發展和原因參考資源：https://zhuanlan.zhihu.com/p/610287151

 

四，模塊初始化工作

4.1.初始化 IIC 接口

MPU6050 採用 IIC 與開發板通信，所以我們需要先初始化與 MPU6050 連接的 SDA 和 SCL 數據線。

4.2.復位 MPU6050

這一步讓 MPU6050 內部所有寄存器恢復默認值，通過對電源管理寄存器 1（0x6B）的bit7 寫 1 實現。 復位後，電源管理寄存器 1 恢復默認值(0x40)，然後必須設置該寄存器爲0x00，以喚醒 MPU6050，進入正常工作狀態。

const uint8_t MPU6050_REGISTER_PWR_MGMT_1 = 0x6B;
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_1, 0x01);

4.3.設置角速度傳感器（陀螺儀）和加速度傳感器的滿量程範圍

這一步，我們設置兩個傳感器的滿量程範圍（FSR）（注3），分別通過陀螺儀配置寄存器（0X1B）和加速度傳感器配置寄存器（0X1C）設置。我們一般設置陀螺儀的滿量程範圍爲 ±2000dps（注4），加速度傳感器的滿量程範圍爲 ±2g。

注3：量程是度量工具的測量範圍，也就是儀器設備所能測量的物理量的最大值。

注4：dps是角速度單位，即degree per second的縮寫。

（1）陀螺儀配置

寄存器地址：0X1B（英文對應於Gyroscope Configuration）

寫入數據 ：0X00，選擇量程爲：±250dps，視情況而定，可以爲±250、±500、±1000、±2000（°/sec）

mpu.setGyroRange(MPU6050_RANGE_250_DEG);//Adafruit_MPU6050庫的寫法

（2）加速度計配置

寄存器地址：0X1C（英文對應於Accelerometer Configuration）

寫入數據 ：0X00，選擇量程爲：±2g（視情況而定）

mpu.setAccelerometerRange(MPU6050_RANGE_2_G);//Adafruit_MPU6050庫的寫法

4.4.設置其他參數

我們還需要配置的參數有：

• 關閉中斷
• 關閉 AUX IIC 接口
• 禁止 FIFO
• 設置陀螺儀採樣率
• 設置數字低通濾波器（DLPF）等。

我們不用中斷方式讀取數據，所以可以關閉中斷。也沒用到 AUX IIC 接口外接其他傳感器，所以也關閉這個接口。如下所示，分別通過中斷使能寄存器（0x38）和用戶控制寄存器（0x6A）控制。

const uint8_t MPU6050_REGISTER_INT_ENABLE = 0x38; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_INT_ENABLE, 0x01); 
const uint8_t MPU6050_REGISTER_USER_CTRL = 0x6A; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_USER_CTRL, 0x00);

MPU6050 可以使用 FIFO 存儲傳感器數據，不過我們也可以不用，所以關閉所有 FIFO 通道。如下所示，通過 FIFO 使能寄存器（0x23）控制，默認都是 0（即禁止 FIFO），所以用默認值就可以了。

const uint8_t MPU6050_REGISTER_FIFO_EN = 0x23; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_FIFO_EN, 0x00);

閱讀寄存器說明手冊可知，MPU6050的陀螺輸出頻率可達8kHz，加速度計爲1kHz，而且可以通過分頻來降低頻率。採樣頻率就是通過陀螺儀輸出頻率分頻得到的。陀螺儀採樣率通過採樣率分頻寄存器（0x19）控制。如果要得到200Hz的採樣率，那麼分頻值就是39。只要設置寄存器25的值爲39，就可以使得DMP以200Hz來更新寄存器中的數據，只要按時讀取寄存器就可以了（注5）。

const uint8_t MPU6050_REGISTER_SMPLRT_DIV = 0x19; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SMPLRT_DIV, 0x07);

注5：採樣頻率分頻解釋參考 https://zhuanlan.zhihu.com/p/21670600

數字低通濾波器(DLPF)則通過配置寄存器（0x1A）設置，一般設置 DLPF 爲帶寬的 1/2 即可。

const uint8_t MPU6050_REGISTER_CONFIG = 0x1A; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_CONFIG, 0x00);

 

4.5.配置系統時鐘源並使能角速度傳感器和加速度傳感器

可選的時鐘源有三種：內部8MHz晶振、基於陀螺儀的時鐘和外部時鐘源。 默認爲內部晶振，強烈建議選擇其中一個陀螺儀（或者外部時鐘源）作爲時鐘源，以提高穩定性。

系統時鐘源同樣是通過電源管理寄存器 1（0X6B）來設置，該寄存器的最低三位用於設置系統時鐘源選擇，默認值是 0（內部晶振），不過我們一般設置爲 1，即選擇 X 軸陀螺 PLL 作爲時鐘源，以獲得更高精度的時鐘。

寄存器地址：0x6B（英語對應於Power Management 1）

寫入數據 ：0x01，選擇陀螺儀的 X 軸作爲時鐘源

同時，使能角速度傳感器和加速度傳感器，這兩個操作通過電源管理寄存器 2（0x6C）來設置，設置對應位爲 0 即可開啓，如下所示。

const uint8_t MPU6050_REGISTER_PWR_MGMT_2 = 0x6C; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_2, 0x00);

 

至此，MPU6050 的初始化就完成了，可以正常工作了（其他未設置的寄存器全部採用默認值即可）。那麼當代碼燒錄後，MPU的參考點是什麼呢？參考點其實就是MPU6050初始化之後一開始的位置，沒有規定說哪一個方向就是基準點，所以初始化之後的初始位置就是(0,0,0)點。

接下來，我們就可以讀取相關寄存器，來得到加速度傳感器、角速度傳感器和溫度傳感器的數據了。

 

五，數據處理

5.1.原始數據格式

我們感興趣的數據位於 0X3B 到 0X48 這14個字節的寄存器當中。這些數據會被動態更新，下面是相關的寄存器地址與數據名稱。注意每個數據都是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

關於模塊的座標系定義如下圖所示，將模塊面朝自己，此時水平方向即爲X軸，豎直方向爲Y軸，指向自己的方向爲Z軸。

 

圖3 可根據右手螺旋定則確定方向

5.2.DMP處理成歐拉角

MPU6050 自帶了數字運動處理器，即 DMP。而且 InvenSense 提供了一個運動驅動庫，結合 DMP，可以將我們的加速度傳感器和角速度傳感器的原始數據，直接轉換成四元數輸出，而得到四元數之後，就可以很方便地計算出歐拉角：航向角（yaw，也叫偏航角）、橫滾角（roll）和俯仰角（pitch）（注6）。

注6：歐拉角就是物體繞座標系三個座標軸(x,y,z軸）的旋轉角度。這三個角的動畫解釋參考：https://zhuanlan.zhihu.com/p/228805569

DMP 輸出的四元數是 q30 格式的，也就是浮點數放大了 2 的 30 次方倍。在換算成歐拉角之前，必須先將其轉換爲浮點數，也就是除以 2 的 30 次方，然後再進行計算，計算公式爲：

q0=quat[0] / q30;//q30格式轉換爲浮點數 
q1=quat[1] / q30; 
q2=quat[2] / q30; 
q3=quat[3] / q30; //計算得到ypr：pitch俯仰角/roll橫滾角/yaw航向角 
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*(q1q2 + q0q3),q0q0+q1q1-q2q2-q3q3) * 57.3;//航向角

上面代碼中，quat[0]～quat[3]是 MPU6050 的 DMP 解算後的四元數，q30 格式，所以要除以一個2 的 30 次方。其中 q30 是一個常量：1073741824，即 2 的 30 次方，然後帶入公式，計算出歐拉角。上述計算公式的 57.3 是弧度轉換爲角度，即 180/π，這樣得到的結果就是以度（°）爲單位的。關於四元數與歐拉角的公式推導，此處不再贅述。

需要注意的是，單靠 MPU6050 無法準確得到 yaw 角，需要和地磁傳感器結合使用，具體原因請看下一節。

5.3.Yaw角的問題

因爲沒有參考量，所以無法求出當前的Yaw角的絕對角度，只能得到Yaw的變化量，也就是角速度GYR_Z。當然，我們可以通過對GYR_Z積分的方法來推算當前Yaw角（以初始值爲準），但由於測量精度的問題，推算值會發生漂移，一段時間後就完全失去意義了。如果必須要獲得絕對的Yaw角，那麼應當選用MPU9250這款九軸運動跟蹤芯片，它可以提供額外的三軸羅盤數據，這樣我們就可以根據地球磁場方向來計算Yaw角了。

5.4.數據校準和濾波

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

（一）校準

校準是比較簡單的工作，我們只需要找出擺動的數據圍繞的中心點即可。以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。

加速度值的偏移來自兩個方面，一是由於芯片的測量精度，導至它測得的加速度向量並不垂直於大地；二是芯片在整個系統（如無人機）上安裝的精度是有限的，系統與芯片的座標系很難達到完美重合。前者我們稱爲讀數偏移，後者我們稱爲角度偏移。

由於校準角度偏移需要專業設備，且對於一般應用來說，兩步校準帶來的精度提升並不大，因此通常只進行讀數校準即可。讀數校準辦法如下所示。

首先將MPU6050牢牢地固定在系統底座上，並使二者座標系儘可能地重合。其次將系統置於水平、堅固的平面上，並充分預熱。此時，我們認爲芯片的加速度方向應當與Z軸負方向重合，且加速度向量的模長爲g，因此ACC_X和ACC_Y的理論值應爲0，ACC_Z的理論值應爲-16384（假設我們設定2g的倍率，1g的加速度的讀數應爲最大值-32768的一半），即在統計偏移量的過程中，ACC_Z每次讀數都要加上16384，再進行統計均值校準，如下代碼所示。

  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;

（二）卡爾曼濾波

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

注7：Klaman源代碼中文註釋參見：https://blog.csdn.net/acktomas/article/details/89087174

 

六、接線方式

不需要都接。一般只需要這幾個：

模塊 : 開發板

VCC : 5V電源

GND : GND

SCL : 主IIC時鐘線(如下圖所示，一般接NodeMCU的D6)——C代表Clock，控制數據發送的時序。

SDA : 主IIC數據線(如下圖所示，一般接NodeMCU的D7)——D代表Data，用來傳輸數據的。

AD0 : 從機地址最低位。模塊電路中將AD0下拉接地，所以不接其他外部信號。I2C從機地址（手冊可以查看到），對於MPU6050來說固定爲0x68（某些批次可能爲0x98），所以可以用於驗證I2C讀出協議是否正常。也可以調整AD0引腳電壓就可以改變其從機地址：AD0接3V的時候（AD0=1)，從機地址爲0x69。

圖4 一種接線方式

上述接線方式能用下述簡單實驗代碼燒錄成功和跑通：

#include <Wire.h>

// MPU6050 Slave Device Address
const uint8_t MPU6050SlaveAddress = 0x68;//MPU6050的I2C地址

// Select SDA and SCL pins for I2C communication 
const uint8_t scl = D6;
const uint8_t sda = D7;

// sensitivity scale factor respective to full scale setting provided in datasheet 
const uint16_t AccelScaleFactor = 16384;
const uint16_t GyroScaleFactor = 131;

// MPU6050 few configuration register addresses
const uint8_t MPU6050_REGISTER_SMPLRT_DIV   =  0x19;
const uint8_t MPU6050_REGISTER_USER_CTRL    =  0x6A;
const uint8_t MPU6050_REGISTER_PWR_MGMT_1   =  0x6B;
const uint8_t MPU6050_REGISTER_PWR_MGMT_2   =  0x6C;
const uint8_t MPU6050_REGISTER_CONFIG       =  0x1A;
const uint8_t MPU6050_REGISTER_GYRO_CONFIG  =  0x1B;
const uint8_t MPU6050_REGISTER_ACCEL_CONFIG =  0x1C;
const uint8_t MPU6050_REGISTER_FIFO_EN      =  0x23;
const uint8_t MPU6050_REGISTER_INT_ENABLE   =  0x38;
const uint8_t MPU6050_REGISTER_ACCEL_XOUT_H =  0x3B;//我們感興趣的數據位於 0X3B 到 0X48 這14個字節的寄存器當中
const uint8_t MPU6050_REGISTER_SIGNAL_PATH_RESET  = 0x68;

int16_t AccelX, AccelY, AccelZ, Temperature, GyroX, GyroY, GyroZ;

void setup() {
  Serial.begin(115200);
  Wire.begin(sda, scl);
  MPU6050_Init();
}

void loop() {
  double Ax, Ay, Az, T, Gx, Gy, Gz;
  
  Read_RawValue(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_XOUT_H);
  
  //divide each with their sensitivity scale factor
  Ax = (double)AccelX/AccelScaleFactor;
  Ay = (double)AccelY/AccelScaleFactor;
  Az = (double)AccelZ/AccelScaleFactor;
  T = (double)Temperature/340+36.53; //temperature formula
  Gx = (double)GyroX/GyroScaleFactor;
  Gy = (double)GyroY/GyroScaleFactor;
  Gz = (double)GyroZ/GyroScaleFactor;

  Serial.print("Ax: "); Serial.print(Ax);
  Serial.print(";Ay: "); Serial.print(Ay);
  Serial.print(";Az: "); Serial.print(Az);
  Serial.print(";T: "); Serial.print(T);
  Serial.print(";Gx: "); Serial.print(Gx);
  Serial.print(";Gy: "); Serial.print(Gy);
  Serial.print(";Gz: "); Serial.println(Gz);

  delay(100);
}

void I2C_Write(uint8_t deviceAddress, uint8_t regAddress, uint8_t data){
  Wire.beginTransmission(deviceAddress);
  Wire.write(regAddress);
  Wire.write(data);
  Wire.endTransmission();
}

// read all 14 register
void Read_RawValue(uint8_t deviceAddress, uint8_t regAddress){
  Wire.beginTransmission(deviceAddress);
  Wire.write(regAddress);
  Wire.endTransmission();
  Wire.requestFrom(deviceAddress, (uint8_t)14);
  AccelX = (((int16_t)Wire.read()<<8) | Wire.read());
  AccelY = (((int16_t)Wire.read()<<8) | Wire.read());
  AccelZ = (((int16_t)Wire.read()<<8) | Wire.read());
  Temperature = (((int16_t)Wire.read()<<8) | Wire.read());
  GyroX = (((int16_t)Wire.read()<<8) | Wire.read());
  GyroY = (((int16_t)Wire.read()<<8) | Wire.read());
  GyroZ = (((int16_t)Wire.read()<<8) | Wire.read());
}

//configure MPU6050
void MPU6050_Init(){
  delay(150);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SMPLRT_DIV, 0x07);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_1, 0x01);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_2, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_CONFIG, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_GYRO_CONFIG, 0x00);//set +/-250 degree/second full scale
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_CONFIG, 0x00);// set +/- 2g full scale
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_FIFO_EN, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_INT_ENABLE, 0x01);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SIGNAL_PATH_RESET, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_USER_CTRL, 0x00);
}

但是需要注意，這時候我們常用來遍歷打印I2C設備地址的Scanning-I2C-Device程序就報告找不到I2C設備。

所以爲了找到I2C設備，爲了能使用Adafruit_MPU6050開源庫，我們採用了下面這種NodeMCU+MPU6050的接線方式：

VCC VU (5V USB) Not available on all boards so use 3.3V if needed.

GND G Ground

SCL D1 (GPIO05) I2C clock

SDA D2 (GPIO04) I2C data

XDA not connected

XCL not connected

AD0 not connected

INT D8 (GPIO15) Interrupt pin

只有按上述方式接線，Scanning-I2C-Device程序纔會打印說找到了I2C設備：

Scanning...

I2C device found at address 0x68 !

Adafruit_MPU6050庫的樣例代碼也才能找到芯片。

附錄B：

引腳講解和簡單示例代碼可參考：https://docs.wokwi.com/parts/wokwi-mpu6050

通常我們爲了方便把IIC設備分爲主設備和從設備，基本上誰控制時鐘線（即控制SCL的電平高低變換）誰就是主設備。

 

七，外接Processing實驗

Processing IDE是一款強大的，能與其他軟件尤其是硬件如Arduino、STM32和ESP32直接交互的編程語言和開發環境。

爲了能讓MPU6050+NodeMCU的串口輸出能夠控制Processing軟件裏的小飛機模型，我們需要準備好以下庫：

1）Arduino IDE需要安裝如下庫或者直接把庫文件複製到你的文件夾下直接引用：

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

2）Processing IDE需要安裝如下庫：

https://github.com/postspectacular/toxiclibs

接下來開始運行。

在Arduino IDE中打開 MPU6050_DMP6 的示例，註釋掉其他選項，只保留下面這一個選項：

#define OUTPUT_READABLE_YAWPITCHROLL

燒錄之後，你會看到串口打印了yaw、pitch、roll三個角的實時變化：

MPU6050 connection successful

Send any character to begin DMP programming and demo:

Initializing DMP...

.****************.***....>......-972.00000, -429.00000, 4374.00000, 96.00000, 16.00000, -34.00000

Enabling DMP...

DMP ready! Waiting for first interrupt...

ypr 24.96 -0.83 -11.36

ypr 23.79 -0.54 -9.71

ypr 23.21 -0.22 -8.21

ypr 22.98 -0.17 -6.86

ypr 22.87 -0.41 -5.73

ypr 22.63 -0.72 -4.76

ypr 21.65 -1.02 -3.73

ypr 20.10 -1.19 -2.59

ypr 18.38 -1.10 -1.33

接下來我們要與Processing IDE配合了。

在 MPU6050_DMP6 中註釋掉其他選項，只保留下面這一個選項：

#define OUTPUT_TEAPOT

燒錄上傳。同時用Processing IDE打開 MPUTeapot.pde，運行起來，Teapot程序會自動給串口發送一個字符讓MPU6050跑起來。此時，正值MPU初始化階段，你的MPU模塊一定要水平地平放在穩固的平面上，如下圖所示，MPU平放在你面前，亮燈的地方就是機尾，機頭方向就是X軸，垂直於模塊向上就是Z軸。放得不平，或者方向不對，小飛機就會飛得奇奇怪怪。

圖5 模塊的機頭機尾方向

初始化成功後，模塊的機頭向上，Processing IDE裏的小飛機也機頭向上了，如下圖所示。

圖6 MPU+Processing第一次聯動

如果用 MPUplane.pde 配合，還可以繪製出3D小飛機的飛行姿態效果，並解算出歐拉角和ypr，可以直觀地對比它倆的區別，如下圖所示。

圖7 MPU+Processing第二次聯動

附錄C：

MPU6050_DMP6 中有一行代碼，可能會引發ESP8266不斷重啓，如下所示：

attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

ESP8266不斷重啓的同時，串口會一直重複打印如下堆棧信息和ISR not in IRAM的報錯：

Initializing I2C devices...

Testing device connections...

MPU6050 connection successful

 

Send any character to begin DMP programming and demo:

Initializing DMP...

>*......>......-1632.00000, 667.00000, 3960.00000, 65.00000, 22.00000, -17.00000

 

Enabling DMP...

Enabling interrupt detection (Arduino external interrupt 2)...

ISR not in IRAM!

 

User exception (panic/abort/assert)

--------------- CUT HERE FOR EXCEPTION DECODER ---------------

 

Abort called

我只好把這段代碼註釋掉。

 

八，外接匿名上位機(V7)

在單片機開發領域裏，什麼是上位機？

上位機指的是可以直接發送操作指令的計算機或者單片機，一般提供用戶操作交互界面並向用戶展示反饋數據。單片機主動發送狀態信息或者報警信息給上位機。只要通信協議可以建立，上位機軟件可以是任意開發語言和任意平臺，下位機可以是單片機。

什麼是匿名上位機？

匿名指的是匿名科創，主頁地址：http://anotc.com/。

這個團隊公佈的上位機可以完成基本收發（類似於串口調試助手）、高級收發收碼（實現比較複雜的自定義接收和發送）、同時顯示20條波形圖（用於調試PID等）、調試無人機（可以監視無人機的飛控狀態以及調試PID），最新的版本號爲V7.2.5。我們可以在匿名資料彙總頁面下載匿名助手、匿名上位機等軟件。

一定要注意下面講的是MPU6050模塊+NodeMCU開發板與匿名上位機V7的聯調，再強調一遍，是V7！一定要匹配V7飛控通信協議！不是V2.6，不是V4.5，是V7！

8.1.單片機上報靈活格式幀給上位機

第一步：單片機一側

將MPU6050獲取到的六軸參數（xyz三個方向的加速度值和xyz三個方向的陀螺儀值）組裝成一幀，功能碼設置爲0XF1。這個0XF1對應於協議裏定義的靈活格式幀的功能碼ID，如下圖所示。

圖8 靈活格式幀說明

由於每一幀的數據長度最大可以是40，所以我們可以在這個長度範圍內把一些相關的數據打包放在一起，組成本幀，如下面代碼所示。

注意，此時的目標地址碼爲0XFF，指的是廣播型輸出。

void Ano_Init(void) 
{
  MyAno.Head = FRAME_HEADER;//0XAA是幀頭
  MyAno.Addr = GENERAL_OUTPUT_ADDR;//地址碼爲0xFF，指的是無特定目標，用於數據廣播型輸出
  MyAno.Lenth = 0;
}
//-----------------與匿名上位機通訊-----------------------------
/**  發送靈活格式幀，幀ID爲0xF1～0xFA
  *  功能：發送加速度傳感器和陀螺儀傳感器數據給匿名上位機（V7）
  *  入口參數：第一個參數accel，它的三個元素是xyz三個方向的加速度值
  *           第二個參數gyro，它的三個元素是xyz三個方向的陀螺儀值     
  *  返回值：無
  *  注：數據格式：幀頭0xAA+目標地址0xFF+功能碼0xF1+數據長度LEN+DATA+和校驗SC+附加校驗AC
  */
void mpu6050_send_data2ano(VectorInt16 *accel,VectorInt16 *gyro)
{ 
  uint8_t nul = 0;
  Ano_Set_Mdata(0xF1,(int16_t*)&accel->x,sizeof(accel->x),1);
  Ano_Set_Mdata(0xF1,(int16_t*)&accel->y,sizeof(accel->y),2);
  Ano_Set_Mdata(0xF1,(int16_t*)&accel->z,sizeof(accel->z),3);
  Ano_Set_Mdata(0xF1,(int16_t*)&gyro->x,sizeof(gyro->x),4);
  Ano_Set_Mdata(0xF1,(int16_t*)&gyro->y,sizeof(gyro->y),5);
  Ano_Set_Mdata(0xF1,(int16_t*)&gyro->z,sizeof(gyro->z),6);
  Ano_Set_Mdata(0xF1,(uint8_t*)&nul,sizeof(nul),7);//加載數據到對應的數據位
  Ano_SendMdata();//發送數據
}

第二步：上位機一側

上位機根據指定的波特率打開相應的串口連接，如下圖所示：

圖9 上位機連接設置

第三步，打開上位機的“協議解析”界面，這時候應該已經收到了來自於MPU6050的上報信息。如下圖所示，注意看ID這一列，全都是F1。我們將點擊右下角的設置，來使能F1幀，並自定義用戶數據。

圖10 上位機協議解析界面

第四步，首先，我們在“高級收碼設置”的“自定義幀數據配置”裏勾選F1這一行的“使能該幀”，如下圖所示：

圖11 上位機自定義幀數據設置

其次，我們繼續在“高級收碼設置”的“數據容器配置”裏，依次修改容器的名稱、用戶幀和數據位。

如下圖所示，因爲我們在F1幀裏傳入了六軸參數，所以用戶幀都選F1，數據位依次是1到6。爲了方便顯示和理解，我們把容器名稱也都改一改。

圖12 上位機數據容器設置

第五步，前面我們準備好了數據容器，就可以打開上位機的“波形分析”界面。默認它展示的是飛控基本波形的數據容器列表，沒有我們的。所以我們需要打開“設置”界面，點擊“用戶數據波形”，然後點擊“確定”，如下圖所示。

圖13 上位機波形設置

到此，上位機配置完成。只需要我們的單片機按照靈活格式幀的協議格式將數據發送至上位機，即可觀察到自定義數據容器對應的數據值開始刷新，並繪製對應數據波形。

現在回到“波形顯示”界面，左側“波形選擇”區域已經展示了我們剛纔定義的六個數據容器，全部勾選之後，就可以看到右側區域開始繪製波形了。波形顯示的本質就是在一個固定的時間間隔內建立座標然後進行連線，對於上位機的波形來說，這個時間取決於單片機發數據的頻率。

上下左右搖動我們的MPU6050模塊，就可以看到波形有一些大幅度的變化。

圖14 上位機波形顯示

到此，NodeMCU+MPU6050如何向匿名上位機（V7）發送串口數據，上位機如何配置使能該幀和數據容器，如何配置波形顯示，都演示完畢了。

8.2.單片機上報飛行狀態數據給上位機

畢竟如果MPU6050模塊是一個無人機，還是要看到無人機模型的動畫展示才更直觀，下面講一下配置流程。

第一步：單片機一側

上位機的飛控狀態至少需要單片機上報兩種數據幀。

第一種是慣性傳感器數據，功能碼爲0X01。我們把六軸參數逐一壓進去即可，震動狀態可以暫時置爲零，如下圖所示。

圖15 上位機慣性數據說明

第二種是歐拉角格式數據，功能碼爲0X03。我們把算出來的角度乘以100，再壓進去，上位機在展示的時候會自己除以100，如下圖所示。

圖16 上位機歐拉角數據說明

這麼做的原因是爲了提高數據傳輸的效率，當有浮點數類型數據需要傳輸時，根據數據類型的特點，適當截取小數點後固定幾位，比如保留歐拉角的小數點後兩位即可（即乘以100），將浮點數轉化成整數類型進行傳輸，可縮短數據長度，並且避免浮點數傳輸時發生異 常，解析成非法浮點數。

此時的目標地址碼仍然是0XFF（廣播型輸出）。

好，現在飛控相關的基本信息類幀就組裝完畢了，如下面的代碼所示：

//-----------------與匿名上位機通訊-----------------------------
/**  發送基本信息類幀，功能碼ID爲0x01～0x04之間，在飛控通信協議裏屬於飛控相關信息類
  *  功能：發送慣性傳感器、歐拉角等數據給匿名上位機（V7）
  *  入口參數：第一個參數accel，它的三個元素是xyz三個方向的加速度值
  *           第二個參數gyro，它的三個元素是xyz三個方向的陀螺儀值
  *           第三個參數ypr，這是一個float[]數組，分別是yaw/pitch/roll angles原始值
  *  返回值：無
  *  注：數據格式：幀頭0xAA+目標地址0xFF+功能碼+數據長度LEN+DATA+和校驗SC+附加校驗AC
  *  注：爲了提高數據傳輸的效率，當有浮點數類型數據需要傳輸時，根據數據類型的特點，適當截取小數點後固定幾
        位，比如飛控姿態數據，保留角度的小數點後兩位即可（即乘以100）。
        將浮點數轉化成整數類型進行傳輸，可縮短數據長度，並且避免浮點數傳輸時發生異
        常，解析成非法浮點數。
  */
void mpu6050_send_flydata2ano(VectorInt16 *accel,VectorInt16 *gyro,float *ypr)
{ 
  uint8_t nul = 0;
  //ID：0x01：慣性傳感器數據
  //DATA 區域內容：
  //ACC、GYR：依次爲加速度、陀螺儀傳感器數據。
  //SHOCK_STA：震動狀態
  Ano_Set_Mdata(0x01,(int16_t*)&accel->x,sizeof(accel->x),1);
  Ano_Set_Mdata(0x01,(int16_t*)&accel->y,sizeof(accel->y),2);
  Ano_Set_Mdata(0x01,(int16_t*)&accel->z,sizeof(accel->z),3);
  Ano_Set_Mdata(0x01,(int16_t*)&gyro->x,sizeof(gyro->x),4);
  Ano_Set_Mdata(0x01,(int16_t*)&gyro->y,sizeof(gyro->y),5);
  Ano_Set_Mdata(0x01,(int16_t*)&gyro->z,sizeof(gyro->z),6);
  Ano_Set_Mdata(0x01,(uint8_t*)&nul,sizeof(nul),7);
  Ano_SendMdata();//發送數據
  //ID：0x03：飛控姿態：歐拉角格式
  //DATA 區域內容：
  //ROL、PIT、YAW：姿態角，依次爲橫滾、俯仰、航向，精確到 0.01。
  //FUSION _STA：融合狀態
  //注意：角度最終要乘以100，上位機那邊會除以100做展示
  int16_t rol = (int16_t)(ypr[2] * 180 * 100.00 / M_PI);
  int16_t pit = (int16_t)(ypr[1] * 180 * 100.00 / M_PI);
  int16_t yaw = (int16_t)(ypr[0] * 180 * 100.00 / M_PI);
  Ano_Set_Mdata(0x03,(int16_t*)&rol,sizeof(rol),1);
  Ano_Set_Mdata(0x03,(int16_t*)&pit,sizeof(pit),2);
  Ano_Set_Mdata(0x03,(int16_t*)&yaw,sizeof(yaw),3);
  Ano_Set_Mdata(0x03,(uint8_t*)&nul,sizeof(nul),4);
  Ano_SendMdata();//發送數據
}

第二步，上位機一側

在上位機裏打開連接後，我們就會發現已經有數據進來了。打開“飛行狀態”界面（如下圖所示），點擊下方的“數據顯示”按鈕。

圖17 上位機飛行狀態界面

我們就會看到飛控通信協議裏的基本信息類幀都在這裏羅列了，如下圖所示。我們點開單片機本次上報的“Frame ID: 0x01”和“Frame ID: 0x03”，就會看到兩個要點：

第一，數據值這一列一直在變化，說明數據都能被正確地解析；

第二，傳輸縮放這一列裏，慣性傳感器數據都沒有做縮放，而歐拉角則做了100（即1.0E+2）的倍數縮放。

圖18 上位機數據顯示界面

第三步，我們打開上位機的“波形顯示”畫面，在它的設置項裏選“飛控基本波形”並確定，就會看到波形顯示的左側區域展示了慣性傳感器和歐拉角等預定義數據項，勾選之後就會看到波形在實時變化，隨着你搖動旋轉MPU6050模塊。

圖19 上位機波形設置

第四步，MPU6050與上位機飛行狀態聯動的效果如下圖所示：

圖20 單片機與上位機飛行姿態聯動

到此，單片機按照匿名公司的飛控通信協議V7（注意版本號哦），將MPU6050模塊啓用自帶的DMP計算出來的歐拉角等數據，以基本信息類幀的數據格式，通過串口上報給電腦端的匿名上位機（V7），從而可以讓MPU6050的運動姿態聯動上位機的無人機模型。

 

最後，我們總結一下：

0x01：【元器件基礎】學習瞭如何基於Adafruit_MPU6050和Kalman_Filter_Library庫，利用MPU6050模塊，獲得六軸參數，並進一步計算俯仰角和橫滾角。

0x02：【元器件進階】學習MPU6050如何對接Processing IDE，在電腦端實時繪製模型的飛行姿態。

0x03：【元器件進階】學習MPU6050如何對接匿名上位機（V7），在上位機裏收碼、波形顯示和實時飛行姿態。

 

-END-