跳轉到

第 7 章：氣壓定高

本章學習目標

  • 理解「氣壓計量高度」為什麼準又不準，要怎麼補救
  • 看懂高度的互補濾波融合（和第 3 章姿態同一個套路）
  • 看懂定高的串級：高度環 → 垂直速度環 → 油門
  • 一張圖看懂：高度來源怎麼自動切換、油門怎麼湊出來——沒有 ToF/光流也照樣定高

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
輸出 目標垂直速度 油門的增減量

實例：飛機被你壓低了 30 公分，它怎麼自己爬回去

假設定高目標是 150 cm，但一陣下洗氣流把它壓到 120 cm，當下垂直速度約 0。跟著程式走一圈（數字為示意）：

  1. 外環：高度差 = 150 − 120 = 30；乘 kp=2.0 → 輸出 60，這就是「請以 60（單位：內部速度）往上爬」的目標垂直速度。
  2. 內環：目標速度 60、現在 0，差 60；乘 kp=0.30 → 油門修正量 ≈ +18。
  3. 這 +18 加進基礎油門 → 四顆馬達同時多轉一點 → 飛機開始上升。
  4. 升到接近 150 cm 時，高度差 → 0、修正量 → 0，油門自動收回，停在 150 cm。

反過來：若飛機衝高到 170 cm，外環算出的是負的目標速度 → 內環給負修正量 → 馬達收油門 → 自己降回來。整個過程你完全沒碰油門桿。

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

7.4 一張圖看懂：高度來源自動切換 + 油門怎麼湊出來

這是很多人卡住的地方：「高度」到底是誰量的？沒有光流/ToF 還能定高嗎？ 答案是——能。 每個控制週期，程式都會照下面這張圖，自動挑一個可用的高度來源，再決定油門：

flowchart TD
    A["每個控制週期"] --> B{"定高開關 flag_bara<br/>(雙搖桿手勢切換)"}
    B -->|"關閉（手動油門）"| Z["不定高<br/>Control_high = 0"]
    B -->|"開啟（定高）"| C{"光流模組在線?<br/>mini.ok==1 且 雷射高度 < 4m"}
    C -->|"是：有光流雷射/ToF"| D["用光流的測距高度<br/>Control_high = mini.flow_High / 10"]
    C -->|"否：沒裝 / 壞了 / 超過 4m"| E["★ 退回氣壓高度 ★<br/>Control_high = bara_height"]
    D --> F["高度串級 PID<br/>高度環 → 速度環 → 油門修正量"]
    E --> F
    F --> G["油門 = 油門桿基礎油門<br/>＋ 定高修正量 pidHeightRate.out"]
    Z --> H["油門 = 油門桿直接輸出"]
    G --> M["四顆馬達一起增減"]
    H --> M

對應的程式（HAL/control.c 的 HeightPidControl() 開頭）：

HAL/control.c（高度來源選擇，註解翻成正體中文）
76
77
78
79
80
81
82
83
84
85
86
if (flag_bara == 0)                          // 定高開啟
{
    if (mini.ok && mini.flow_High < 4000)    // 光流模組正常、且測距高度 < 4 公尺
        Control_high = mini.flow_High / 10;  // → 優先用光流的「雷射測距高度」(較準)
    else
        Control_high = FlightData.High.bara_height;  // → 退回「氣壓高度」(SPL06)
}
else
{
    Control_high = 0;                        // 定高關閉，回手動油門
}

重點解讀（直接回答「沒有 ToF 會怎樣」）：

  • 這台的「ToF/雷射高度」不是獨立感測器，而是光流模組內建的雷射測距（mini.flow_High，從光流模組串列埠傳來）。
  • 雷射只是「低空（4 公尺內）更準的優先來源」。一旦沒裝光流、模組故障 mini.ok=0、或飛超過 4 公尺，程式就走 else，自動退回氣壓計。
  • 換句話說：氣壓計才是定高的後備底盤，永遠都在。拔掉光流模組，定高照常運作，只是低空精度差一點、室內容易上下漂個幾十公分。

兩種高度來源，分工一句話

氣壓計：到處都能用、但短期會抖（怕門窗氣流）。光流雷射：低空很準、但要對得到地面、超過 4m 就失效。 程式的策略就是「低空有雷射就用雷射，其餘一律靠氣壓」——所以有沒有 ToF 都能飛定高。

7.5 怎麼開關定高（搖桿手勢）

上圖最上面的 flag_bara 用一個雙搖桿手勢切換（HeightPidControl() 開頭）：

搖桿動作 flag_bara 效果
油門低 + 偏航右 + 橫滾左 + 俯仰前 1 關定高（回手動油門）
油門低 + 偏航右 + 橫滾左 + 俯仰後 0 開定高

小提醒：實際飛行時這台大多是「一直開著定高」的設計（多數飛行模式都把 height_lock 設為 1）， 油門桿在定高下變成「升／降速度微調」——往上推爬升、往下拉下降、放回中間就停住。

7.6 動手驗證

  • 開上位機畫 Height.High：把飛機用手上下移動，看高度波形跟著變、放回原處又回來。
  • 看 Height.Speed：上移時為正、下移為負，靜止接近 0。
  • 驗證自動退回氣壓：把光流模組鏡頭遮起來（或拔掉），看上位機 mini.ok 變 0，定高是否仍可用（這時走的就是氣壓高度）。
  • 安全場地試飛：起飛到一定高度後切定高，放開油門，看它會不會自己穩住高度（先低空、戴護目鏡）。

7.7 思考題

  • 把 τ 從 3.0 改成 1.5，三個 K 各變多少？這樣會更信任氣壓還是更信任加速度？飛起來可能更穩還是更抖？
  • 7.3 的實例若飛機衝高到 170 cm，外環、內環各算出什麼符號的值？馬達會怎麼動？
  • 為什麼「定高只動平均油門、不影響姿態」？（回顧第 5 章混控）
  • 飛到 4.5 公尺高空時，定高是用雷射還是氣壓？為什麼程式要設這個 4 公尺門檻？

7.8 本章對應的原始碼

檔案 看什麼
HAL/high_process.c Strapdown_INS_High()：高度三階互補濾波
HAL/control.c HeightPidControl()：高度來源自動切換、高度串級 PID、定高開關；MotorControl()：油門疊加
HARDWARE/spl06.c SPL06 氣壓計驅動與高度換算
HARDWARE/flow.c 光流模組（含 mini.flow_High 雷射測距高度）

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