跳轉到

第 2 章：感測器驅動 —— 飛機怎麼「感覺」自己

本章學習目標

  • 搞懂飛控怎麼透過 I2C 跟 MPU6050「問」資料
  • 讀懂 MpuGetData()：從暫存器連讀 → 高低位元組拼成數值 → 減零偏 → 濾波
  • 帶你算一次：把原始數字換算成「幾度/秒」「幾個 g」
  • 學會用 ANO 上位機看波形驗證感測器有沒有正常

2.1 先有感覺：感測器是飛機的「內耳」

人能閉著眼睛知道自己有沒有歪，是靠內耳的平衡器官。飛機沒有內耳，靠的是 MPU6050—— 裡面有兩個東西：

  • 陀螺儀（Gyroscope）：量「轉動的快慢」，單位是度/秒（°/s）。
  • 加速度計（Accelerometer）：量「受力」，靜止時只受地心引力，所以能拿來找「哪邊是下」。

問題是，MPU6050 是一顆獨立的晶片，它不會主動把資料塞給主控。主控（STM32）得自己去「問」它。 怎麼問？透過一條叫 I2C 的兩線匯流排。

2.2 I2C 一分鐘：像點名一樣問答

I2C 只用兩條線（SCL 時脈、SDA 資料），上面可以掛很多顆晶片，每顆有自己的「位址」。 溝通像點名：

主控：「位址 0x68 的 MPU6050 在嗎？我要讀你第 0x3B 號暫存器開始的資料。」 MPU6050：「在，這是你要的 6 個位元組……」

這台飛機用「軟體 I2C」

F103 雖然有硬體 I2C，但這份韌體用 GPIO 自己模擬 I2C 時序（CONFIG/IIC_SOFTWARE.c）， 好處是接腳自由、避開 F103 硬體 I2C 的一些坑。你在電路圖上找 MPU6050 的 SCL/SDA 接到哪兩隻腳， 就能對應到這份程式裡 GPIO 的設定。

2.3 讀一筆資料的完整旅程：MpuGetData()

這是 2ms 任務裡第一件事（還記得第 1 章的 Duty_2ms 嗎？）。我們一段段看它做什麼：

HARDWARE/mpu6050.c（已將 GBK 註解翻成正體中文）
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
#define Gyro_Read()  IIC_read_Bytes(MPU6050_ADDRESS, 0x3B, buffer,    6) // 讀加速度三軸（各2位元組）
#define Acc_Read()   IIC_read_Bytes(MPU6050_ADDRESS, 0x43, &buffer[6], 6) // 讀陀螺三軸（各2位元組）

void MpuGetData(void)
{
    uint8_t i;
    uint8_t buffer[12];

    Gyro_Read();                       // ① 一次連讀 6 個位元組（加速度 X/Y/Z）
    Acc_Read();                        // ② 再連讀 6 個位元組（陀螺 X/Y/Z）

    for (i = 0; i < 6; i++)
    {
        // ③ 每個軸是「高位元組在前」的兩個位元組，拼成一個 16 位元有號數，再減掉零偏（校準值）
        pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1]) - MpuOffset[i];

        if (i < 3)                     // ④ 加速度：走一維卡爾曼濾波（抗振動雜訊）
        {
            static struct _1_ekf_filter ekf[3] = { /* 每軸一組濾波器狀態 */ };
            kalman_1(&ekf[i], (float)pMpu[i]);
            pMpu[i] = (int16_t)ekf[i].out;
        }
        if (i > 2)                     // ⑤ 陀螺：走一階低通濾波（factor=0.15）
        {
            const float factor = 0.15f;
            static float tBuff[3];
            uint8_t k = i - 3;
            pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
        }
    }
}

把它讀成一句白話：「連讀 12 個位元組 → 每兩個拼成一個數 → 扣掉零偏 → 加速度用卡爾曼、陀螺用低通各自濾一下。」

為什麼要『高位元組在前』拼起來？

感測器一個軸的數值範圍是 −32768~32767，一個位元組（8 位元）裝不下，要用兩個位元組。 MPU6050 規定先送高 8 位、再送低 8 位，所以程式把 buffer[高] << 8 | buffer[低]， 就是把兩半合回一個 16 位元的完整數值。

2.4 帶你算一次①：原始數字 → 看得懂的物理量

pMpu[] 裡是「原始讀數（LSB）」，還不是度/秒或 g。換算靠靈敏度，而靈敏度由 2.3 節初始化時設的量程決定：

  • 陀螺設 ±2000°/s、加速度設 ±4g（mpu6050.c 寫入 GYRO_CONFIG=0x18、ACCEL_CONFIG=0x09）。

16 位元有號數的範圍是 ±32768，把它對應到量程：

\[ \text{陀螺靈敏度} = \frac{32768}{2000} = 16.384 \ \text{LSB 每 } (°/s) \qquad \text{加速度靈敏度} = \frac{32768}{4} = 8192 \ \text{LSB 每 } g \]

實際算一次：假設某時刻讀到 gyroX = 1638、accZ = 8192：

  • 角速度 = 1638 ÷ 16.384 = ≈ 100 °/s（飛機這軸正以每秒 100 度轉動）
  • 受力 = 8192 ÷ 8192 = 1.000 g（剛好等於地心引力 → 代表飛機這時平放、Z 軸朝上）

程式裡這個換算常數叫 Gyro_G ≈ 0.0610（= 1/16.384），第 3 章姿態解算就會用到它。

2.5 帶你算一次②：低通濾波在做什麼

陀螺那行 tBuff = tBuff*(1-0.15) + raw*0.15 就是一階低通濾波，白話是 「新值只採信新讀數的 15%，其餘 85% 沿用舊值」，這樣突如其來的雜訊會被壓掉。

算一次：上一刻濾波值 old = 100，這次新讀數 raw = 160（含一個跳動）：

\[ \text{new} = 0.85 \times 100 + 0.15 \times 160 = 85 + 24 = \mathbf{109} \]

看到沒？輸出只從 100 移到 109，而不是暴衝到 160——雜訊被「拖住」了。 代價是反應稍微變慢（這就是濾波永遠的取捨：越平滑 vs 越靈敏）。

2.6 為什麼開機要「靜置校準」

程式初始化時呼叫 MpuGetOffset()：飛機靜止平放時連續取樣，算出陀螺三軸的平均值當作「零偏」， 之後每筆讀數都先減掉它（就是 2.3 節的 - MpuOffset[i]）。加速度 Z 軸則以 8192（=1g）為基準。

校準時一定要放平、不要動

陀螺只要有一點點零偏沒扣掉，積分起來角度就會持續漂移（第 3 章會看到漂移的後果）。 所以每次上電校準的那兩秒，飛機務必靜止放在水平面上。

2.7 其他感測器走哪條路

感測器 量什麼 匯流排 對應程式
MPU6050 角速度、加速度 I2C（軟體模擬） HARDWARE/mpu6050.c
SPL06-001 氣壓 → 高度 I2C / SPI HARDWARE/spl06.c
PMW3901 光流（水平位移） SPI HARDWARE/flow.c
nRF24L01 2.4G 遙控收發 SPI HARDWARE/nrf24l01.c

想知道每顆接到 STM32 哪幾隻腳？打開電路圖 4.四轴飞机资料/飞控_2电路图/飞控电路图.pdf， 對照各晶片的 SCL/SDA 或 SCK/MOSI/MISO/CS。氣壓計與光流的原理我們留到第 7、8 章再深入。

2.8 每個感測器：有他不一樣、沒他會怎樣

把這台飛機的感測器想成一個團隊，各司其職。用「有他 → / 沒他 →」幫你建立感覺：

MPU6050（慣性測量，唯一不可或缺）

  • 有他 → 飛機知道自己「轉多快、往哪歪」，這是能穩定飛行的根本。
  • 沒他 → 根本飛不起來。連自己是正的還是翻的都不知道，馬達無從修正。全機只有它「拿掉就完全沒戲」。

SPL06-001（氣壓計 → 高度）

  • 有他 → 能「定高」：油門放中間，飛機自己維持高度不掉不衝。
  • 沒他 → 只能純手動控油門，手一鬆就掉高或暴衝（第 7 章）。

PMW3901（光流 → 水平位移）

  • 有他 → 室內沒 GPS 也能「定點懸停」，賴在原地不亂跑。
  • 沒他 → 會慢慢水平漂移，得一直推桿修正（第 8 章）。

nRF24L01（2.4G 無線，嚴格說是通訊不是感測）

  • 有他 → 收得到遙控器指令。
  • 沒他 → 等於斷線（觸發失控保護）。

🧭 這台「缺席」的兩個感測器（很重要）

  • 磁力計／指南針：這台沒有 → 所以 yaw 航向會慢慢漂（第 3 章已親眼驗證）。加上它 → yaw 有絕對參考、不漂。
  • GPS：這台沒有 → 不能戶外定位、不能一鍵返航；室內定點只能靠光流。

規律：陀螺/加速度管「姿態」，氣壓/光流管「高度與位置」，磁力計/GPS 管「絕對方向與絕對位置」。 這台做到前兩層，所以室內穩穩飛；少了第三層，所以 yaw 會漂、不能戶外自動返航。

2.9 想升級 IMU？MPU6050 → ICM-42688 到底差多少

你問得很實際：現在 MPU6050 已飛很穩，換 ICM-42688-P 值不值？先看原廠數字（TDK InvenSense），再用生活化方式解釋：

指標 MPU6050 ICM-42688-P 差距
陀螺噪聲密度 ~5 mdps/√Hz 2.8 mdps/√Hz 約少一半
加速度噪聲密度 ~400 µg/√Hz 70 µg/√Hz 約 1/5～1/6
最高取樣率（ODR） 8 kHz 32 kHz 4 倍
介面 I2C（慢） SPI（快很多） 能跑更高迴圈頻率

🩺 生活化比喻①：吵房間 vs 安靜房間聽心跳 感測器的「噪聲」就像背景雜音。MPU6050 像在有點吵的房間用聽診器聽心跳——聽得到，但要用力分辨；ICM-42688 像在安靜房間聽，同樣的心跳清清楚楚。心跳（真實轉動訊號）沒變，但你「聽得更乾淨」。

📷 生活化比喻②：相機的 ISO 雜訊 低噪的 ICM 像低 ISO：畫面乾淨。高噪的 MPU 像高 ISO：滿是雜點，你得開「降噪（濾波）」才能看——但降噪會讓畫面變糊、變慢（控制上就是反應變鈍、有延遲）。因為 ICM 本身乾淨，可以少濾波 → 反應更跟手。

對你這台的實際差別

  • 溫和飛行（穩穩懸停、慢慢飛）：幾乎無感。就像安靜或稍吵都聽得到心跳——你現在 MPU6050 飛得穩，正是因為溫和飛行對噪聲不敏感。
  • 劇烈飛行 / 特技 / 震動大的機架：差最多。大油門與翻滾讓螺旋槳震動灌進感測器，MPU6050 的高噪聲逼你加重濾波 → 控制變鈍或開始抖；ICM 維持乾淨 → 控制扎實、翻滾俐落。
  • 更高迴圈頻率：ICM 走 SPI 又快，控制迴圈能從現在的 2ms（500Hz）拉到 1kHz 以上，修正更即時。

升級要付出的代價（工程面）

  • ICM 走 SPI、暫存器位址與初始化跟 MPU6050 不同 → HARDWARE/mpu6050.c 那層驅動要重寫成 ICM 的 SPI 讀寫＋暫存器設定。
  • 2.4 節那些靈敏度換算常數要改（ICM 的 LSB/°/s、LSB/g 不同）。
  • 好消息：姿態解算 imu.c 和 PID 完全不用動！因為它們吃的是「°/s 和 g」這種物理量，跟底下是哪顆晶片無關——這就是「驅動層／演算法層分離」的威力。

一句話結論：溫和飛行幾乎無感；要玩特技、抗震、衝穩定性極限才有感。你既然想自學設計、之後玩特技，ICM 是值得的升級；但若只要穩穩飛，現在的 MPU6050 完全夠用。

FPV 圈現況：自 2025 起 ICM-42688-P 已是中高階飛控的新標準，因為它在劇烈飛行下的乾淨訊號明顯勝出。 資料來源：TDK InvenSense ICM-42688-P 產品頁、 ICM-42688-P 規格書（PDF）、 UAVMODEL：FPV 陀螺比較 MPU6000 vs ICM-42688 vs BMI270。

2.10 哪個演算法用哪個感測器的資料

把「資料從哪來、被誰用」整理成一張表（也是整台飛機的資訊流總結）：

計算 / 功能 用到的感測器 備註
姿態 roll / pitch（四元數＋Mahony） 陀螺（主積分）＋加速度（修正重力方向） 第 3 章
姿態 yaw（航向） 只有陀螺 → 會漂（無磁力計） 第 3 章已驗證
角速度內環 陀螺 第 4 章
角度外環 姿態角（由陀螺＋加速度算來） 第 4 章
高度定高 氣壓計 SPL06 ＋ 光流測距高度 第 7 章
水平定點 光流 PMW3901（量水平位移） 第 8 章
遙控指令 nRF24L01 / ELRS 第 6 章

一句話記法：陀螺撐起「快速反應」，加速度負責「不要長期歪」，氣壓/光流管「高度與位置」，遙控給「你的意圖」。

2.11 動手驗證（用 ANO 上位機看波形）

這是確認「感測器到底有沒有讀對」最直接的方法：

  • 接上飛控、開 ANO 上位機，把陀螺三軸畫成波形。飛控靜止時，三條線應該都貼在 0 附近（校準有效）。
  • 把飛控繞某一軸轉動，看對應那條陀螺波形跟著上下，放手回正又回到 0。
  • 看加速度：平放時 Z≈8192（1g）、X/Y≈0；把飛控側立 90°，會看到 1g「轉移」到另一個軸。
  • 敲一下桌面製造振動，比較「原始」與「卡爾曼濾波後」的加速度——濾波後明顯比較平滑。

做完這四步，你就親眼確認了 2.3~2.5 節講的每一件事。

2.12 思考題

  • 把低通的 factor 從 0.15 改成 0.5，用 2.5 節同樣的數字再算一次，輸出會變多少？這樣濾波是更靈敏還是更平滑？
  • 如果校準時手在抖，MpuOffset 會被算錯，飛機起飛後最可能出現什麼現象？（提示：跟「漂移」有關，第 3 章見分曉）
  • 加速度為什麼能拿來「找下方」，陀螺卻不行？（提示：哪一個量到的東西「長期穩定」）
  • 承上題的延伸：為什麼換成 ICM-42688 後，imu.c 和 PID 都不用改、只要改 mpu6050.c 那層驅動？這對你「設計自己的飛控」有什麼啟發？（提示：分層）

下一章：我們手上有了乾淨的陀螺與加速度，但飛機要的是「現在傾斜幾度」。 怎麼把感測器資料變成角度？這就是姿態解算。 👉 第 3 章：姿態解算