跳轉到

第 7 章：氣壓定高

本章學習目標

  • 理解「氣壓計量高度」為什麼準又不準，要怎麼補救
  • 看懂高度的互補濾波融合（和第 3 章姿態同一個套路）
  • 看懂定高的串級：高度環 → 垂直速度環 → 油門

7.1 放開油門，它自己停在原地高度

到目前為止，高度都得你用油門手動控制——手一鬆就掉。定高要做到的是： 油門放到中間，飛機自己維持當前高度。這需要兩樣東西：先量到高度，再用 PID 控制它。

7.2 高度從哪來？氣壓計 + 加速度（又是互補濾波）

這台用 SPL06-001 氣壓計：氣壓隨高度變化，換算就能得到高度。但它有個老問題—— 長期準、短期抖（氣流、噪聲讓讀數跳動），而且更新慢。

是不是很耳熟？這跟第 3 章「加速度計長期準、短期抖」一模一樣。解法也一樣：互補濾波。 只是這次搭檔換成「加速度計的垂直分量」——它短期靈敏但積分會漂。

🔁 和第 3 章對照： 姿態：陀螺（快、漂）＋ 加速度（穩、抖）→ 融合出角度。 高度：加速度垂直分量（快、漂）＋ 氣壓（穩、抖）→ 融合出高度。 同一個哲學：用「快的」當主力，用「穩的」拉回漂移。

程式在 HAL/high_process.c 的 Strapdown_INS_High()，是一個 APM 風格的三階互補濾波： 它同時修正位置、速度、加速度三層，用一個時間常數 τ 調快慢：

HAL/high_process.c（已將 GBK 註解翻成正體中文）
63
64
65
66
temp = Altitude_Estimate - History_Z[High_Delay_Cnt];   // 氣壓量到的高度 − 估計的高度 ＝ 誤差
acc_correction += temp * K_ACC_ZER * dt;                // 用誤差去修「加速度」
vel_correction += temp * K_VEL_ZER * dt;                // 修「速度」
pos_correction += temp * K_POS_ZER * dt;                // 修「位置（高度）」

帶你算一次（三個 K 從哪來）：時間常數 τ = 3.0s：

\[ K_{pos} = \frac{3}{\tau} = 1.0, \quad K_{vel} = \frac{3}{\tau^2} = 0.33, \quad K_{acc} = \frac{1}{\tau^3} = 0.037 \]

看出規律了嗎？越「底層」（加速度）修得越輕（0.037），越「表層」（位置）修得越重（1.0）。 這樣高頻交給加速度積分（反應快），低頻交給氣壓校正（不漂）——融合出又快又不漂的 Height.High 和 Height.Speed。

剛上電的 500 拍會先「歸零」

程式開頭 if(delay_cnt<500) 會先把氣壓當前值記成基準 high_offset，之後的高度都是「相對起飛點」。 所以定高的 0 公尺＝你開機的地方，不是海拔。

7.3 控制高度：又是串級

有了高度，怎麼控制它？跟姿態一樣用串級 PID（HAL/control.c 的 HeightPidControl()）：

HAL/control.c（節錄）
150
151
152
pidUpdate(&pidHeightHigh, dt);              // ① 外環：目標高度 vs 當前高度
pidHeightRate.desired = pidHeightHigh.out;  // ② 外環輸出 → 內環的「目標垂直速度」
pidUpdate(&pidHeightRate, dt);              // ③ 內環：目標速度 vs 當前垂直速度 → 油門修正量

和第 4 章姿態的串級結構完全一樣，只是物理量換成「高度／垂直速度」：

外環 pidHeightHigh 內環 pidHeightRate
看什麼 想要的高度 vs 現在高度 想要的爬升速度 vs 現在垂直速度
增益 kp=2.0 kp=0.30、kd=0.003
輸出 目標垂直速度 油門的增減量

內環輸出的「油門修正量」最後加進第 5 章的基礎油門 thr_temp，四顆馬達一起增減 → 飛機升或降。 （還記得第 5 章「混控不改變平均油門」嗎？正因如此，定高只要動「平均油門」就好，不會影響姿態。）

7.4 怎麼切換定高

程式用一個雙搖桿手勢切換 flag_bara（HeightPidControl() 開頭）：

  • 油門低 + 偏航右 + 橫滾左 + 俯仰前 → flag_bara = 0 → 開氣壓定高。
  • 油門低 + 偏航右 + 橫滾左 + 俯仰後 → flag_bara = 1 → 關（回手動油門）。

另外，若光流模組在線且測到的 mini.flow_High < 4000（約 4 公尺內），會優先用光流的測距高度（更準）， 否則才用氣壓高度——這就是第 2 章「哪個算法用哪個感測器」表裡寫的「定高用氣壓＋光流測距」。

7.5 動手驗證

  • 開上位機畫 Height.High：把飛機用手上下移動，看高度波形跟著變、放回原處又回來。
  • 看 Height.Speed：上移時為正、下移為負，靜止接近 0。
  • 安全場地試飛：起飛到一定高度後切定高，放開油門，看它會不會自己穩住高度（先低空、戴護目鏡）。

7.6 思考題

  • 把 τ 從 3.0 改成 1.5，三個 K 各變多少？這樣會更信任氣壓還是更信任加速度？飛起來可能更穩還是更抖？
  • 為什麼「定高只動平均油門、不影響姿態」？（回顧第 5 章混控）
  • 室內為什麼用光流的測距高度比氣壓準？（提示：氣壓對門窗開關、氣流很敏感）

7.7 本章對應的原始碼

檔案 看什麼
HAL/high_process.c Strapdown_INS_High()：高度三階互補濾波
HAL/control.c HeightPidControl()：高度串級 PID 與定高切換
HARDWARE/spl06.c SPL06 氣壓計驅動與高度換算

下一章：高度顧好了，水平方向怎麼不漂走？這就是 光流定點懸停。