無人機自學計畫：自動飛行 / 程式（純模擬）

目標

12 週內，在模擬器中用程式控制無人機完成自主任務（起飛、航點、感測器回饋、簡單視覺追蹤），並理解飛控與 MAVLink 的運作原理。

約束

• 預算 ≈ 0，不買硬體，全部在模擬中完成
• 環境：macOS（arm64 Apple Silicon）；Gazebo/PX4 在 macOS 較不穩，必要時改用 Docker 或 Linux VM
• 每週 5–8 小時；每週結束留一份筆記到 notes/week-NN/

工具選擇

路線圖

階段 1：基礎觀念（第 1–3 週）

• W1 飛行原理：四軸力學、roll/pitch/yaw/throttle、飛行模式（Stabilize/Loiter/Guided/RTL）
• W2 飛控架構：IMU、GPS、氣壓計、EKF、PID 控制迴路；ArduPilot 與 PX4 比較
• W3 環境建置：安裝 ArduPilot SITL + MAVProxy + QGroundControl，手動起飛降落
  • 產出：能在 QGC 看到虛擬無人機並下指令

階段 2：用程式飛（第 4–6 週）

• W4 MAVLink 協定：訊息結構、heartbeat、常見訊息（ATTITUDE、GLOBAL_POSITION_INT）；用 pymavlink 讀遙測
• W5 Guided 模式：arm、takeoff、goto、速度控制、降落（pymavlink 與 MAVSDK 各做一次）
• W6 任務與安全：上傳航點任務、geofence、failsafe、RTL
  • 專案 1：projects/waypoint-mission 自動飛正方形並回家

階段 3：感知與進階（第 7–9 週）

• W7 模擬器進階：Gazebo 世界、攝影機、距離感測器；ArduPilot/PX4 與 Gazebo 串接
• W8 電腦視覺：OpenCV 讀模擬相機畫面、顏色/ArUco 標記偵測
• W9 ROS 2 入門：節點、topic、MAVROS 或 px4_msgs
  • 專案 2：projects/aruco-landing 偵測標記並精準降落

階段 4：整合與延伸（第 10–12 週）

• W10 控制理論：調 PID、看 log（.bin/ULog）分析、參數調校
• W11 避障與路徑規劃：簡單 A*／potential field，在模擬中繞障
• W12 總結專案：projects/final 自選主題（目標追蹤、多機、搜索覆蓋路徑）
  • 產出：README + 示範錄影 + 心得

每週流程

1. 讀（官方文件為主，約 2 小時）
2. 做（實作與小實驗，約 3–4 小時）
3. 寫（在 notes/week-NN/README.md 記錄：學到什麼、卡在哪、下週問題）

資源

• ArduPilot Dev 文件：https://ardupilot.org/dev/
• PX4 文件：https://docs.px4.io/
• MAVLink 指南：https://mavlink.io/en/
• MAVSDK-Python：https://mavsdk.mavlink.io/
• QGroundControl：https://qgroundcontrol.com/
• Gazebo：https://gazebosim.org/

日後上實機

目前不涉及。若要飛實機，先查台灣民航局無人機註冊、操作證與禁限航區規定，再從 <250g 小機與開闊場地開始。

進度

• 階段 1（W1–3）：W1 飛行原理、W2 飛控架構（含 4 輪測驗與實作）、W3 環境建置皆完成
• 階段 2（W4–6）：W4 MAVLink 協定完成（實作 1–6）、W5 Guided 模式完成（實作 1–3，pymavlink 與 MAVSDK 各一次；實作 4 略過）、W6 任務與安全完成（實作 1–4 與專案 1；實作 5 略過）
• 階段 3（W7–9）
• 階段 4（W10–12）

目前位置（2026-10-13）：階段 2（W4–6）完成，專案 1 waypoint-mission 通過驗收；下一步是 W7 模擬器進階（Gazebo 世界、攝影機、距離感測器）。

W01 飛行原理

學到什麼

核心一句話：四個馬達轉速不同，機身就會傾斜或轉向，傾斜之後就往那個方向飛。

四個控制量

四個馬達中，兩個順時針、兩個逆時針，轉動力互相抵消，機身才不會自旋；要 yaw 時就是故意讓兩組轉速不同。編號與方向不必背，飛控會自己算。

飛行模式：飛控幫多少

口訣：Stabilize 穩姿勢 → AltHold 加定高 → Loiter 加定點 → Guided 換程式指揮。

其他名詞

• Arm / Disarm：解鎖／上鎖。沒 arm 前馬達不會轉。
• Telemetry（遙測）：無人機回傳的資料，如高度、姿態、電量。
• EKF：綜合多種感測器，估算位置與姿態的演算法（之後再深入）。
• GPS 衛星數：QGC 右上角的數字，越多定位越準。

實測紀錄（W3 時順便觀察）

• 判斷是否在空中，要看 EXTENDED_SYS_STATE.landed_state 與馬達輸出，不能只看單一高度數字。
• Stabilize 懸停時沒有定高；用 pymavlink 切 Land（模式編號 9）後，3 公尺約 15 秒平穩降到 0。

實驗：有風時比較 AltHold、Loiter、Stabilize

腳本：scripts/w1_modes.py（先執行 scripts/sitl.sh，QGC 用來觀察）。流程：Guided 起飛到 10 m → 開風 5 m/s → 用 RC override 把搖桿全放中間（油門 1500）→ 依序切模式，每秒記錄。

QGC 截圖（降落後）：紅線從 Home（綠色圖示）一路延伸到左側，離 Home 39.6 m，是 AltHold 與 Stabilize 兩段被風吹走的累積結果。左側 Vehicle Messages 的紅字 Arm: Throttle (RC3) is not neutral 就是解鎖被拒絕的訊息。

實驗中學到的

• 手控模式會讀虛擬遙控器：SITL 預設油門 1000（最低）。切到 AltHold 若沒人撥油門，會被當成「油門拉到底」而降落，看起來像墜毀，其實不是。Guided 不讀遙控器，所以起飛時沒事。
• RC override 必須持續發送（約 10 Hz），否則飛控視為遙控器斷線。
• 解鎖被拒絕：Arm: Throttle (RC3) is not neutral，油門 1000 或 1500 都一樣。QGC 的 Takeoff 用強制解鎖（MAV_CMD_COMPONENT_ARM_DISARM 的 param2 = 21196）繞過，腳本照做；只適合模擬，實機不要用。
• 腳本要檢查結果：第一版沒檢查解鎖是否成功、也沒有逾時，被拒絕後就一直空等；現在會檢查 COMMAND_ACK 並設 30 秒逾時。
• 輸出被緩衝：背景或管線執行 Python 時要加 -u，否則看不到進度。
• QGC Takeoff 的預設高度只有 3 m，不是 10 m。

卡在哪

• 容易搞混的詞：
  • arm 與 disarm（沒有 unarm）
  • RTL 是 Return To Launch，不是 to land
  • Stabilize 不會定高也不會定點，一度以為放開搖桿會停在原地

自我測驗

三輪共 15 題，得 11.5 分（✅ 1 分、⚠️ 0.5 分、❌ 0 分）。

第一輪（3/5）

第二輪（4.5/5）

第三輪（4/5）

需要加強

• 回答「為什麼」時要說出理由，不只是模式名稱（第三輪第 2 題）
• RTL 的行為：飛回起飛點，然後降落

補充：手控、電腦控、全自動

另有 Auto 模式：按事先上傳的航點任務自動飛，W6 會用到。

下週問題

• 實際在 QGC 切換 Stabilize、AltHold、Loiter，差別看起來是什麼樣子？
• 飛控內部的 PID 控制迴路怎麼讓機身保持水平？
• IMU、氣壓計、GPS 各自提供什麼資訊給 EKF？

W02 飛控架構

本週要知道

學完能回答：無人機為什麼能自己保持水平、定高、抗風？

1. 資料流：感測器 → 估算（EKF）→ 控制（PID）→ 馬達。
2. 四種感測器各測什麼：
   • IMU（陀螺儀 + 加速度計）：旋轉速度與加速度
   • 氣壓計：高度
   • GPS：位置
   • 磁力計：朝向
3. EKF 的用途：感測器都有雜訊，EKF 把它們融合成「我在哪、朝哪」的最佳估計。不用懂數學。
4. PID 的用途：誤差越大修正越多（P）、長期偏差慢慢消除（I）、抑制過衝（D）。
5. 控制是分層的：飛行模式決定目標 → 姿態控制算目標轉速 → 轉速控制（PID）算推力 → 馬達混控分給四個馬達。
6. W1 現象對應的原因：
   • AltHold 定高：氣壓計加高度 PID
   • Loiter 抗風：GPS 加位置 PID
   • Stabilize 不定高：沒有高度控制這一層
7. ArduPilot 與 PX4 同類：都是開源飛控，架構相近；本計畫用 ArduPilot。

兩條鏈：資料流與控制分層

兩條鏈講的是同一件事的不同縮放層級：第二條是第一條「控制」那一段的展開。

資料流（誰把什麼交給誰）
  感測器 ──▶ EKF ──▶ PID ──▶ 馬達
            估算      控制
         「我在哪、朝哪」  「該出多少力」
                       │
                       ▼ 放大「控制」這一段
控制分層（目標怎麼變成馬達指令）
  飛行模式 ──▶ 姿態控制 ──▶ 轉速控制（PID）──▶ 馬達混控 ──▶ PWM
  去哪裡      要傾斜多少    要轉多快           各馬達多少    送出訊號

• EKF 屬於「估算」，只出現在第一條，不在控制分層裡。
• 題目問「資料流」就從感測器答起；問「控制分層」就從飛行模式答起。

縮寫

可以跳過

• 原始碼函式名稱（如 angle_ef_roll_pitch_rate_ef_yaw()）
• 單位換算（centi-degree、cm/s）
• Kalman Filter 的數學推導
• PID 調校參數細節（W10 才用）

閱讀清單

必讀（約 1.5 小時）

1. EKF 概觀：https://ardupilot.org/copter/docs/common-apm-navigation-extended-kalman-filter-overview.html：看文字，知道它融合了哪些感測器就好
2. PID 調校入門：https://ardupilot.org/copter/docs/ac_rollpitchtuning.html：只看 P、I、D 各自的作用，參數先略過

選讀

• 姿態控制（開發者視角）：https://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html：只看「分層流水線」的概念，圖是原始碼呼叫路徑，看不懂正常
• 位置控制與導航：https://ardupilot.org/dev/docs/code-overview-copter-poscontrol-and-navigation.html
• PX4 控制器架構圖（對照用）：https://docs.px4.io/main/en/flight_stack/controller_diagrams.html

實作

1. 在 SITL 靜止時，用 pymavlink 讀 RAW_IMU、SCALED_PRESSURE、GPS_RAW_INT，印出原始感測器值
2. 再讀 ATTITUDE，比較原始值與 EKF 估算結果
3. 觀察 IMU 靜止時的雜訊大小（結果見「學到什麼」）

學到什麼

IMU 不等於陀螺儀

IMU（慣性測量單元）= 陀螺儀 + 加速度計。ArduPilot 通常把磁力計獨立計算。

兩者特性互補，EKF 把它們融合起來。

PID：以「升到 10 m 並停住」為例

• P（比例）：誤差越大，推力越大。
  • 問題：差 0.5 m 時，P 算出的推力剛好只夠撐住機身，誤差再縮小推力就不夠，所以停在差 0.5 m 的地方到不了目標（穩態誤差）；而且接近目標時速度仍快，會衝過頭、來回震盪。
• I（積分）：累積誤差。
  • 還差 0.5 m 時，I 項每一瞬間都多加一點，越積越大，直到推力夠、誤差變 0 才停止增加。此時 I 項的值就是「撐住機身的基本推力」。
  • 比喻：定速巡航上坡時速度掉了一點，I 會一直補油門，直到回到設定速度。
• D（微分）：看誤差變化有多快。
  • 無人機正快速接近目標，D 就反向出力，像提前點煞車，接近目標時自然減速，避免衝過頭。

調 PID 要三個一起平衡（W10 會做）。

用模擬看 PID：升到 10 m 並停住

X 軸是時間（秒），Y 軸是高度（公尺），只畫 10 m 附近（前面的爬升段省略）。

為什麼是 9.5 m：模型中懸停需要 50% 推力，P 的規則是「推力 = KP × 誤差」，KP = 1。穩定時推力要等於重力，所以 1 × 誤差 = 0.5，誤差 = 0.5 m，停在 10 − 0.5 = 9.5 m。9.5 這個數字沒有特殊意義，是 0.5 ÷ KP 的結果（KP = 2 → 9.75 m，KP = 10 → 9.95 m）；有意義的是「一定會有非零誤差」，這就是需要 I 項的原因。

假設與限制：這是自己寫的一維簡化模型（懸停推力 50%、有空氣阻力），不是 ArduPilot 的真實控制器；增益 P = 1、I = 0.5、D = 1.2 是為了讓三種行為都看得清楚而挑的。程式：scripts/pid_demo.py，用法 vendor/venv/bin/python -I scripts/pid_demo.py <輸出.png>，可自行改 KP、KI、KD 觀察。

用模擬看 P、I、D 的數字怎麼變：0 → 9.5 → 10 m

同一個一維模型與增益（P = 1、I = 0.5、D = 1.2），但這次三項一起開，並把三項各自的「數字」畫出來（1.0 = 100% 推力，不管單位）。灰色垂直線是高度剛過 9.5 m 的時刻（t = 4.9 秒）。

怎麼看這張圖：

• P（藍）：永遠等於「誤差 × 1」，從 10 一路平滑降到 0，跟高度曲線是鏡像。9.5 m 時誤差 0.5，P 就是 0.5。
• I（橘）：這裡幾乎是一條水平線，0.1 秒就升到 0.5 後不動。這就是前面「為什麼只有 P 會卡在 9.5 m」的答案：只有 P 時，要靠 P = 0.5 才撐得住機身，所以停在差 0.5 m；加了 I，由 I 先頂住 0.5，P 就可以降到 0，高度才能走完最後 0.5 m。
• D（青）：全程是負值（煞車），升得越快越負（約 −3.9），接近目標時回到 0。它不是在「幫忙升」，是在「防止衝過頭」。
• 實際推力（虛線）：P + I + D 的和，再夾在 0–1。前約 2 秒被夾在 1.0（飽和），此時三項加起來超過 1，多出來的部分被截掉；之後才由三項相加直接決定。

注意：

• 這張圖的積分上限是 ±1，I 在起飛 0.1 秒就頂到 0.5 的上限，所以看不到「9.5 → 10 m 之間 I 慢慢爬升」；想看 I 慢慢累積，見下一張圖（上限放寬到 ±2、情境改成載重改變）。
• 這是簡化模型，不是 ArduPilot 的真實數字。程式：scripts/pid_climb.py，用法 vendor/venv/bin/python -I scripts/pid_climb.py <輸出.png>。

用模擬看 I「累積」是什麼意思

情境：無人機本來穩穩懸停在 10 m（I 單獨撐住 50% 推力）；t = 2 秒時突然多了載重，需要的推力變成 60%。三項各自貢獻多少「推力」，全部畫在同一個單位（1.0 = 100%）。

重點：

• I 的累積量不會消失。誤差回到 0 後，I 保持在 60%，由它長期撐住載重；P 和 D 都回到 0。
• 這就是 W2「PID 一直都在運作、沒有介入時間點」的實際樣子：三條線從頭到尾都在算，只是各自的份量不同。
• 反應速度排序：D 最快（立刻），P 次之，I 最慢（要累積）。

注意：前一張圖（升到 10 m）的模擬把積分上限設為 ±1，I 項在起飛後 0.1 秒就頂到上限 0.5，剛好等於所需的懸停推力，所以看不出「慢慢累積」；這張圖把上限放寬到 ±2，並用「載重改變」這個情境，才看得到 I 逐步累積。程式：scripts/pid_terms.py，用法 vendor/venv/bin/python -I scripts/pid_terms.py <輸出.png>。

實作：讀原始感測器值

腳本 scripts/w2_sensors.py（先執行 scripts/sitl.sh）：SITL 靜止時各取 50 筆。

• 預設的 SITL 感測器幾乎沒有雜訊（加速度計 z 標準差 0.003 m/s²），看不出 EKF 的作用。
• GPS fix_type 回報 6（RTK 固定解），是 SITL 的模擬值；真實 GPS 通常是 3（3D 定位）。

實作：開雜訊，比較原始值與 EKF

腳本 scripts/w2_noise.py：起飛到 10 m 懸停，開啟模擬的 IMU 振動，比較「只用加速度計算的 roll」與 EKF 估算的 roll。加 --hold 則帶著雜訊一直懸停，給 QGC 觀察，Ctrl-C 結束並降落。

QGC 的 Analyze Tools → MAVLink Inspector：

• 上圖：RAW_IMU 的 xacc、yacc 約 ±150 mG（單位是毫 g，1000 mG = 1 g），zacc 在 −1000 mG（重力）附近抖。若單看加速度計，相當於機身傾斜約 ±8°。
• 下圖：ATTITUDE 的 roll、pitch 約 ±0.003 rad（約 ±0.2°），是平滑的慢波。
• 機身其實幾乎沒動，晃的只是馬達振動；EKF 靠陀螺儀（短時間準）把振動過濾掉，大約縮小到 1/40。這就是「融合」的實例。
• 兩張圖單位不同（mG vs 弧度），要換算才能比；QGC 只以 3～10 Hz 顯示，而振動是 50～70 Hz，所以鋸齒的形狀是取樣造成的，只看抖動幅度。

踩到的坑：

• 參數名稱是 SIM_ACC1_RND、SIM_GYR1_RND（含 IMU 編號），不是 SIM_ACC_RND；先查原始碼 libraries/SITL/SITL.cpp 確認。
• 雜訊只在馬達轉動時才有，必須在空中懸停；陀螺儀雜訊還會乘上油門。
• 加速度計的雜訊要 SIM_VIB_FREQ_X/Y/Z 非 0 才會加進去；只開 ACC1_RND 沒有效果。
• SITL 的 TCP 連線如果不讀封包，緩衝區塞滿後會卡住整個模擬（SITL CPU 掉到 1%，QGC 所有訊息變 0 Hz）。懸停等待時也要持續 recv_match()。
• 腳本用 MAV_CMD_DO_SET_MODE 切 Land；結束時一定要把 SIM_* 參數歸零。

姿態控制頁面的圖看不懂是正常的

那張圖畫的是原始碼的函式呼叫路徑，給開發者看，不是概念圖。只要記住分層流水線：飛行模式 → 姿態控制 → 轉速控制（PID）→ 馬達混控 → PWM。

卡在哪

• 姿態控制頁面的圖表看不懂（已確認可跳過）
• 陀螺儀與加速度計的可靠時段容易記反（見自我測驗第一輪第 5 題）

自我測驗

第一輪（3/5）

✅ 1 分、⚠️ 0.5 分、❌ 0 分。

第二輪（2.5/5）

需要加強：EKF 屬於「估算」，PID 屬於「控制」，兩者不同層；回答「為什麼」要說出機制，不只現象。

第三輪（4/5）

需要加強：D 看的是「變化速度」，不是「誤差大小」；P 看大小、I 看累積、D 看變化。

第四輪（2.5/5）

需要加強：實作中的細節（第 3、4 題）沒有留下印象，回頭看「實作：開雜訊」那節的圖與「踩到的坑」。

下週問題

• W4：我對 RAW_IMU 要求 20 Hz，QGC 的 MAVLink Inspector 卻顯示 4 Hz；誰決定實際速率？多個連線要的速率會互相覆蓋嗎？
• W4：COMMAND_ACK 的 result 有哪些值？（實測：0 = 成功；對已在空中的無人機下 takeoff 得到 4）
• W4：TCP 連線不讀封包就卡住整個 SITL，MAVLink 本身有沒有流量控制，還是只能自己持續讀？
• W4：訊息欄位的單位為什麼這麼雜（毫 g、毫弧度／秒、經緯度 ×1e7、公分）？去哪查？

W03 環境建置

學到什麼

• 架構：你的程式／QGC／MAVProxy 透過 MAVLink 與飛控溝通；SITL 是把 ArduPilot 編成能在電腦上跑的程式，並模擬感測器與物理。
• 連接埠：SITL 的 TCP 5760、5762 給程式連；QGC 預設聽 UDP 14550，所以中間要用 MAVProxy 轉送（--out udp:127.0.0.1:14550）。
• 模式：Stabilize 不會定高，要降落用 Land；MAVLink 切模式可用 set_mode_send（Land = 9）。

安裝（全部在專案內，vendor/ 已 gitignore）

步驟：

1. git clone --depth 1 --branch Copter-4.7.1 --recurse-submodules --shallow-submodules ArduPilot 到 vendor/
2. uv venv vendor/venv，裝 empy==3.3.4 pexpect future setuptools wheel numpy pymavlink MAVProxy gnureadline
3. python modules/waf/waf-light configure --board sitl，再 waf-light copter
4. 從 GitHub release 下載 QGC 的 dmg，把 .app 複製到 vendor/

不使用官方 install-prereqs-mac.sh：它會用 sudo 並修改 shell 設定。

啟動

scripts/sitl.sh                      # SITL + 轉送到 14550
open vendor/QGroundControl.app       # 會自動連線

驗證：QGC 左上角顯示綠色 Ready 與 Stabilize，地圖出現在 Canberra 附近。按 Takeoff 後可升空；用 pymavlink 切 Land 後平穩降落。

卡關與解法

• 連結失敗 ld: pointer not aligned in AP_FWVersion::fwver：新版 Xcode linker 不接受 PACKED 類別內的指標。-ld_classic 已被移除，無效。解法：在 libraries/AP_Common/AP_FWVersion.h 讓 __APPLE__ 不套用 PACKED（只改 vendor，SITL 不需要該結構的封裝）。
• MAVProxy 啟動失敗 No module named 'gnureadline'：在 venv 補裝。
• QGC 顯示 Disconnected：用 --no-mavproxy 時只有 TCP，沒有 UDP 14550，需另外啟動 MAVProxy 轉送。
• macOS 沒有 timeout 指令（需 coreutils）：用背景執行加 pkill 取代。
• 我一度誤判：從 GLOBAL_POSITION_INT 的高度 3 公尺判斷它在地面，實際上 landed_state 是 IN_AIR。判斷是否在空中，應看 EXTENDED_SYS_STATE.landed_state 與馬達輸出，不要只看單一訊息。

下週問題

• W4：怎麼用 pymavlink 持續接收 ATTITUDE、GLOBAL_POSITION_INT？request_data_stream_send 與 SET_MESSAGE_INTERVAL 差在哪？
• 5760 只允許一個連線時，多個程式怎麼同時連？

W04 MAVLink 協定

本週要知道

學完能回答：程式怎麼跟無人機講話？要資料、下指令各用什麼訊息？

1. MAVLink 是序列協定：不管底層是 TCP、UDP 或序列埠，封包都是「起始標記、長度、序號、系統 ID、元件 ID、訊息 ID、資料、校驗碼」。MAVLink 2 最長 280 位元組，可簽章。
2. HEARTBEAT 每秒 1 次：用來發現對方存在，也用來判斷對方是否斷線。
3. 要資料有兩種方式：
   • REQUEST_DATA_STREAM：舊式，一次要一整群訊息
   • SET_MESSAGE_INTERVAL（COMMAND_LONG 指令 511）：ArduPilot 4.0 以上，單一訊息、指定間隔（微秒）
   • 另有 REQUEST_MESSAGE（指令 512）：只要一次
4. 下指令用 COMMAND_LONG，結果看 COMMAND_ACK：W1 的強制解鎖、W2 的起飛都是這條路。
5. 常見訊息：ATTITUDE、GLOBAL_POSITION_INT、LOCAL_POSITION_NED、GPS_RAW_INT、SYS_STATUS、EXTENDED_SYS_STATE。W1、W2 已經碰過其中幾個。
6. 連線埠：SITL 的 5760／5762／5763 各自獨立；QGC 走 MAVProxy 轉送的 UDP 14550。
7. 實務：連線要持續讀取，否則 SITL 會被卡住（W2 踩過；本週只堆了約 4 KB，沒重現卡住）。有要求但沒讀的資料不會被丟掉，會堆在緩衝區，恢復讀取時一次收到舊資料（見「學到什麼」的實作 5）。

可以跳過

• 校驗碼與序列化細節（serialization 那頁）
• 訊息簽章
• 自訂訊息與 XML 定義

閱讀清單

必讀

1. MAVLink 基礎：https://ardupilot.org/dev/docs/mavlink-basics.html：封包結構與訊息流程
2. 向 ArduPilot 要資料：https://ardupilot.org/dev/docs/mavlink-requesting-data.html：REQUEST_DATA_STREAM 與 SET_MESSAGE_INTERVAL 的差別，W3 留下的問題在這頁

選讀

• HEARTBEAT 協定：https://mavlink.io/en/services/heartbeat.html
• 標準訊息一覽：https://mavlink.io/en/messages/index.html：查欄位與單位
• 序列化（對照用，看不懂正常）：https://mavlink.io/en/guide/serialization.html

實作

1. 用 pymavlink 持續接收 ATTITUDE 與 GLOBAL_POSITION_INT，印出頻率與值（腳本：scripts/w4_telemetry.py）
2. 比較 REQUEST_DATA_STREAM 與 SET_MESSAGE_INTERVAL 實際得到的速率（腳本：scripts/w4_rates.py）
3. 同時開兩個程式，分別連 5762 與 5763（W3 留下的問題：5760 只允許一個連線時，多個程式怎麼同時連？）（腳本：scripts/w4_two_ports.py）
4. 暫停（SIGSTOP）或殺掉 SITL，觀察 HEARTBEAT 斷線時程式怎麼偵測（逾時）（腳本：scripts/w4_heartbeat.py）
5. （額外）有要求但暫停讀取、或沒要求時，資料會被丟掉還是堆積？（腳本：scripts/w4_backlog.py）
6. （額外）REQUEST_MESSAGE 只要一次，實際行為是什麼？（腳本：scripts/w4_request_message.py）

學到什麼

概念：用 WebSocket 類比 MAVLink

MAVLink 像一條雙向長連線（WebSocket），上面混著兩種流量：

COMMAND_LONG 與 HTTP request / response 不同的地方：

• 沒有 request ID：ACK 靠 ack.command 欄位對應是哪個指令，不是靠順序。
• ACK 混在遙測裡：同一條連線上一直有 ATTITUDE 等資料湧來，所以要用 recv_match(type="COMMAND_ACK") 過濾。腳本是「發一個、等下一個 ACK」，同時有多個未回應的指令就可能對錯。
• 結果有狀態：ACCEPTED、DENIED、IN_PROGRESS（起飛這種要一陣子的指令，之後還會再回最終結果）。
• 協定沒有內建逾時或重試，要自己寫。
• 不是所有指令都有 ACK：REQUEST_DATA_STREAM 沒有回應，只能看之後有沒有資料來；PARAM_SET 的回應是 PARAM_VALUE；SET_MODE 要看 HEARTBEAT 的模式有沒有變。後三項是一般知識，沒有逐一實測。

封包結構（標頭、序號、校驗碼）不用背，pymavlink 會處理；只要記得 system ID（哪台機器）、component ID（機器上的哪個零件）、message ID（哪種訊息）三個欄位，對應程式裡的 m.target_system、m.target_component、MAVLINK_MSG_ID_*。

實作 1：不要就不送

連上 5762 後，SITL 重啟後的 4 秒內只收到 5 個 HEARTBEAT 和 1 個 TIMESYNC，沒有 ATTITUDE、也沒有 GLOBAL_POSITION_INT。要資料必須先「要」。用 SET_MESSAGE_INTERVAL 要 ATTITUDE 10 Hz、GLOBAL_POSITION_INT 5 Hz 後，實測是 10.0 Hz 與 4.0–5.5 Hz。

實作 2：兩種要法的行為

同一條連線依序下指令，每階段量 4 秒（每種訊息的 Hz）：

• REQUEST_DATA_STREAM 以群組為單位：POSITION 一個指令同時帶出 GLOBAL_POSITION_INT 和 LOCAL_POSITION_NED；SET_MESSAGE_INTERVAL 只動單一訊息（階段 5 後 LOCAL_POSITION_NED 沒被影響）。
• 後下的指令蓋過先前的（階段 3：ATTITUDE 從 22.5 掉到 11.2），沒有取較大值或合併。要精準控制單一訊息，用 SET_MESSAGE_INTERVAL。
• 設定留在 SITL 的連接埠上，斷線重連也不會消失：沒重設就重跑，階段 0 會殘留上次的速率。重設方法：REQUEST_DATA_STREAM 的 MAV_DATA_STREAM_ALL 速率設 0，並把各訊息的 SET_MESSAGE_INTERVAL 設 0（還原預設）。階段 0 仍有約 0.2 Hz（4 秒 1 筆），推測是重設瞬間漏出的訊息，沒有進一步確認。
• JS 類比：飛控內部有一張「訊息 → 送出間隔」的 Map，每個連接埠一份；SET_MESSAGE_INTERVAL 是 map.set(key, v)，REQUEST_DATA_STREAM 是對整組 key 迴圈 set；後寫的贏，而且狀態存在 server 端，不是 socket 上。
• ATTITUDE 實測比設定高約 12%（4 → 4.5、10 → 11.2、20 → 22.5，實作 3 的 5 → 5.7～6.0 也一樣）：原因未確認。已排除兩件事：不是 SITL 時鐘比真實時間快（飛控時間與牆鐘的比值是 1.000），也不是全部訊息都這樣（SYSTEM_TIME 要 10 Hz，實測約 10.2 Hz）。所以是 ATTITUDE 這類訊息的送出排程造成的，細節沒有再追。
• LOCAL_POSITION_NED 在 SITL 剛開機時是 0，放了一陣子後才出現（階段 4 的 2.0 Hz）。推測是 EKF 還沒有位置原點，但沒有直接檢查 EKF 狀態，只是佐證。實驗前先讓 SITL 開一陣子。

實作 3：同時連 5762 與 5763

同一個程式開兩條連線（A 連 5762、B 連 5763；對 SITL 來說就是兩個 TCP 連線，效果等同兩個程式），各自設定 ATTITUDE 的速率，同時讀取：

• 每個連接埠各有一份速率設定：A 改速率不影響 B（階段 3），B 停止也不影響 A（階段 4）。實作 2 的「後下的蓋過先前的」只在同一個連接埠內成立。這也回答了 W2 的問題：QGC 走另一個連接埠，所以我要 20 Hz，QGC 顯示 4 Hz 是它自己的設定，不是被我蓋掉（QGC 一側的實際原因沒有驗證）。
• COMMAND_ACK 只回給下指令的那條連線：另一條連線不會收到，所以前面「ACK 混在遙測裡」只需要過濾自己這條連線。
• 同一個連接埠只服務一個客戶端：A 佔著 5762 時，第二次連 5762 沒有收到 HEARTBEAT（TCP 連得上，但沒有資料）；5760 被 MAVProxy 佔用，連 5760 也一樣。
• 回答 W3 的問題：5760 被 MAVProxy 用了，你的程式各自連 5762、5763，一個埠一個程式；需要更多就要用 MAVProxy 的 --out 轉送出更多埠（UDP），這個沒有實測。
• JS 類比：每個連接埠像一個獨立的 server instance，各有自己的訂閱表，每個只接受一個 client；不是同一個 server 上的多個 session 共用一份狀態。

實作 4：HEARTBEAT 斷線偵測

偵測方法：超過 3 秒（TIMEOUT）沒收到飛控的 HEARTBEAT 就判定斷線。用兩種方式讓飛控「消失」：

• 斷線偵測只能靠逾時。recv_match 沒資料時回傳 None，跟「連線已經關閉」回傳的結果一樣，單看回傳值分不出來。偵測延遲介於 TIMEOUT − 1 到 TIMEOUT 秒之間（取決於斷線時離上次 HEARTBEAT 多久）。
• 凍結比殺掉難偵測：凍結時 TCP 看起來一切正常（沒有 EOF、沒有錯誤），只有逾時能發現；殺掉至少連線層會知道。
• 連線關閉後 pymavlink 會空轉：recv_match(blocking=True, timeout=0.5) 不再阻塞，10 秒內狂印了約 290 萬行 EOF on TCP socket、吃滿一顆 CPU。實務上偵測到斷線後要立刻關掉連線或停止迴圈，不能繼續呼叫 recv_match。
• 連線上有兩個 HEARTBEAT 來源：飛控（system 1，type 2 = 四旋翼）和一個 GCS（system 255，type 6），各約 1 Hz。GCS 那個推測是 MAVProxy 發的、由飛控轉送到這個埠（凍結期間它也跟著消失，支持這個推測，但沒有直接確認來源）。偵測斷線要過濾掉 GCS 的，否則只要地面站活著，就可能讓你誤判飛控還活著。
• 凍結恢復後的 HEARTBEAT 爆量，是 GCS 的、不是飛控的：未過濾時，恢復後 1 秒內收到 9 個；過濾後飛控自己只有 1 個，GCS 的約 8 個。這 8 個是 MAVProxy 在凍結的 8 秒裡送給飛控、堆在緩衝區裡的，飛控恢復後一次讀完再轉送出來；飛控自己的 HEARTBEAT 在凍結時根本沒有產生，所以不會補送。這正好呼應實作 5：沒產生的不會補，已經送出但沒人讀的會堆積。
• JS 類比：WebSocket 的 ping / pong 逾時。網路層沒掉時（凍結），onclose 不會觸發，只能靠應用層心跳；掉了（殺掉）會收到 onclose，pymavlink 卻不會告訴你，只會空轉。
• 沒釐清的：w4_telemetry.py 印出 component=0，可能跟 wait_heartbeat 先收到哪個 HEARTBEAT 有關，沒有確認。

實作 5：沒被讀取的資料去哪了

要 ATTITUDE 20 Hz，暫停讀取 5 秒再讀取；另外也測了「沒要求」5 秒後再要求。

• 有要求但不讀：不會丟，會堆積。141 ≈ 堆積的約 100 筆（5 秒 × 20 Hz）加上之後即時的約 40 筆。第一筆緊接著暫停前的最後一筆（只差 0.05 秒，剛好是 20 Hz 的一個間隔），中間沒有缺口，所以拿到的是一批舊資料，不是只剩最新的。TCP 是可靠傳輸，資料留在作業系統的緩衝區，恢復讀取時一次吐出來。
• 沒要求：什麼都沒有。飛控不產生這些訊息，沒有東西在排隊，所以之後要求時是從那一刻開始送，不會補送。
• JS 類比：沒人 subscribe 就不 emit（B）；有 subscribe 但沒人消費 stream，資料堆在內部緩衝區，直到你開始讀才一口氣流出來（A）。
• 實務：讀取程式如果中途停了一陣子，恢復後要預期會先收到舊資料；要拿「現在」的值，需要把緩衝區讀到空，或看 time_boot_ms 判斷新舊。
• 沒測到的：5 秒 × 20 Hz 只有約 4 KB，遠小於緩衝區，所以沒有重現 W2 的「SITL 被卡住」。緩衝區塞滿後 SITL 內部怎麼反應，沒有驗證；UDP 是否直接丟包，也沒有測。

實作 6：REQUEST_MESSAGE 只要一次

先把 ATTITUDE 停掉（沒要求時 3 秒內 0 筆），再各送一次 REQUEST_MESSAGE（指令 512）：

• 一次就是一次：沒有持續推送，不需要再停掉；想要更新就再送一次，像單次的 fetch。
• 適合查靜態資訊：固件版本、電池資訊這類不常變的東西，不必為了它開串流。
• 指令有 ACK，資料另外到：ACK 只代表「飛控接受了這個請求」，要的訊息是另一個封包，要自己用 recv_match 收。

卡在哪

• 第一次跑 w4_telemetry.py 什麼都沒印：5762 預設只送 HEARTBEAT，要先用 SET_MESSAGE_INTERVAL 要求才有 ATTITUDE。
• 實驗 2 階段 0 不乾淨：上次實驗留下的速率表還在（設定存在飛控、斷線不會清），所以每次開頭都要 reset()。
• HEARTBEAT 計數偏高：GCS（system 255）的 HEARTBEAT 也會經飛控轉送到連線上，要用 type 過濾。
• kill 模式輸出 49 MB：連線關閉後 pymavlink 空轉狂印 EOF on TCP socket，要用 grep -v 濾掉。
• 沒解開的疑問：
  • ATTITUDE 實測比要求的速率高約 12%（SITL 時鐘比 1.000，原因不明）。
  • QGC 顯示 4 Hz 的原因。
  • 緩衝區滿了 SITL 會不會卡住（只堆了約 4 KB，沒測到）。
  • 剛開機的 SITL 上 LOCAL_POSITION_NED 為 0（推測是 EKF 還沒設定原點，未驗證）。
  • w4_telemetry.py 印出 component=0 的原因。

下週問題

• W5 Guided 模式：arm、takeoff、goto 各自要送哪個 MAVLink 指令？怎麼確認指令真的被接受、執行完了（COMMAND_ACK 之外還要看什麼）？
• 速度控制（SET_POSITION_TARGET_*）的座標系與單位是什麼？
• 同時有 pymavlink 與 MAVSDK 兩個程式連線時，埠要怎麼分？

W05 Guided 模式

本週要知道

學完能回答：怎麼用程式讓無人機自己起飛、飛到指定位置、降落？

1. GUIDED 是「由程式指揮」的模式：飛控自己穩住姿態，程式只給目標（位置或速度），不用管馬達。
2. 固定流程：切 GUIDED → arm → takeoff → 給目標 → 降落。每一步都要確認條件成立（COMMAND_ACK、高度、距離）再做下一步，不能靠 sleep。
3. arm 會被拒絕：EKF 或 GPS 還沒準備好時 pre-arm 檢查不過，要重試或看 STATUSTEXT。
4. takeoff 是指令、goto 是目標訊息：
   • 起飛：COMMAND_LONG（MAV_CMD_NAV_TAKEOFF）
   • 位置／速度：SET_POSITION_TARGET_LOCAL_NED（或 GLOBAL_INT），用 type_mask 選要控制哪些欄位
5. 座標系：local NED 是 x 北、y 東、z 向下（高度要給負數）；全域用緯度、經度、相對高度。
6. 速度控制要持續發送：速度指令約 3 秒（GUID_TIMEOUT）沒更新就失效，飛機才會停下來（類似 W1 的 RC override）。
7. 降落：切 LAND 模式，落地後飛控自動 disarm。

可以跳過

• 姿態目標（SET_ATTITUDE_TARGET）：下層控制，之後再說
• 各種 MAV_FRAME 的細節，先用 LOCAL_NED 與 GLOBAL_RELATIVE_ALT_INT 兩種

閱讀清單

必讀

1. Guided 模式（Copter）：https://ardupilot.org/copter/docs/ac2_guidedmode.html
2. 以 MAVLink 控制 Copter：https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html：takeoff、位置、速度指令與 type_mask

選讀

• MAVSDK-Python：https://mavsdk.mavlink.io/main/en/python/quickstart.html：實作 3 用
• SET_POSITION_TARGET_LOCAL_NED：https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED

實作

1. pymavlink 完成「GUIDED → arm → takeoff 10 m → goto 北 20 m、東 20 m → 降落」（腳本：scripts/w5_guided_basic.py）
2. 用速度控制飛一個正方形，並觀察停止發送速度指令後飛機的反應（腳本：scripts/w5_velocity_square.py；加參數 timeout 做後者）
3. 用 MAVSDK-Python 重做實作 1，比較兩者的寫法（PLAN 要求兩種各做一次）（腳本：scripts/w5_mavsdk_basic.py；要先關掉 QGC）
4. （額外）故意在 arm 前不等 EKF，觀察被拒絕的原因

學到什麼

實作 1：pymavlink 基本流程

流程是「切 GUIDED → arm → takeoff → goto → LAND」，每步都等條件成立：

• 指令的 ACK 只代表「收到」，不代表「做完」：回答 W4 留下的問題，完成與否要看遙測（高度、距離）。
• goto 沒有 ACK：SET_POSITION_TARGET_* 是目標訊息不是指令，送出去就沒下文，到沒到要自己看位置。
• arm 會被拒絕：SITL 剛開機 EKF 還沒好時，約 20 秒內都 result=4，要重試。拒絕原因在 STATUSTEXT（實測在 sitl.sh 的輸出裡看到 Arm: Throttle (RC3) is not neutral；腳本本身沒有印 STATUSTEXT），ACK 本身只說 FAILED。
• QGC 會干擾：QGC 的虛擬搖桿開著時，油門不在中位，arm 一直被拒絕；關掉虛擬搖桿後 1 秒就 arm 成功。
• goto 是直線飛過去：軌跡是 Home 到目標的一條直線（對照 W6 任務的圓角路徑），落地點離 Home 28.6 m（QGC 左下），與 √(20² + 20²) ≈ 28.3 m 大致吻合，約差 0.3 m；飛行距離顯示 28.8 m（QGC 右下，水平累計）。

實作 2：速度控制

SET_POSITION_TARGET_LOCAL_NED 的 type_mask 決定哪些欄位有效：用速度就忽略位置、加速度、yaw、yaw 速率。座標系 local NED：x 北、y 東、z 向下（往上要給負的）。

• 要持續發送：腳本以 10 Hz 送。timeout 模式停止發送後，飛機繼續以 2 m/s 飛了約 3.5 秒才停（多滑行約 7 m），符合 GUID_TIMEOUT 預設 3 秒。
• 轉角會減速：每換一邊地速掉到約 0.5 m/s 再回到 2 m/s，因為指令從 (2, 0) 直接跳到 (0, 2)，飛控受加速度上限限制。正方形終點與起點偏差約 1 m。
• 想不減速：把速度向量的方向逐步旋轉，或放寬加速度參數（任務模式的是 WP_ACC、轉角加速度是 WP_ACC_CNR，見 ArduPilot 文件；Guided 速度模式用哪個沒有驗證）；更合適的是 W6 的任務與 spline 航點。
• type_mask 的位元值不要手算：一開始寫的 0b0000_1111_1111_1000 多設了 bit 9（FORCE_SET），改成用 POSITION_TARGET_TYPEMASK_*_IGNORE 常數組合才精確。

實作 3：MAVSDK-Python 4.0

同一流程用 MAVSDK 重做，約 34 秒跑完：

• 基本動作 MAVSDK 好寫很多，但把細節包起來了：看不到 result=4 或 STATUSTEXT，低階控制（type_mask、任意訊息、SET_MESSAGE_INTERVAL）也較弱。
• 4.0 與 3.x 不同：內嵌原生函式庫、不再需要 mavsdk_server 與 grpcio，import 路徑是 mavsdk.asyncio；udp:// 已棄用，要用 udpin://。
• JS 類比：pymavlink 像直接寫 fetch 加手動處理回應，MAVSDK 像一層有型別的 SDK client。

卡在哪

• arm 一直被拒絕：先是 EKF 還沒好（等約 20 秒），後來是 QGC 虛擬搖桿讓 RC3 不在中位；COMMAND_ACK 只說 FAILED，原因要看 STATUSTEXT。
• MAVSDK 連不上：QGC 還開著會搶 14550，要先關掉；用 udp:// 會有棄用警告。
• 用高度判斷落地不可靠：飛機已落地並 disarm，relative_alt 仍讀到 1.17 m（相對 home 的值會漂移），< 0.2 m 的條件逾時；改成等飛控 disarm。
• 等 disarm 被 GCS 的 HEARTBEAT 誤觸：連線上也有 system 255 的 HEARTBEAT（沒有 ARMED 旗標），第一版沒過濾，腳本在降落開始 0.6 秒就誤判完成；要只看 get_srcSystem() == m.target_system。同樣的問題 W4 實作 4 就提過。
• goto 馬上「完成」：目標是相對 EKF 原點的 (20, 20)，連續多次不重啟 SITL 時，飛機降落點已經在 (20, 20)，所以一開始就在目標上。要重現飛行過程需重啟 SITL。
• type_mask 的位元寫錯：手算的值多了 FORCE_SET，實測雖然能飛，但不精確。
• 沒解開的疑問：
  • Guided 速度模式的加速度上限到底是哪個參數（任務模式是 WP_ACC，這個沒驗證；原本猜的 WPNAV_ACCEL 是舊版名稱）。
  • MAVSDK 的 offboard 速度控制在 ArduPilot 上能不能用（沒測）。
  • 實作 4（arm 被拒絕的 PreArm 原因）沒做，只在實作 1 碰到兩種。

下週問題

• W6 上傳航點任務：MAVLink 的任務協定（MISSION_COUNT、MISSION_REQUEST_INT、MISSION_ACK）流程是什麼？跟 MAVSDK 的 mission 外掛差多少？
• 任務模式能不能不減速過彎（spline 航點、WP_RADIUS_M）？
• geofence 與 failsafe（RTL、電池、斷線）各自怎麼設定、怎麼觸發？
• 同時開 pymavlink 與 MAVSDK 時，埠要怎麼分（MAVSDK 搶 14550，QGC 也要用）？

W06 任務與安全

本週要知道

學完能回答：怎麼把一串航點交給無人機自己飛完？飛出範圍或出狀況時，飛控怎麼自保？

1. 任務是一份「清單」存在飛控裡：用 AUTO 模式執行，不是你一直發指令。W5 的 Guided 是「你指揮每一步」，W6 的任務是「先交代好、再放手」。
2. 上傳任務有固定協定（MAVLink mission protocol）：
   • 你送 MISSION_COUNT（共幾項）
   • 飛控逐項回 MISSION_REQUEST_INT（要第幾項；SITL 實測 7 次要項目全都是舊版 MISSION_REQUEST，回 MISSION_ITEM_INT 也被接受）
   • 你回 MISSION_ITEM_INT（該項內容）
   • 全部收完，飛控回 MISSION_ACK
   • 跟 W4 的 COMMAND_LONG 不同：這是飛控主動來要，你被動回應
3. 任務項目 = 指令 + 參數 + 座標：NAV_TAKEOFF、NAV_WAYPOINT、NAV_RETURN_TO_LAUNCH、NAV_LAND 等；第 0 項在 ArduPilot 是 home，不能當航點用（需查文件確認細節）。數量上限取決於飛控的儲存空間：SITL 手動實測（沒有留下腳本）MISSION_COUNT 650 可以、651 起回 MISSION_ACK type 4（NO_SPACE）。
4. 航點之間會減速轉彎還是順過：一般航點到達判定看 WP_RADIUS_M（速度 WP_SPD、加速度 WP_ACC、轉角加速度 WP_ACC_CNR）；spline 航點（NAV_SPLINE_WAYPOINT）才會平滑過彎。這是 W5 速度控制轉角減速的答案。
5. Geofence（地理圍欄）：用一組 FENCE_* 參數設定圓柱範圍（半徑、高度上限）與越界動作（RTL、降落…）；越界時飛控自己處理，不靠你的程式。
6. Failsafe（故障保護）：遙控器斷線、地面站斷線、電池低電量、EKF 失效等情況各有參數決定動作，常見是 RTL 或 LAND。
7. RTL（Return To Launch）：回到起飛點上空（先爬到 RTL_ALT_M），再降落；也是常見的 failsafe 動作（要自己設，例如 BATT_FS_LOW_ACT 預設是 0，不會動作）。
8. 安全機制是「最後防線」：先在模擬器故意觸發一次，看到它真的動作，才算理解。

可以跳過

• 任務項目的條件與事件指令（CONDITION_*、DO_* 大全）：用到再查
• Rally point（備降點）與 ArduPilot 的 mission 重新排序細節
• 固定翼與 VTOL 的 failsafe

閱讀清單

必讀

1. MAVLink 任務協定：https://mavlink.io/en/services/mission.html：上傳／下載流程與狀態
2. Auto 模式：https://ardupilot.org/copter/docs/auto-mode.html
3. 任務指令一覽（ArduPilot 支援的）：https://ardupilot.org/copter/docs/common-mavlink-mission-command-messages-mav_cmd.html
4. RTL 模式：https://ardupilot.org/copter/docs/rtl-mode.html
5. Failsafe 總覽：https://ardupilot.org/copter/docs/failsafe-landing-page.html

選讀

• Geofence：https://ardupilot.org/copter/docs/common-geofencing-landing-page.html
• 簡易圍欄設定：https://ardupilot.org/copter/docs/common-ac2_simple_geofence.html
• 電池 failsafe：https://ardupilot.org/copter/docs/failsafe-battery.html
• 地面站 failsafe：https://ardupilot.org/copter/docs/gcs-failsafe.html
• MAVSDK 任務（C++ 指南，Python 用法對照）：https://mavsdk.mavlink.io/main/en/cpp/guide/missions.html

實作

1. pymavlink 上傳一個正方形任務（起飛 → 四個航點 → RTL），切 AUTO 執行並觀察進度（MISSION_CURRENT、MISSION_ITEM_REACHED）（腳本：scripts/w6_mission_square.py）
2. 觀察任務協定：只送 MISSION_COUNT 不回應會怎樣？（逾時與重送）（腳本：scripts/w6_mission_timeout.py）
3. 設圓柱 geofence，用 Guided 往外飛，觀察越界時的動作（腳本：scripts/w6_geofence.py）
4. 觸發一種 failsafe（例如地面站斷線或電池低電量），確認飛控自己 RTL（腳本：scripts/w6_failsafe_battery.py，觸發低電量）
5. （選做）用 MAVSDK 的 mission 外掛重做實作 1，比較兩者
6. 專案 1：projects/waypoint-mission，整合以上做成「自動飛正方形並回家」

學到什麼

實作 1：上傳任務並用 AUTO 執行

• 上傳：送 MISSION_COUNT = 7，飛控依序要第 0–6 項（全部用舊版 MISSION_REQUEST），回 MISSION_ITEM_INT 即被接受，最後 MISSION_ACK ACCEPTED；不到 0.1 秒完成。
• 在地面切 AUTO 不會起飛：只看到「Mission: 1 Takeoff」，約 10 秒後飛控因為沒離地而自動 disarm；要再送 MAV_CMD_MISSION_START。
• 進度：MISSION_CURRENT 是「現在執行第幾項」，MISSION_ITEM_REACHED 是「到了第幾項」；整趟約 65 秒（起飛 10 秒、四個角各約 5–7 秒、RTL 含降落約 31 秒）。結束後 MISSION_CURRENT 回到第 0 項。
• 圖上看到的：
  • 起飛（第 1 項）沒有座標，QGC 不畫點；第 5 項就在 Home 上，與 Home 圖示重疊，所以只看到 2、3、4 三個圓點
  • 軌跡的角是圓角，飛機沒有飛到航點上才轉彎，而是提早切角（推測與 WP_RADIUS_M、轉角加速度 WP_ACC_CNR 有關，沒有驗證）；與 W5 用速度指令時「先減速再轉」的直角完全不同
  • 飛機落地後機頭朝西（羅盤 270°）

實作 2：任務協定逾時與重送

腳本：scripts/w6_mission_timeout.py。送 MISSION_COUNT = 3 之後完全不回應。

• 重送：飛控每 1 秒重送一次 MISSION_REQUEST seq=0，共 8 次（0–7 秒）。
• 放棄：第 8 秒送 MISSION_ACK type=15（OPERATION_CANCELLED）與 STATUSTEXT：Mission upload timeout，整個上傳作廢。
• 遲到的回應：逾時後才回 MISSION_ITEM_INT seq=0，飛控回 MISSION_ACK type=1（ERROR），不會接受。
• 意義：上傳是「飛控主動要、你被動回」，你這邊停了，飛控也不會永遠等；寫程式時收到 MISSION_ACK 的 type 不是 0 就得從 MISSION_COUNT 重來。

實作 3：圓柱 geofence

腳本：scripts/w6_geofence.py。設 FENCE_ENABLE = 1、FENCE_TYPE = 3（高度上限 + 圓）、FENCE_ACTION = 1（RTL，不行就 LAND）、FENCE_RADIUS = 30（最小值）、FENCE_ALT_MAX = 20；起飛到 10 m 後用速度指令往北 3 m/s。

• QGC 才畫得出圍欄：圓只在 FENCE_ENABLE = 1 時顯示；腳本結束還原參數後圓就消失，截圖要用 keep 選項（結束不還原）。
• 越界：北約 28–30 m 時飛控送出 Circle fence breached，GUIDED 變 RTL；慣性讓飛機衝到北約 32.5 m，之後先爬到 15 m（RTL_ALT_M）再飛回 Home，回到圍欄內時送出 Fence breach cleared，約 45 秒後落地。
• 三個沒預料到的地方（一開始都沒觸發越界）：
  1. Guided 的 goto 目標在圍欄外會被直接拒絕：飛機完全不動、沒有任何回應。要飛出去只能用速度指令。
  2. AVOID_ENABLE 預設 3（含 UseFence）：速度指令飛到離圍欄約 2 m（北 27.6 m）就被擋下來、停在原地，不會越界。要觀察越界必須把 AVOID_ENABLE 設成 0（腳本結束會還原）。
  3. Home 每次 arm 都會重設：圍欄圓心是 Home，上次降落在北 28 m 的話，下次 arm 的圍欄圓心就在那裡；要回到起點才能看到同樣的結果（SITL 用重啟）。
• 重啟後剛開機：EKF 約 20 秒才有位置，這之前切 GUIDED 會出現 Need Position Estimate，arm 也會被拒絕，等 EKF3 IMU0 is using GPS 即可。

實作 4：觸發低電量 failsafe

腳本：scripts/w6_failsafe_battery.py。

• 預設不會動作：BATT_FS_LOW_ACT 預設 0（無），電壓低於 BATT_LOW_VOLT（預設 10.5 V）時只會發出警告，不會 RTL；要自己設成 2（RTL）才會自保。
• 怎麼觸發：SITL 電壓約 12.6 V，把 BATT_LOW_VOLT 調到 13.6 V、BATT_LOW_TIMER 調到 2 秒（預設 10 秒），不需要等電池真的放電。
• 時間軸（起點 = 飛到北 30 m、改完參數）：約 2.2 秒後飛控送出 Battery 1 is low 12.60V used 914 mAh 與 Battery Failsafe，模式從 GUIDED 變 RTL；之後腳本沒有送任何指令，約 43 秒後落地、Disarming motors。與 BATT_LOW_TIMER = 2 吻合。
• 要還原參數：腳本結尾用 finally 還原三個參數，否則下次起飛就帶著 13.6 V 的門檻，一 arm 就觸發 failsafe。
• failsafe 會鎖住：參數還原、飛機也落地了，QGC 仍顯示「Not Ready」與 PreArm: Battery failsafe，arm 被拒絕（Arm: Battery failsafe，COMMAND_ACK result 4）。要重啟飛控才解除（SITL 用 MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN，param1 = 1）；重啟後 Accels inconsistent、waiting for home 等開機檢查會暫時出現，等 EKF 準備好。

卡在哪

• 在地面切 AUTO 不會起飛：只看到 Mission: 1 Takeoff，約 10 秒後因為沒離地而自動 disarm；要再送 MAV_CMD_MISSION_START。
• goto 在圍欄外被默默拒絕：飛機不動、沒有任何回應，一度以為是腳本壞掉；要改用速度指令才飛得出去。
• 圍欄一直沒觸發：AVOID_ENABLE 預設就把速度指令擋在圍欄內 2 m；加上 Home 每次 arm 都重設，連續多次不重啟 SITL 時圓心跟著移動。要重現需關掉 AVOID_ENABLE 並重啟 SITL。
• failsafe 觸發後會鎖住：參數還原、落地後仍 PreArm: Battery failsafe，要重啟飛控才解除。
• 重啟飛控後剛開機：Need Position Estimate、Accels inconsistent、waiting for home 都只是 EKF 還沒好，等約 20 秒。
• QGC 的圍欄圓只在 FENCE_ENABLE = 1 時顯示：腳本結束還原參數後圓就消失，截圖要用 keep 選項。
• 沒解開的疑問：
  • 軌跡的圓角到底由 WP_RADIUS_M 還是 WP_ACC_CNR 決定（只是推測，沒有改參數驗證）。
  • SITL 任務容量 650 是否含 home（MISSION_COUNT 650 可、651 起 NO_SPACE）。
  • NAV_SPLINE_WAYPOINT 實際軌跡長怎樣（沒做）。
  • 只觸發了低電量 failsafe；遙控器斷線、地面站斷線（FS_GCS_ENABLE）、EKF 失效沒測。
  • 實作 5（MAVSDK mission）略過，沒有與 pymavlink 比較。

下週問題

• W7 Gazebo 在 arm64 macOS 上穩不穩，要用 Docker 還是 Linux VM？ArduPilot 與 Gazebo 怎麼串接（ArduPilot Gazebo plugin、SITL 的 JSON 介面）？
• 模擬裡怎麼加攝影機與距離感測器，資料怎麼送出來（Gazebo transport、ROS 2 bridge）？
• 換成 Gazebo 之後，W5、W6 的 pymavlink 腳本還能不能直接跑（連線埠、HOME、物理與 SITL 內建模型的差異）？
• 圍欄與 failsafe 的行為在 Gazebo 裡會不會不同（例如電池模型）？

W07 模擬器進階

學到什麼

卡在哪

下週問題

W08 電腦視覺

學到什麼

卡在哪

下週問題

W09 ROS 2 入門

學到什麼

卡在哪

下週問題

W10 控制理論

學到什麼

卡在哪

下週問題

W11 避障與路徑規劃

學到什麼

卡在哪

下週問題

W12 總結專案

學到什麼

卡在哪

下週問題

專案

• waypoint-mission（W6）：自動飛正方形並回家
• aruco-landing（W9）：偵測標記並精準降落
• final（W12）：總結專案

專案 1：waypoint-mission

W6 專案：自動飛正方形並回家，並在安全參數（geofence、低電量 RTL）到位的前提下執行。

腳本：scripts/w6_waypoint_mission.py

目標與驗收

• 起飛 10 m，依序飛過邊長 20 m 正方形的四個角（北 20 m 起順時針），RTL 回家。
• 全程不手動介入：只啟動 SITL，再執行一支腳本。
• 驗收（腳本最後會印出）：
  • 任務項目 2–5（四個角）都有 MISSION_ITEM_REACHED
  • 落地點離 Home 不超過 3 m
  • 任務在 300 秒內結束

流程

1. 設安全參數：圍欄半徑 50 m、高度上限 20 m、越界 RTL（FENCE_*）；低電量 RTL（BATT_FS_LOW_ACT = 2）。這是實作 3、4 的結果。
2. 上傳任務：7 項（home、TAKEOFF、四個角、RTL），依任務協定逐項回應（實作 1、2）。
3. arm → AUTO → MISSION_START：在地面切 AUTO 不會自己起飛，要再送 MISSION_START。
4. 觀察：記錄到達的項目與離 Home 的最大距離，直到 disarm。
5. 驗收並還原參數：用 finally 保證參數一定設回。

實測結果

耗時 64 秒；到達項目 [1, 2, 3, 4, 5, 6]；最遠離 Home 26.6 m；落地點離 Home 0.0 m
結果：通過

• 飛行距離 74.3 m：四邊共 80 m，圓角切掉約 6 m。
• 四個角之間約 5–7 秒，起飛約 10 秒，RTL（含降落）約 30 秒。
• 最遠只離 Home 26.6 m，比正方形對角的 28.3 m 近：軌跡在角上是圓角，沒有飛到航點上才轉彎（見 W6 筆記實作 1）。
• 圍欄半徑 50 m 遠大於正方形，全程沒有觸發；圍欄與低電量 failsafe 在這裡只是「最後防線」。

可以再做

• 把 NAV_WAYPOINT 換成 NAV_SPLINE_WAYPOINT，比較軌跡（W6 筆記提到但沒有實測）。
• 縮小圍欄到 30 m（最小值）並把邊長改成 30 m，看圍欄會不會在任務中途接手。

附錄：程式碼

筆記裡用到的所有腳本，原始檔在 repo 的 scripts/，這裡直接顯示內容。w*.py 都連到 tcp:127.0.0.1:5762，使用前先執行 scripts/sitl.sh；pid_*.py 是純模擬，不需要 SITL。

• pid_climb.py：PID 完整升空：0 m → 9.5 m → 10 m，同時畫出高度與 P、I、D 三項的數字。
• pid_demo.py：PID 示範：一維高度控制，從 0 m 升到 10 m，比較 P、PI、PID。
• pid_terms.py：PID 三項各自的「量」：懸停在 10 m，t=2 秒時突然多了載重，看 P、I、D 如何貢獻推力。
• sitl.sh：啟動 ArduCopter SITL，並用 MAVProxy 轉送到 UDP 14550（QGC 預設埠）。Ctrl-C 結束。
• w1_modes.py：W1 實驗：有風時比較 AltHold、Loiter、Stabilize。
• w2_noise.py：W2 實驗：開啟模擬的 IMU 震動雜訊，比較原始值與 EKF 估算。
• w2_sensors.py：W2 實作：讀 SITL 靜止時的原始感測器值，與 EKF 估算的姿態比較。
• w4_backlog.py：W4 實驗：沒被讀取的資料會被丟掉，還是下次讀取時一次收到？
• w4_heartbeat.py：W4 實驗 4：HEARTBEAT 斷線偵測。
• w4_rates.py：W4 實驗 2：比較 REQUEST_DATA_STREAM 與 SET_MESSAGE_INTERVAL 實際得到的速率。
• w4_request_message.py：W4 實驗：REQUEST_MESSAGE（指令 512）只要一次，不會持續推送。
• w4_telemetry.py：W4 實驗 1：持續接收 ATTITUDE 與 GLOBAL_POSITION_INT，每秒印出實際頻率與最新值。
• w4_two_ports.py：W4 實驗 3：同時連 5762 與 5763，看速率設定是各自一份還是共用，以及同一個埠能不能連兩次。

pid_climb.py

PID 完整升空：0 m → 9.5 m → 10 m，同時畫出高度與 P、I、D 三項的數字。

檔案位置：scripts/pid_climb.py

"""PID 完整升空：0 m → 9.5 m → 10 m，同時畫出高度與 P、I、D 三項的數字。

模型與增益同 pid_demo.py：推力 u 在 0–1 之間，懸停需要 u = 0.5；a = 2g(u - 0.5) - c·v。
左欄是整段 15 秒，右欄放大 4–13 秒（接近 9.5 m 到停在 10 m 的過程）。
用法：vendor/venv/bin/python -I scripts/pid_climb.py <輸出.png>
"""
import sys

import matplotlib

matplotlib.use("Agg")
import matplotlib.pyplot as plt

# G：重力加速度（m/s²）；DRAG：空氣阻力係數；DT：模擬步長（秒）；T_END：模擬總時間（秒）；TARGET：目標高度（m）
G, DRAG, DT, T_END, TARGET = 9.8, 3.0, 0.01, 15.0, 10.0
# KP、KI、KD：P、I、D 三項的增益（係數）
KP, KI, KD = 1.0, 0.5, 1.2

# alt：高度（m）；vel：垂直速度（m/s）；integral：誤差累積；prev_err：上一步的誤差
alt = vel = integral = 0.0
prev_err = TARGET - alt
# 紀錄：ts 時間、hs 高度、ps/is_/ds 為 P/I/D 三項的值（is_ 避開 Python 關鍵字 is）、us 實際推力
ts, hs, ps, is_, ds, us = [], [], [], [], [], []
for n in range(int(T_END / DT)):
    err = TARGET - alt
    integral = min(max(integral + err * DT, -1.0), 1.0)  # 同 pid_demo.py：限制積分
    # err：誤差 = 目標 - 現在；p、i、d：P、I、D 三項各自貢獻的推力
    p, i, d = KP * err, KI * integral, KD * (err - prev_err) / DT
    prev_err = err
    u = min(max(p + i + d, 0.0), 1.0)
    vel += (2 * G * (u - 0.5) - DRAG * vel) * DT
    alt = max(alt + vel * DT, 0.0)
    ts.append(n * DT); hs.append(alt); ps.append(p); is_.append(i); ds.append(d); us.append(u)

t95 = next(t for t, h in zip(ts, hs) if h >= 9.5)
print(f"到達 9.5 m：t={t95:.2f} 秒")
for t in (0.0, 0.1, 1.0, 2.0, 3.0, t95, 6.0, 8.0, 11.0, 14.9):
    k = min(int(round(t / DT)), len(ts) - 1)
    print(f"t={ts[k]:5.2f}  高度={hs[k]:6.2f}  P={ps[k]:6.2f}  I={is_[k]:5.2f}  D={ds[k]:6.2f}  實際推力={us[k]:.2f}")

plt.rcParams["font.sans-serif"] = ["PingFang TC", "Heiti TC", "Arial Unicode MS"]
plt.rcParams["axes.unicode_minus"] = False
fig, axes = plt.subplots(2, 2, figsize=(12, 7.5), dpi=150, facecolor="#fcfcfb",
                         gridspec_kw={"height_ratios": [1, 1.6]})
(alt_l, alt_r), (pid_l, pid_r) = axes
for ax in axes.flat:
    ax.set_facecolor("#fcfcfb")
    ax.grid(axis="y", color="#e6e5e1", linewidth=0.8)
    ax.tick_params(colors="#52514e")
    for side in ("top", "right", "left"):
        ax.spines[side].set_visible(False)
    ax.spines["bottom"].set_color("#c3c2b7")

for ax in (alt_l, alt_r):
    ax.plot(ts, hs, color="#0b0b0b", linewidth=2)
    ax.axhline(TARGET, color="#8a8984", linestyle="--", linewidth=1.2)
    ax.axhline(9.5, color="#8a8984", linestyle=":", linewidth=1.2)
    ax.axvline(t95, color="#c3c2b7", linewidth=1)
for ax in (pid_l, pid_r):
    ax.axhline(0, color="#c3c2b7", linewidth=1)
    ax.plot(ts, ps, color="#2a78d6", linewidth=2, label="P（現在差多少）")
    ax.plot(ts, is_, color="#eb6834", linewidth=2, label="I（累積的誤差）")
    ax.plot(ts, ds, color="#1baf7a", linewidth=2, label="D（誤差變化多快）")
    ax.plot(ts, us, color="#0b0b0b", linewidth=1.5, linestyle=":", label="實際推力（P+I+D，夾在 0–1）")
    ax.axvline(t95, color="#c3c2b7", linewidth=1)

alt_l.set_xlim(0, T_END); alt_l.set_ylim(0, 11)
alt_l.text(T_END - 0.1, TARGET + 0.2, "目標 10 m", color="#52514e", fontsize=10, ha="right")
alt_l.text(7.5, 8.6, "虛線 9.5 m", color="#52514e", fontsize=10)
alt_l.set_ylabel("高度（公尺）", color="#52514e")
alt_l.set_title("整段：0 → 10 m（15 秒）", loc="left", color="#0b0b0b", fontsize=12)
alt_r.set_xlim(4, 13); alt_r.set_ylim(9.0, 10.2)
alt_r.text(t95 + 0.1, 9.05, f"t = {t95:.1f} 秒：到達 9.5 m", color="#52514e", fontsize=10)
alt_r.set_title("放大：4–13 秒（9.5 → 10 m）", loc="left", color="#0b0b0b", fontsize=12)

pid_l.set_xlim(0, T_END); pid_l.set_ylim(-4.5, 3)
pid_l.set_xlabel("時間（秒）", color="#52514e")
pid_l.set_ylabel("推力貢獻（1.0 = 100%，不管單位）", color="#52514e")
pid_l.text(0.15, 2.55, "P 起始 10（圖外）", color="#2a78d6", fontsize=10)
pid_l.text(0.15, 1.15, "實際推力：前 2 秒 = 1.0", color="#0b0b0b", fontsize=10)
pid_l.text(2.0, -4.25, "D 負值 = 煞車", color="#1baf7a", fontsize=10)
pid_l.legend(frameon=False, loc="upper right", labelcolor="#0b0b0b", fontsize=9)

pid_r.set_xlim(4, 13); pid_r.set_ylim(-0.1, 1.05)
pid_r.set_xlabel("時間（秒）", color="#52514e")
pid_r.text(12.9, 0.52, "I 撐住 0.5", color="#eb6834", fontsize=10, ha="right", va="bottom")
pid_r.text(12.9, 0.03, "P、D 漸漸回到 0", color="#52514e", fontsize=10, ha="right", va="bottom")

fig.suptitle("PID 三項的數字怎麼變：0 m → 9.5 m → 10 m", x=0.01, ha="left", color="#0b0b0b", fontsize=14)
fig.tight_layout()
fig.savefig(sys.argv[1], facecolor=fig.get_facecolor())

pid_demo.py

PID 示範：一維高度控制，從 0 m 升到 10 m，比較 P、PI、PID。

檔案位置：scripts/pid_demo.py

"""PID 示範：一維高度控制，從 0 m 升到 10 m，比較 P、PI、PID。

模型：推力 u 在 0–1 之間，懸停需要 u = 0.5；a = 2g(u - 0.5) - c·v（c 是空氣阻力）。
用法：vendor/venv/bin/python -I scripts/pid_demo.py <輸出.png>
"""
import sys

import matplotlib

matplotlib.use("Agg")
import matplotlib.pyplot as plt

# G：重力加速度（m/s²）；DRAG：空氣阻力係數；DT：模擬步長（秒）；T_END：模擬總時間（秒）；TARGET：目標高度（m）
G, DRAG, DT, T_END, TARGET = 9.8, 3.0, 0.01, 15.0, 10.0
T_START = 2.5  # 圖只顯示 10 m 附近，略過前面的爬升段
# KP、KI、KD：P、I、D 三項的增益（係數）
KP, KI, KD = 1.0, 0.5, 1.2


def simulate(kp, ki, kd):
    # alt：高度（m）；vel：垂直速度（m/s）；integral：誤差累積；prev_err：上一步的誤差；ts、hs：時間與高度的紀錄
    alt = vel = integral = 0.0
    prev_err = TARGET - alt
    ts, hs = [], []
    for i in range(int(T_END / DT)):
        err = TARGET - alt  # err：誤差 = 目標 - 現在
        integral = min(max(integral + err * DT, -1.0), 1.0)  # 限制積分，避免累積過頭（anti-windup）
        deriv = (err - prev_err) / DT  # deriv：誤差變化率（derivative）
        prev_err = err
        # u：推力（0–1）；acc：加速度（m/s²）
        u = min(max(kp * err + ki * integral + kd * deriv, 0.0), 1.0)
        acc = 2 * G * (u - 0.5) - DRAG * vel
        vel += acc * DT
        alt = max(alt + vel * DT, 0.0)
        ts.append(i * DT)
        hs.append(alt)
    return ts, hs


runs = [("只有 P", (KP, 0, 0), "#2a78d6"), ("P + I", (KP, KI, 0), "#eb6834"), ("P + I + D", (KP, KI, KD), "#1baf7a")]
results = {name: simulate(*gains) for name, gains, _ in runs}
for name, (_, hs) in results.items():
    print(f"{name}: 最後 {hs[-1]:.2f} m，最高 {max(hs):.2f} m")

plt.rcParams["font.sans-serif"] = ["PingFang TC", "Heiti TC", "Arial Unicode MS"]
plt.rcParams["axes.unicode_minus"] = False
fig, ax = plt.subplots(figsize=(9, 5), dpi=150, facecolor="#fcfcfb")
ax.set_facecolor("#fcfcfb")
ax.axhline(TARGET, color="#8a8984", linestyle="--", linewidth=1.2)
ax.text(T_END - 0.1, TARGET + 0.04, "目標 10 m", color="#52514e", fontsize=10, ha="right")
for name, _, color in runs:
    ts, hs = results[name]
    ax.plot(ts, hs, color=color, linewidth=2, label=name)
ax.text(T_END + 0.15, results["只有 P"][1][-1], "只有 P", color="#0b0b0b", fontsize=10, va="center")
ax.text(6.9, 9.72, "P + I + D：平順到 10 m", color="#0b0b0b", fontsize=10)
ax.set_xlim(T_START, T_END)
ax.set_ylim(9.0, 10.7)
peak_t, peak_h = max(zip(*results["P + I"]), key=lambda p: p[1])
ax.annotate(f"P + I：衝過頭 {peak_h - TARGET:.2f} m", (peak_t, peak_h), (peak_t + 1.0, peak_h + 0.1),
            color="#52514e", fontsize=10, arrowprops=dict(arrowstyle="-", color="#8a8984"))
ax.annotate("", (T_END - 1.5, 9.5), (T_END - 1.5, TARGET),
            arrowprops=dict(arrowstyle="<->", color="#2a78d6"))
ax.text(T_END - 1.3, 9.72, f"卡住，差 {TARGET - results['只有 P'][1][-1]:.1f} m", color="#52514e", fontsize=10)
ax.set_xlabel("時間（秒）", color="#52514e")
ax.set_ylabel("高度（公尺）", color="#52514e")
ax.set_title("PID 控制高度：放大 10 m 附近（從 0 m 升到 10 m）", loc="left", color="#0b0b0b", fontsize=13)
ax.grid(axis="y", color="#e6e5e1", linewidth=0.8)
ax.tick_params(colors="#52514e")
for side in ("top", "right", "left"):
    ax.spines[side].set_visible(False)
ax.spines["bottom"].set_color("#c3c2b7")
ax.legend(frameon=False, loc="lower right", labelcolor="#0b0b0b")
fig.subplots_adjust(right=0.86)
fig.savefig(sys.argv[1], facecolor=fig.get_facecolor())

pid_terms.py

PID 三項各自的「量」：懸停在 10 m，t=2 秒時突然多了載重，看 P、I、D 如何貢獻推力。

檔案位置：scripts/pid_terms.py

"""PID 三項各自的「量」：懸停在 10 m，t=2 秒時突然多了載重，看 P、I、D 如何貢獻推力。

模型：a = 2g(u - h) - c·v，h 是懸停所需推力，載重使 h 由 0.5 變 0.6。
起始時積分已累積到剛好撐住 0.5 的量（代表之前已穩定懸停）。
用法：vendor/venv/bin/python -I scripts/pid_terms.py <輸出.png>
"""
import sys

import matplotlib

matplotlib.use("Agg")
import matplotlib.pyplot as plt

# G：重力加速度（m/s²）；DRAG：空氣阻力係數；DT：模擬步長（秒）；T_END：模擬總時間（秒）；TARGET：目標高度（m）
G, DRAG, DT, T_END, TARGET = 9.8, 3.0, 0.01, 12.0, 10.0
# KP、KI、KD：P、I、D 三項的增益（係數）
KP, KI, KD = 1.0, 0.5, 1.2
LOAD_T, HOVER_BEFORE, HOVER_AFTER = 2.0, 0.5, 0.6

# alt：高度（m）；vel：垂直速度（m/s）；integral：誤差累積；prev_err：上一步的誤差
alt, vel = TARGET, 0.0
integral = HOVER_BEFORE / KI  # 積分已累積到剛好撐住 0.5
prev_err = 0.0
# 紀錄：ts 時間、hs 高度、ps/is_/ds 為 P/I/D 三項的值（is_ 避開 Python 關鍵字 is）、us 實際推力
ts, hs, ps, is_, ds, us = [], [], [], [], [], []
for n in range(int(T_END / DT)):
    t = n * DT
    hover = HOVER_AFTER if t >= LOAD_T else HOVER_BEFORE
    err = TARGET - alt
    integral = min(max(integral + err * DT, -2.0), 2.0)
    # p、i、d：P、I、D 三項各自貢獻的推力
    p, i, d = KP * err, KI * integral, KD * (err - prev_err) / DT
    prev_err = err
    u = min(max(p + i + d, 0.0), 1.0)
    vel += (2 * G * (u - hover) - DRAG * vel) * DT
    alt += vel * DT
    ts.append(t); hs.append(alt); ps.append(p); is_.append(i); ds.append(d); us.append(u)

print(f"最低 {min(hs):.2f} m，最後 {hs[-1]:.3f} m")
for t in (1.9, 2.1, 2.5, 3, 4, 6, 8, 11.9):
    k = int(t / DT)
    print(f"t={t:5.1f}  P={ps[k]:6.3f}  I={is_[k]:6.3f}  D={ds[k]:6.3f}  合計={us[k]:.3f}")

plt.rcParams["font.sans-serif"] = ["PingFang TC", "Heiti TC", "Arial Unicode MS"]
plt.rcParams["axes.unicode_minus"] = False
fig, (top, bot) = plt.subplots(2, 1, figsize=(9, 7), dpi=150, facecolor="#fcfcfb",
                               sharex=True, gridspec_kw={"height_ratios": [1, 2]})
for ax in (top, bot):
    ax.set_facecolor("#fcfcfb")
    ax.grid(axis="y", color="#e6e5e1", linewidth=0.8)
    ax.tick_params(colors="#52514e")
    for side in ("top", "right", "left"):
        ax.spines[side].set_visible(False)
    ax.spines["bottom"].set_color("#c3c2b7")
    ax.axvline(LOAD_T, color="#8a8984", linestyle="--", linewidth=1.2, label="_load")

top.plot(ts, hs, color="#0b0b0b", linewidth=2)
top.axhline(TARGET, color="#8a8984", linewidth=1)
top.set_ylabel("高度（公尺）", color="#52514e")
top.set_title("PID 三項各自的量：懸停中突然多了載重（t = 2 秒）", loc="left", color="#0b0b0b", fontsize=13)
top.set_ylim(9.92, 10.01)
top.text(LOAD_T + 0.15, 9.925, "多了載重，需要的推力 50% → 60%", color="#52514e", fontsize=10)

bot.axhline(0, color="#c3c2b7", linewidth=1, label="_zero")
bot.plot(ts, ps, color="#2a78d6", linewidth=2, label="P（現在差多少）")
bot.plot(ts, is_, color="#eb6834", linewidth=2, label="I（累積的誤差）")
bot.plot(ts, ds, color="#1baf7a", linewidth=2, label="D（誤差變化多快）")
bot.plot(ts, us, color="#0b0b0b", linewidth=1.5, linestyle=":", label="合計 = P + I + D")
bot.set_ylim(-0.3, 0.8)
bot.set_xlim(0, T_END)
bot.set_xlabel("時間（秒）", color="#52514e")
bot.set_ylabel("推力貢獻（1.0 = 100%）", color="#52514e")
bot.text(0.05, 0.44, "載重前：I 撐住 50%", color="#52514e", fontsize=10)
bot.text(T_END - 0.1, 0.63, "I 慢慢累積到 60%", color="#eb6834", fontsize=10, ha="right", va="bottom")
bot.text(T_END - 0.1, 0.03, "P、D 回到 0", color="#52514e", fontsize=10, ha="right", va="bottom")
bot.legend(frameon=False, loc="center right", bbox_to_anchor=(1.0, 0.6), labelcolor="#0b0b0b", fontsize=10)
fig.tight_layout()
fig.savefig(sys.argv[1], facecolor=fig.get_facecolor())

sitl.sh

啟動 ArduCopter SITL，並用 MAVProxy 轉送到 UDP 14550（QGC 預設埠）。Ctrl-C 結束。

檔案位置：scripts/sitl.sh

#!/usr/bin/env bash
# 啟動 ArduCopter SITL，並用 MAVProxy 轉送到 UDP 14550（QGC 預設埠）。Ctrl-C 結束。
set -euo pipefail
root="$(cd "$(dirname "$0")/.." && pwd)"
export PATH="$root/vendor/venv/bin:$PATH"
mkdir -p "$root/vendor/sitl-run" && cd "$root/vendor/sitl-run"

cleanup() {
  trap - EXIT INT TERM
  pkill -f "[m]avproxy.py" || true
  pkill -f "[a]rducopter" || true
  pkill -f "[s]im_vehicle" || true
}
trap cleanup EXIT
trap 'exit 130' INT TERM

python ../ardupilot/Tools/autotest/sim_vehicle.py -v ArduCopter --no-rebuild --no-mavproxy -w >sitl.log 2>&1 &
sleep 10
# 5760: 給 MAVProxy；5762: 留給自己的 pymavlink 程式；14550: QGC
# 放背景再 wait，Ctrl-C 才會立刻觸發 trap（前景子程序會延後 trap）
mavproxy.py --master tcp:127.0.0.1:5760 --out udp:127.0.0.1:14550 --daemon --non-interactive &
wait $!

w1_modes.py

W1 實驗：有風時比較 AltHold、Loiter、Stabilize。

檔案位置：scripts/w1_modes.py

"""W1 實驗：有風時比較 AltHold、Loiter、Stabilize。

先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762，起飛後依序切換模式，
用 RC override 把搖桿全部放在中間（油門 1500），每秒印出高度與水平漂移。
"""
import math
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
TAKEOFF_ALT = 10
PHASE_SECONDS = 15
WIND_SPEED = 5  # m/s
WIND_DIR = 90  # 度，風從哪個方向吹來

MODES = {"STABILIZE": 0, "ALT_HOLD": 2, "GUIDED": 4, "LOITER": 5, "LAND": 9}

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component,
                               mavutil.mavlink.MAV_DATA_STREAM_ALL, 10, 1)
last_rc = 0.0  # 上次送出 RC override 的時間（rc = remote control，遙控器）
sticks_centered = True


def send_rc():
    """RC override 必須持續發送（約 10 Hz），否則飛控會當作遙控器斷線。"""
    global last_rc
    if sticks_centered and time.time() - last_rc > 0.1:
        m.mav.rc_channels_override_send(m.target_system, m.target_component,
                                        1500, 1500, 1500, 1500, 0, 0, 0, 0)
        last_rc = time.time()


def set_mode(name):
    m.mav.set_mode_send(m.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
                        MODES[name])


def set_param(name, value):
    m.mav.param_set_send(m.target_system, m.target_component, name.encode(), value,
                         mavutil.mavlink.MAV_PARAM_TYPE_REAL32)


def pause(seconds):
    end = time.time() + seconds
    while time.time() < end:
        send_rc()
        time.sleep(0.05)


def position():
    """回傳 (x, y, 高度) 公尺；NED 座標的 z 向下為正，所以高度取負號。"""
    msg = m.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=2)
    return (msg.x, msg.y, -msg.z) if msg else None


def observe(label, seconds):
    print(f"\n=== {label}（{seconds} 秒）===")
    origin = position()
    start = last_print = time.time()
    while time.time() - start < seconds:
        send_rc()
        pos = position()
        if pos and time.time() - last_print >= 1:
            drift = math.hypot(pos[0] - origin[0], pos[1] - origin[1])
            print(f"高度 {pos[2]:5.1f} m   水平漂移 {drift:5.1f} m")
            last_print = time.time()


try:
    print("切到 Guided、解鎖、起飛")
    set_mode("GUIDED")
    pause(1)
    # param2=21196 是強制解鎖（略過 "Throttle (RC3) is not neutral" 等檢查），QGC 的 Takeoff 也這樣做；
    # 只適合模擬，實機不要用
    m.mav.command_long_send(m.target_system, m.target_component,
                            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 21196, 0, 0, 0, 0, 0)
    ack = m.recv_match(type="COMMAND_ACK", blocking=True, timeout=5)
    if ack is None or ack.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
        raise SystemExit(f"解鎖被拒絕：{ack}")
    pause(2)
    m.mav.command_long_send(m.target_system, m.target_component,
                            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, TAKEOFF_ALT)
    deadline = time.time() + 30
    while time.time() < deadline:
        send_rc()
        pos = position()
        if pos and pos[2] > TAKEOFF_ALT - 0.5:
            break
    else:
        raise SystemExit("30 秒內沒有到達起飛高度")
    print(f"已到 {pos[2]:.1f} m；開風 {WIND_SPEED} m/s")
    set_param("SIM_WIND_SPD", WIND_SPEED)
    set_param("SIM_WIND_DIR", WIND_DIR)
    pause(3)

    set_mode("ALT_HOLD")
    observe("AltHold：高度不變，位置被風吹走", PHASE_SECONDS)
    set_mode("LOITER")
    observe("Loiter：高度不變，位置也頂住風", PHASE_SECONDS)
    set_mode("STABILIZE")
    observe("Stabilize：高度與位置都不管", 10)
finally:
    print("\n降落並關風")
    sticks_centered = False
    m.mav.rc_channels_override_send(m.target_system, m.target_component, 0, 0, 0, 0, 0, 0, 0, 0)
    set_mode("LAND")
    set_param("SIM_WIND_SPD", 0)

w2_noise.py

W2 實驗：開啟模擬的 IMU 震動雜訊，比較原始值與 EKF 估算。

檔案位置：scripts/w2_noise.py

"""W2 實驗：開啟模擬的 IMU 震動雜訊，比較原始值與 EKF 估算。

SIM_ACC1_RND / SIM_GYR1_RND 只在馬達轉動時生效（陀螺儀還會乘上油門），
加速度計的雜訊還要 SIM_VIB_FREQ 非 0 才會加進去，所以必須在空中懸停。
先執行 scripts/sitl.sh；用法：vendor/venv/bin/python -u -I scripts/w2_noise.py [--hold]
"""
import math
import statistics
import sys
import time

from pymavlink import mavutil

HOLD = "--hold" in sys.argv  # 懸停並保持雜訊，Ctrl-C 結束（給 QGC 觀察用）
# ACC_RND、GYR_RND：模擬加速度計、陀螺儀的隨機雜訊（rnd = random）強度；VIB_FREQ：x、y、z 軸的振動頻率
ACC_RND, GYR_RND = (10.0, 10.0) if HOLD else (3.0, 5.0)
VIB_FREQ = (50.0, 60.0, 70.0)  # Hz；加速度計雜訊要有振動頻率才會加進去
N = 100  # 每階段取樣筆數

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=10)


def set_param(name, value):
    m.mav.param_set_send(m.target_system, m.target_component, name.encode(), value,
                         mavutil.mavlink.MAV_PARAM_TYPE_REAL32)


def command(cmd, *params, timeout=10):  # cmd：MAV_CMD 指令編號；params：最多 7 個指令參數，不足補 0
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0,
                            *(list(params) + [0] * (7 - len(params))))
    ack = m.recv_match(type="COMMAND_ACK", blocking=True, timeout=timeout)
    if ack is None or ack.result != 0:
        sys.exit(f"指令 {cmd} 失敗：{ack}")


def interval(name, hz):
    mid = getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}")  # mid：message id，訊息編號
    command(mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, mid, int(1e6 / hz))


def sample():
    imu, att = [], []  # imu：RAW_IMU 原始值；att：EKF 估算的 ATTITUDE
    deadline = time.time() + 30
    while time.time() < deadline and (len(imu) < N or len(att) < N):
        msg = m.recv_match(type=["RAW_IMU", "ATTITUDE"], blocking=True, timeout=1)
        if msg is None:
            continue
        (imu if msg.get_type() == "RAW_IMU" else att).append(msg)
    if len(imu) < N or len(att) < N:
        sys.exit("取樣逾時")
    return imu[:N], att[:N]


def report(label, imu, att):
    sd = statistics.stdev  # sd：standard deviation，標準差，用來表示雜訊大小
    # roll_acc：只用加速度計算的 roll；roll_ekf：EKF 估算的 roll（度）
    roll_acc = [math.degrees(math.atan2(r.yacc, -r.zacc)) for r in imu]
    roll_ekf = [math.degrees(r.roll) for r in att]
    print(f"\n== {label} ==")
    print(f"  陀螺儀 x 標準差   {sd([r.xgyro / 1000 for r in imu]):.4f} rad/s")
    print(f"  加速度計 z 標準差 {sd([r.zacc / 1000 * 9.80665 for r in imu]):.4f} m/s²")
    print(f"  roll 只用加速度計  標準差 {sd(roll_acc):.3f}°（範圍 {min(roll_acc):.2f}~{max(roll_acc):.2f}）")
    print(f"  roll EKF 估算      標準差 {sd(roll_ekf):.3f}°（範圍 {min(roll_ekf):.2f}~{max(roll_ekf):.2f}）")


try:
    command(mavutil.mavlink.MAV_CMD_DO_SET_MODE, 1, 4)  # Guided
    command(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 1, 21196)  # 模擬用強制解鎖
    command(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 10)
    for name in ("RAW_IMU", "ATTITUDE"):
        interval(name, 20)
    print("起飛中…")
    while True:
        pos = m.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=2)
        if pos and -pos.z > 9.5:
            break
    time.sleep(3)

    if not HOLD:
        imu, att = sample()
        report("無雜訊（懸停 10 m）", imu, att)

    set_param("SIM_ACC1_RND", ACC_RND)
    set_param("SIM_GYR1_RND", GYR_RND)
    for axis, hz in zip("XYZ", VIB_FREQ):
        set_param(f"SIM_VIB_FREQ_{axis}", hz)
    time.sleep(2)
    if HOLD:
        print("雜訊已開啟，懸停中；在 QGC 的 MAVLink Inspector 觀察，Ctrl-C 結束")
        while True:
            m.recv_match(blocking=True, timeout=1)  # 要持續讀，否則 TCP 緩衝區塞滿會卡住 SITL
    imu, att = sample()
    report(f"有雜訊（SIM_ACC1_RND={ACC_RND}, SIM_GYR1_RND={GYR_RND}）", imu, att)
finally:
    set_param("SIM_ACC1_RND", 0)
    set_param("SIM_GYR1_RND", 0)
    for axis in "XYZ":
        set_param(f"SIM_VIB_FREQ_{axis}", 0)
    m.mav.command_long_send(m.target_system, m.target_component,
                            mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 1, 9, 0, 0, 0, 0, 0)  # Land
    print("\n已關閉雜訊並切 Land")

w2_sensors.py

W2 實作：讀 SITL 靜止時的原始感測器值，與 EKF 估算的姿態比較。

檔案位置：scripts/w2_sensors.py

"""W2 實作：讀 SITL 靜止時的原始感測器值，與 EKF 估算的姿態比較。

先執行 scripts/sitl.sh，等出現 EKF3 IMU0 is using GPS。
用法：vendor/venv/bin/python -u -I scripts/w2_sensors.py
"""
import math
import statistics
import sys
import time

from pymavlink import mavutil

N = 50  # 取樣筆數
TYPES = ["RAW_IMU", "SCALED_PRESSURE", "GPS_RAW_INT", "ATTITUDE"]

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=30)

# 要求每個訊息 10 Hz（單位：微秒）
for name in TYPES:
    # mid：message id，訊息編號
    mid = getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}")
    m.mav.command_long_send(m.target_system, m.target_component,
                            mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
                            mid, 100_000, 0, 0, 0, 0, 0)

data = {t: [] for t in TYPES}
deadline = time.time() + 30
while time.time() < deadline and min(len(v) for v in data.values()) < N:
    msg = m.recv_match(type=TYPES, blocking=True, timeout=1)
    if msg and len(data[msg.get_type()]) < N:
        data[msg.get_type()].append(msg)

if min(len(v) for v in data.values()) < N:
    sys.exit(f"逾時，收到筆數：{ {k: len(v) for k, v in data.items()} }")


def stat(xs):  # xs：一串數值（取樣）；回傳 (平均, 標準差)
    return statistics.mean(xs), statistics.stdev(xs)


def show(label, xs, unit):
    mean, sd = stat(xs)  # sd：standard deviation，標準差，用來表示雜訊大小
    print(f"  {label:<10} 平均 {mean:10.3f}  雜訊（標準差）{sd:8.4f}  {unit}")


# imu：RAW_IMU 原始值（陀螺儀、加速度計）；att：EKF 估算的 ATTITUDE；gps：最新一筆 GPS_RAW_INT
imu = data["RAW_IMU"]
print(f"== RAW_IMU（{N} 筆）==")
show("gyro x", [r.xgyro / 1000 for r in imu], "rad/s")
show("gyro y", [r.ygyro / 1000 for r in imu], "rad/s")
show("gyro z", [r.zgyro / 1000 for r in imu], "rad/s")
show("accel x", [r.xacc / 1000 * 9.80665 for r in imu], "m/s²")
show("accel y", [r.yacc / 1000 * 9.80665 for r in imu], "m/s²")
show("accel z", [r.zacc / 1000 * 9.80665 for r in imu], "m/s²（靜止時約 -9.8，即重力）")

print("== SCALED_PRESSURE ==")
show("氣壓", [r.press_abs for r in data["SCALED_PRESSURE"]], "hPa")
show("溫度", [r.temperature / 100 for r in data["SCALED_PRESSURE"]], "°C")

gps = data["GPS_RAW_INT"][-1]
print("== GPS_RAW_INT ==")
print(f"  fix_type {gps.fix_type}（3 = 3D 定位，6 = RTK 固定解；SITL 模擬的 GPS 回報 6），衛星數 {gps.satellites_visible}")
print(f"  緯度 {gps.lat / 1e7:.6f}，經度 {gps.lon / 1e7:.6f}，高度 {gps.alt / 1000:.1f} m")

att = data["ATTITUDE"]
print("== ATTITUDE（EKF 估算）==")
show("roll", [math.degrees(r.roll) for r in att], "°")
show("pitch", [math.degrees(r.pitch) for r in att], "°")
show("yaw", [math.degrees(r.yaw) for r in att], "°")

# 只用加速度計算傾角，對照 EKF 的估算
# ax、ay、az：加速度計 x、y、z 軸原始值（單位 mG）
ax = [r.xacc for r in imu]
ay = [r.yacc for r in imu]
az = [r.zacc for r in imu]
# roll_acc、pitch_acc：只用加速度計算出的 roll、pitch（度）
roll_acc = math.degrees(math.atan2(statistics.mean(ay), -statistics.mean(az)))
pitch_acc = math.degrees(math.atan2(statistics.mean(ax),
                                     math.hypot(statistics.mean(ay), statistics.mean(az))))
print("== 加速度計單獨算的傾角 vs EKF ==")
print(f"  roll  加速度計 {roll_acc:7.3f}°  EKF {statistics.mean(math.degrees(r.roll) for r in att):7.3f}°")
print(f"  pitch 加速度計 {pitch_acc:7.3f}°  EKF {statistics.mean(math.degrees(r.pitch) for r in att):7.3f}°")

w4_backlog.py

W4 實驗：沒被讀取的資料會被丟掉，還是下次讀取時一次收到？

檔案位置：scripts/w4_backlog.py

"""W4 實驗：沒被讀取的資料會被丟掉，還是下次讀取時一次收到？

A. 有要求但暫停讀取：要 ATTITUDE 20 Hz，停止讀取 PAUSE 秒，再讀取，看收到的是不是一批舊資料。
B. 沒要求：停止要求 ATTITUDE，等 PAUSE 秒，再要求，看是不是一下子湧進一批。
先執行 scripts/sitl.sh；用法：vendor/venv/bin/python -u -I scripts/w4_backlog.py
"""
import time

from pymavlink import mavutil

mav = mavutil.mavlink  # MAVLink 的常數與訊息編號都在這個模組裡
PAUSE = 5.0  # 暫停（秒）
HZ = 20

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=10)


def interval(hz):
    """SET_MESSAGE_INTERVAL 設定 ATTITUDE 的速率；hz = -1 表示停止。"""
    mid = getattr(mav, "MAVLINK_MSG_ID_ATTITUDE")  # mid：message id，訊息編號
    us = int(1e6 / hz) if hz > 0 else hz  # us：間隔（微秒）
    m.mav.command_long_send(m.target_system, m.target_component, mav.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
                            mid, us, 0, 0, 0, 0, 0)


def read_for(seconds):
    """讀 seconds 秒，回傳 [(收到時刻（相對本次開始，秒）, 飛控開機後的毫秒數)]。"""
    out, start = [], time.time()
    while time.time() - start < seconds:
        msg = m.recv_match(type="ATTITUDE", blocking=True, timeout=0.5)
        if msg is not None:
            out.append((time.time() - start, msg.time_boot_ms))
    return out


def summary(label, msgs, before_ms=None):
    first_burst = sum(1 for t, _ in msgs if t < 0.5)  # 恢復讀取後 0.5 秒內收到幾筆
    line = f"{label}：共 {len(msgs)} 筆；前 0.5 秒收到 {first_burst} 筆"
    if before_ms is not None and msgs:
        line += f"；第一筆比暫停前最後一筆晚 {(msgs[0][1] - before_ms) / 1000:.2f} 秒（飛控時間）"
    print(line)


m.mav.request_data_stream_send(m.target_system, m.target_component, mav.MAV_DATA_STREAM_ALL, 0, 0)
interval(-1)
time.sleep(1)

print("== A. 有要求，但暫停讀取 ==")
interval(HZ)
warm = read_for(1.0)
summary("暫停前 1 秒", warm)
last_ms = warm[-1][1]
print(f"暫停 {PAUSE:.0f} 秒（不讀取）…")
time.sleep(PAUSE)
after = read_for(2.0)
summary("恢復讀取後 2 秒", after, last_ms)
print(f"  預期：若是即時資料，前 0.5 秒約 {HZ // 2} 筆；若有堆積，會多出約 {int(PAUSE * HZ)} 筆")

print("\n== B. 沒要求（速率 -1），暫停後再要求 ==")
interval(-1)
time.sleep(0.5)
while m.recv_match(blocking=False):  # 丟掉緩衝區裡剩下的
    pass
print(f"不要求 {PAUSE:.0f} 秒（不讀取）…")
time.sleep(PAUSE)
leftover = 0
while m.recv_match(type="ATTITUDE", blocking=False):  # 這段期間有沒有東西在排隊
    leftover += 1
print(f"緩衝區裡的 ATTITUDE：{leftover} 筆")
interval(HZ)
resumed = read_for(2.0)
summary("重新要求後 2 秒", resumed)
interval(-1)

w4_heartbeat.py

W4 實驗 4：HEARTBEAT 斷線偵測。

檔案位置：scripts/w4_heartbeat.py

"""W4 實驗 4：HEARTBEAT 斷線偵測。

用法：vendor/venv/bin/python -u -I scripts/w4_heartbeat.py [freeze|kill]
  freeze（預設）：5 秒時暫停 arducopter 程序（SIGSTOP），13 秒時恢復（SIGCONT）。
                  TCP 連線還在，只是飛控不再送資料，只能靠 HEARTBEAT 逾時偵測。
  kill：5 秒時直接殺掉 arducopter（SIGKILL），TCP 連線被關閉。跑完要重啟 SITL。
        連線關閉後 pymavlink 會空轉狂印 "EOF on TCP socket"（10 秒約 290 萬行），用管線濾掉：
        ... scripts/w4_heartbeat.py kill | grep -v "EOF on TCP socket"
偵測方法：超過 TIMEOUT 秒沒收到「飛控」的 HEARTBEAT 就判定斷線（HEARTBEAT 每秒 1 次）。
連線上還會收到 GCS（地面站，system 255）的 HEARTBEAT，是別的程式發的、經飛控轉送過來，必須過濾掉。
先執行 scripts/sitl.sh。
"""
import os
import signal
import subprocess
import sys
import time

from pymavlink import mavutil

MODE = sys.argv[1] if len(sys.argv) > 1 else "freeze"
TIMEOUT = 3.0  # 超過幾秒沒收到 HEARTBEAT 就判定斷線（約 3 次沒收到）

# m：MAVLink 連線；m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=10)
pid = int(subprocess.check_output(["pgrep", "-x", "arducopter"]).split()[0])  # 飛控程序的 pid

if MODE == "freeze":
    events = [(5.0, signal.SIGSTOP, "暫停飛控（SIGSTOP）"), (13.0, signal.SIGCONT, "恢復飛控（SIGCONT）")]
    end = 20.0
else:
    events = [(5.0, signal.SIGKILL, "殺掉飛控（SIGKILL）")]
    end = 15.0

t0 = time.time()  # 實驗開始時刻
last_hb = t0  # 上次收到 HEARTBEAT 的時刻
last_print = t0
lost_at = None  # 判定斷線的時刻
stopped_at = None  # 飛控被暫停或殺掉的時刻
hb_count = 0
gcs_count = 0  # 被過濾掉的 GCS HEARTBEAT 個數
hb_times = []  # 每個 HEARTBEAT 的收到時刻（相對實驗開始，秒）

while time.time() - t0 < end:
    now = time.time() - t0
    if events and now >= events[0][0]:
        _, sig, label = events.pop(0)
        os.kill(pid, sig)
        if sig != signal.SIGCONT:
            stopped_at = now
        print(f"[{now:5.1f}s] {label}")
    try:
        hb = m.recv_match(type="HEARTBEAT", blocking=True, timeout=0.5)
    except Exception as e:  # 連線被關閉時 pymavlink 可能丟例外
        print(f"[{now:5.1f}s] recv_match 例外：{e!r}")
        hb = None
    if hb is not None and hb.type == mavutil.mavlink.MAV_TYPE_GCS:  # 不是飛控發的，忽略
        gcs_count += 1
        continue
    if hb is not None:
        hb_count += 1
        hb_times.append(time.time() - t0)
        if lost_at is not None:
            print(f"[{time.time() - t0:5.1f}s] 恢復：收到 HEARTBEAT（斷線 {time.time() - t0 - lost_at:.1f} 秒）")
            lost_at = None
        last_hb = time.time()
    gap = time.time() - last_hb
    if gap > TIMEOUT and lost_at is None:
        lost_at = time.time() - t0
        print(f"[{lost_at:5.1f}s] 判定斷線：{gap:.1f} 秒沒收到 HEARTBEAT"
              + (f"（暫停後 {lost_at - stopped_at:.1f} 秒）" if stopped_at is not None else ""))
    if time.time() - last_print >= 1.0:
        print(f"[{time.time() - t0:5.1f}s] 距上次 HEARTBEAT {gap:.1f} 秒" + ("  ← 斷線中" if lost_at else ""))
        last_print = time.time()

# 各階段收到幾個 HEARTBEAT：(開始, 結束, 標籤)
phases = [(0, 5, "暫停前"), (5, 13, "暫停中"), (13, 14, "恢復後 1 秒"), (14, end, "恢復後其餘")] if MODE == "freeze" \
    else [(0, 5, "殺掉前"), (5, end, "殺掉後")]
for lo, hi, label in phases:
    print(f"{label}（{lo}–{hi:.0f} 秒）：{sum(lo <= t < hi for t in hb_times)} 個 HEARTBEAT")
print(f"\n共收到 {hb_count} 個飛控 HEARTBEAT（另外過濾掉 {gcs_count} 個 GCS HEARTBEAT）")

w4_rates.py

W4 實驗 2：比較 REQUEST_DATA_STREAM 與 SET_MESSAGE_INTERVAL 實際得到的速率。

檔案位置：scripts/w4_rates.py

"""W4 實驗 2：比較 REQUEST_DATA_STREAM 與 SET_MESSAGE_INTERVAL 實際得到的速率。

同一條連線依序做六個階段，每階段量 4 秒，印出 ATTITUDE、GLOBAL_POSITION_INT、LOCAL_POSITION_NED 的實際 Hz。
REQUEST_DATA_STREAM 以「群組」為單位（ATTITUDE 在 EXTRA1，位置類在 POSITION）；
SET_MESSAGE_INTERVAL 以單一訊息為單位。看兩者混用時誰蓋過誰。
速率設定會留在 SITL 的連接埠上、斷線也不會消失，所以開頭先重設，不必重啟 SITL；
先執行 scripts/sitl.sh；
用法：vendor/venv/bin/python -u -I scripts/w4_rates.py
"""
import time

from pymavlink import mavutil

mav = mavutil.mavlink  # MAVLink 的常數與訊息編號都在這個模組裡
NAMES = ["ATTITUDE", "GLOBAL_POSITION_INT", "LOCAL_POSITION_NED"]
WINDOW = 4.0

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=10)


def stream(stream_id, hz):
    """REQUEST_DATA_STREAM：以群組為單位；hz = 0 表示停止。"""
    m.mav.request_data_stream_send(m.target_system, m.target_component, stream_id, hz, 1 if hz else 0)


def interval(name, hz):
    """SET_MESSAGE_INTERVAL：單一訊息；hz = -1 表示停止，0 表示還原預設。"""
    mid = getattr(mav, f"MAVLINK_MSG_ID_{name}")  # mid：message id，訊息編號
    us = int(1e6 / hz) if hz > 0 else hz  # us：間隔（微秒）；-1 表示停止，0 表示還原預設
    m.mav.command_long_send(m.target_system, m.target_component, mav.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
                            mid, us, 0, 0, 0, 0, 0)


def measure():
    counts = dict.fromkeys(NAMES, 0)
    start = time.time()
    while time.time() - start < WINDOW:
        msg = m.recv_match(type=NAMES, blocking=True, timeout=1)  # 要持續讀取，否則 SITL 會卡住
        if msg is not None:
            counts[msg.get_type()] += 1
    return {k: v / WINDOW for k, v in counts.items()}


def reset():
    """清掉上次實驗留下的速率：停掉所有串流群組，並把三種訊息的間隔還原成預設。"""
    stream(mav.MAV_DATA_STREAM_ALL, 0)
    for name in NAMES:
        interval(name, 0)
    time.sleep(1)


reset()
stages = [
    ("0. 什麼都不要", lambda: None),
    ("1. DATA_STREAM EXTRA1 → 4 Hz", lambda: stream(mav.MAV_DATA_STREAM_EXTRA1, 4)),
    ("2. DATA_STREAM EXTRA1 → 20 Hz", lambda: stream(mav.MAV_DATA_STREAM_EXTRA1, 20)),
    ("3. 再加 INTERVAL ATTITUDE → 10 Hz", lambda: interval("ATTITUDE", 10)),
    ("4. DATA_STREAM POSITION → 2 Hz", lambda: stream(mav.MAV_DATA_STREAM_POSITION, 2)),
    ("5. 再加 INTERVAL GLOBAL_POSITION_INT → 10 Hz", lambda: interval("GLOBAL_POSITION_INT", 10)),
]

print(f"{'階段':<46}" + "".join(f"{n:>22}" for n in NAMES))
for label, action in stages:
    action()
    time.sleep(0.5)  # 讓新設定生效，並丟掉換速率瞬間的暫態
    rates = measure()
    print(f"{label:<44}" + "".join(f"{rates[n]:>19.1f} Hz" for n in NAMES))

w4_request_message.py

W4 實驗：REQUEST_MESSAGE（指令 512）只要一次，不會持續推送。

檔案位置：scripts/w4_request_message.py

"""W4 實驗：REQUEST_MESSAGE（指令 512）只要一次，不會持續推送。

先把 ATTITUDE 停掉，再各送 REQUEST_MESSAGE 一次，觀察 COMMAND_ACK 與收到的訊息筆數；
另外要一次平常不會主動送的 AUTOPILOT_VERSION（固件版本）。
先執行 scripts/sitl.sh；用法：vendor/venv/bin/python -u -I scripts/w4_request_message.py
"""
import time

from pymavlink import mavutil

mav = mavutil.mavlink  # MAVLink 的常數與訊息編號都在這個模組裡
WINDOW = 3.0  # 每次觀察的秒數

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=10)


def command(cmd, *params):
    """送 COMMAND_LONG；params 最多 7 個，不足補 0。"""
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0,
                            *(list(params) + [0] * (7 - len(params))))


def message_id(name):
    return getattr(mav, f"MAVLINK_MSG_ID_{name}")  # 訊息編號


def observe(name):
    """觀察 WINDOW 秒，回傳 (COMMAND_ACK 的結果或 None, 收到的 name 訊息列表, 第一筆延遲秒數)。"""
    acks, got, first = [], [], None
    start = time.time()
    while time.time() - start < WINDOW:
        msg = m.recv_match(type=["COMMAND_ACK", name], blocking=True, timeout=0.5)
        if msg is None:
            continue
        if msg.get_type() == "COMMAND_ACK":
            acks.append(msg)
        else:
            got.append(msg)
            first = first if first is not None else time.time() - start
    return acks, got, first


def request_once(name):
    command(mav.MAV_CMD_REQUEST_MESSAGE, message_id(name))
    return observe(name)


# 停掉所有持續推送，讓「收到幾筆」只反映 REQUEST_MESSAGE
m.mav.request_data_stream_send(m.target_system, m.target_component, mav.MAV_DATA_STREAM_ALL, 0, 0)
command(mav.MAV_CMD_SET_MESSAGE_INTERVAL, message_id("ATTITUDE"), -1)  # -1 表示停止
time.sleep(1)
while m.recv_match(blocking=False):  # 丟掉緩衝區裡剩下的
    pass

print("== 沒要求時：觀察 3 秒 ==")
_, got, _ = observe("ATTITUDE")
print(f"ATTITUDE 收到 {len(got)} 筆")

print("\n== REQUEST_MESSAGE ATTITUDE 一次 ==")
acks, got, first = request_once("ATTITUDE")
print(f"COMMAND_ACK：{[(a.command, a.result) for a in acks]}（result 0 = ACCEPTED）")
print(f"ATTITUDE 收到 {len(got)} 筆，第一筆在 {first:.3f} 秒後" if got else "ATTITUDE 收到 0 筆")

print("\n== 再要一次 ==")
acks, got, _ = request_once("ATTITUDE")
print(f"COMMAND_ACK {len(acks)} 個；ATTITUDE 收到 {len(got)} 筆")

print("\n== REQUEST_MESSAGE AUTOPILOT_VERSION（平常不會主動送）==")
acks, got, _ = request_once("AUTOPILOT_VERSION")
print(f"COMMAND_ACK：{[(a.command, a.result) for a in acks]}")
if got:
    v = got[0].flight_sw_version  # v：固件版本，4 個位元組，依序是 major、minor、patch、type
    print(f"固件版本 {v >> 24}.{(v >> 16) & 0xFF}.{(v >> 8) & 0xFF}（type 位元組 {v & 0xFF}）")
else:
    print("沒收到 AUTOPILOT_VERSION")

w4_telemetry.py

W4 實驗 1：持續接收 ATTITUDE 與 GLOBAL_POSITION_INT，每秒印出實際頻率與最新值。

檔案位置：scripts/w4_telemetry.py

"""W4 實驗 1：持續接收 ATTITUDE 與 GLOBAL_POSITION_INT，每秒印出實際頻率與最新值。

先執行 scripts/sitl.sh；用法：vendor/venv/bin/python -u -I scripts/w4_telemetry.py [秒數]
5762 預設只送 HEARTBEAT，所以先用 SET_MESSAGE_INTERVAL 要求 ATTITUDE 10 Hz、GLOBAL_POSITION_INT 5 Hz（預設跑 10 秒）。
"""
import math
import sys
import time

from pymavlink import mavutil

DURATION = float(sys.argv[1]) if len(sys.argv) > 1 else 10.0

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
m.wait_heartbeat(timeout=10)
print(f"連線 system={m.target_system} component={m.target_component}")

# 要求訊息速率：hz = 每秒幾筆；mid = message id（訊息編號）
for name, hz in (("ATTITUDE", 10), ("GLOBAL_POSITION_INT", 5)):
    mid = getattr(mavutil.mavlink, f"MAVLINK_MSG_ID_{name}")
    m.mav.command_long_send(m.target_system, m.target_component,
                            mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0, mid, int(1e6 / hz), 0, 0, 0, 0, 0)

# counts：這一秒內每種訊息收到幾筆；latest：每種訊息最新一筆
counts = {"ATTITUDE": 0, "GLOBAL_POSITION_INT": 0}
latest = {}
start = last_print = time.time()

while time.time() - start < DURATION:
    msg = m.recv_match(type=list(counts), blocking=True, timeout=1)  # 要持續讀取，否則 SITL 會卡住
    if msg is not None:
        t = msg.get_type()  # t：訊息種類名稱
        counts[t] += 1
        latest[t] = msg
    now = time.time()
    if now - last_print >= 1.0 and len(latest) == len(counts):
        dt = now - last_print
        a, p = latest["ATTITUDE"], latest["GLOBAL_POSITION_INT"]  # a：姿態（attitude）；p：位置（position）
        print(f"ATTITUDE {counts['ATTITUDE'] / dt:5.1f} Hz  "
              f"roll={math.degrees(a.roll):6.2f}° pitch={math.degrees(a.pitch):6.2f}° "
              f"yaw={math.degrees(a.yaw):7.2f}° | "
              f"GLOBAL_POSITION_INT {counts['GLOBAL_POSITION_INT'] / dt:5.1f} Hz  "
              f"lat={p.lat / 1e7:.6f} lon={p.lon / 1e7:.6f} "
              f"alt={p.alt / 1000:.1f}m rel={p.relative_alt / 1000:.1f}m")
        counts = dict.fromkeys(counts, 0)
        last_print = now

w4_two_ports.py

W4 實驗 3：同時連 5762 與 5763，看速率設定是各自一份還是共用，以及同一個埠能不能連兩次。

檔案位置：scripts/w4_two_ports.py

"""W4 實驗 3：同時連 5762 與 5763，看速率設定是各自一份還是共用，以及同一個埠能不能連兩次。

A、B 兩條連線在同一個程式裡同時讀取（效果等同兩個程式，對 SITL 來說都是兩個 TCP 連線）。
最後試著再連一次 5762（已被 A 佔用）與 5760（被 MAVProxy 佔用），看有沒有 HEARTBEAT。
先執行 scripts/sitl.sh；用法：vendor/venv/bin/python -u -I scripts/w4_two_ports.py
"""
import time

from pymavlink import mavutil

mav = mavutil.mavlink  # MAVLink 的常數與訊息編號都在這個模組裡
WINDOW = 3.0  # 每階段觀察的秒數
ATT = mav.MAVLINK_MSG_ID_ATTITUDE  # ATTITUDE 的訊息編號

# a、b：分別連 5762、5763 的兩條 MAVLink 連線
a = mavutil.mavlink_connection("tcp:127.0.0.1:5762")
b = mavutil.mavlink_connection("tcp:127.0.0.1:5763")
for name, conn in (("A(5762)", a), ("B(5763)", b)):
    conn.wait_heartbeat(timeout=10)
    print(f"{name} 連線成功 system={conn.target_system}")


def interval(conn, hz):
    """SET_MESSAGE_INTERVAL 設定 ATTITUDE 的速率；hz = -1 表示停止。"""
    us = int(1e6 / hz) if hz > 0 else hz  # us：間隔（微秒）
    conn.mav.command_long_send(conn.target_system, conn.target_component,
                               mav.MAV_CMD_SET_MESSAGE_INTERVAL, 0, ATT, us, 0, 0, 0, 0, 0)


def measure():
    """同時讀 a、b 各 WINDOW 秒，回傳各自的 (ATTITUDE 每秒筆數, COMMAND_ACK 個數)。"""
    att, ack = {"A": 0, "B": 0}, {"A": 0, "B": 0}
    start = time.time()
    while time.time() - start < WINDOW:
        for key, conn in (("A", a), ("B", b)):
            msg = conn.recv_match(blocking=False)  # 不阻塞，輪流讀兩條連線
            while msg is not None:
                if msg.get_type() == "ATTITUDE":
                    att[key] += 1
                elif msg.get_type() == "COMMAND_ACK":
                    ack[key] += 1
                msg = conn.recv_match(blocking=False)
        time.sleep(0.002)
    return {k: v / WINDOW for k, v in att.items()}, ack


def stage(label, action):
    action()
    time.sleep(0.5)  # 讓新設定生效
    rates, acks = measure()
    print(f"{label:<34} A {rates['A']:5.1f} Hz   B {rates['B']:5.1f} Hz   ACK：A {acks['A']} 個、B {acks['B']} 個")


for conn in (a, b):  # 清掉上次實驗留下的速率
    conn.mav.request_data_stream_send(conn.target_system, conn.target_component, mav.MAV_DATA_STREAM_ALL, 0, 0)
    interval(conn, -1)
time.sleep(1)

print(f"\n{'階段':<34} ATTITUDE 實際速率")
stage("0. 都沒要求", lambda: None)
stage("1. A 要 20 Hz", lambda: interval(a, 20))
stage("2. B 再要 5 Hz", lambda: interval(b, 5))
stage("3. A 改成 2 Hz", lambda: interval(a, 2))
stage("4. B 停止", lambda: interval(b, -1))
interval(a, -1)

print("\n== 同一個埠連兩次 ==")
for port in (5762, 5760):
    extra = mavutil.mavlink_connection(f"tcp:127.0.0.1:{port}")
    hb = extra.wait_heartbeat(timeout=3)
    print(f"第二次連 {port}：{'收到 HEARTBEAT' if hb else '3 秒內沒有 HEARTBEAT'}")
    extra.close()  # 一定要關掉，否則它可能在 A 斷線後接手這個埠

w5_guided_basic.py

W5 實驗 1：Guided 模式基本流程（pymavlink）：切 GUIDED、arm、takeoff、goto、降落。

檔案位置：scripts/w5_guided_basic.py

"""W5 實驗 1：Guided 模式基本流程（pymavlink）：切 GUIDED、arm、takeoff、goto、降落。

用法：vendor/venv/bin/python -u -I scripts/w5_guided_basic.py
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762，每個階段都等條件成立（不是睡固定秒數）。
"""
import math
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
TAKEOFF_ALT = 10  # 起飛高度，公尺（相對起飛點）
GOTO_NORTH = 20  # goto 目標：往北幾公尺（相對 EKF 原點，SITL 即起飛點）
GOTO_EAST = 20  # goto 目標：往東幾公尺（相對 EKF 原點，SITL 即起飛點）
GUIDED = 4  # ArduCopter 的 GUIDED 模式編號
TIMEOUT = 60  # 每個階段最多等幾秒

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component,
                               mavutil.mavlink.MAV_DATA_STREAM_ALL, 10, 1)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def command(cmd, *params):
    """送 COMMAND_LONG 並等 COMMAND_ACK；回傳 result（0 = ACCEPTED）。"""
    p = list(params) + [0] * (7 - len(params))  # p：補滿 7 個的 COMMAND_LONG 參數（param1–param7）
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0, *p)
    ack = m.recv_match(type="COMMAND_ACK", condition=f"COMMAND_ACK.command=={cmd}",
                       blocking=True, timeout=5)
    return None if ack is None else ack.result


def wait_for(label, check):
    """每次收到 GLOBAL_POSITION_INT 就呼叫 check(msg)；成立就回傳，逾時就結束腳本。"""
    end = time.time() + TIMEOUT
    while time.time() < end:
        msg = m.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=1)
        if msg is not None and check(msg):
            log(f"{label}：完成")
            return msg
    raise SystemExit(f"逾時：{label}")


def wait_disarmed():
    """等 HEARTBEAT 的 base_mode 不再有 ARMED 旗標（落地後飛控自動 disarm）；逾時就結束腳本。
    不用高度判斷：relative_alt 是相對 home 的值，落地後可能仍讀到 1 m 以上。"""
    end = time.time() + TIMEOUT
    while time.time() < end:
        hb = m.recv_match(type="HEARTBEAT", blocking=True, timeout=1)
        if hb is None or hb.get_srcSystem() != m.target_system:  # 連線上也有 GCS（system 255）的 HEARTBEAT，要略過
            continue
        if not hb.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
            log("降落：完成（已 disarm）")
            return
    raise SystemExit("逾時：降落")


def rel_alt(msg):
    return msg.relative_alt / 1000  # relative_alt 單位 mm → m


# 1. 切 GUIDED
m.mav.set_mode_send(m.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, GUIDED)
if m.recv_match(type="HEARTBEAT", condition=f"HEARTBEAT.custom_mode=={GUIDED}",
                 blocking=True, timeout=5) is None:
    raise SystemExit("切 GUIDED 失敗")
log("模式：GUIDED")

# 2. arm（SITL 剛啟動時 EKF 還沒準備好會被拒絕，所以重試）
for _ in range(30):
    result = command(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 1)
    if result == 0:
        break
    log(f"arm 被拒絕（result={result}），1 秒後重試")
    time.sleep(1)
else:
    raise SystemExit("arm 失敗")
log("已 arm")

# 3. takeoff：高度達到目標的 95% 就算完成
log(f"takeoff 結果：{command(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, TAKEOFF_ALT)}")
wait_for("takeoff", lambda g: rel_alt(g) >= TAKEOFF_ALT * 0.95)

# 4. goto：SET_POSITION_TARGET_LOCAL_NED（local NED：x 北、y 東、z 下；負 z 表示上方）
# type_mask 只啟用位置（x, y, z），忽略速度、加速度、yaw、yaw 速率
T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
POS_ONLY = (T.POSITION_TARGET_TYPEMASK_VX_IGNORE | T.POSITION_TARGET_TYPEMASK_VY_IGNORE
            | T.POSITION_TARGET_TYPEMASK_VZ_IGNORE | T.POSITION_TARGET_TYPEMASK_AX_IGNORE
            | T.POSITION_TARGET_TYPEMASK_AY_IGNORE | T.POSITION_TARGET_TYPEMASK_AZ_IGNORE
            | T.POSITION_TARGET_TYPEMASK_YAW_IGNORE | T.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)
m.mav.set_position_target_local_ned_send(
    0, m.target_system, m.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED,
    POS_ONLY, GOTO_NORTH, GOTO_EAST, -TAKEOFF_ALT, 0, 0, 0, 0, 0, 0, 0, 0)
log(f"goto：北 {GOTO_NORTH} m、東 {GOTO_EAST} m")


def reached(_):
    pos = m.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=1)
    if pos is None:
        return False
    dist = math.hypot(pos.x - GOTO_NORTH, pos.y - GOTO_EAST)  # 到目標的水平距離，公尺
    log(f"距目標 {dist:.1f} m")
    return dist < 1.0


wait_for("goto", reached)

# 5. 降落：LAND 模式（編號 9），等飛控 disarm
m.mav.set_mode_send(m.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 9)
log("模式：LAND")
wait_disarmed()

w5_mavsdk_basic.py

W5 實驗 3：Guided 基本流程（MAVSDK-Python 4.0）：arm、takeoff、goto、降落。

檔案位置：scripts/w5_mavsdk_basic.py

"""W5 實驗 3：Guided 基本流程（MAVSDK-Python 4.0）：arm、takeoff、goto、降落。

用法：vendor/venv/bin/python -u -I scripts/w5_mavsdk_basic.py
與 scripts/w5_guided_basic.py 做同一件事，用來比較兩種寫法。
先執行 scripts/sitl.sh；本腳本聽 udpin://0.0.0.0:14550（MAVProxy 轉送的埠），所以要先關掉 QGC，
否則兩個程式搶同一個埠。MAVSDK 的 goto 吃經緯度與海拔（AMSL），不是 local NED。
"""
import asyncio
import math
import time

from mavsdk.asyncio import ComponentType, Configuration, Mavsdk
from mavsdk.asyncio.plugins.action import ActionAsync, ActionError
from mavsdk.asyncio.plugins.telemetry import TelemetryAsync

CONN = "udpin://0.0.0.0:14550"  # udpin：本機監聽（udp:// 已被 MAVSDK 標為棄用）
TAKEOFF_ALT = 10  # 起飛高度，公尺（相對起飛點）
GOTO_NORTH = 20  # goto 目標：往北幾公尺（相對起飛點）
GOTO_EAST = 20  # goto 目標：往東幾公尺（相對起飛點）
M_PER_DEG_LAT = 111_320  # 每度緯度約幾公尺（近似值）
TIMEOUT = 60  # 每個階段最多等幾秒
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


async def wait_for(label, check):
    """每個位置更新就呼叫 check(position)；成立就回傳，逾時就結束腳本。"""
    async def loop():
        async for pos in telemetry.subscribe_position():
            if check(pos):
                return pos

    try:
        pos = await asyncio.wait_for(loop(), TIMEOUT)
    except asyncio.TimeoutError:
        raise SystemExit(f"逾時：{label}")
    log(f"{label}：完成")
    return pos


async def main():
    global telemetry
    config = Configuration.create_with_component_type(ComponentType.GROUND_STATION)
    async with Mavsdk(config) as mavsdk:
        await mavsdk.add_any_connection(CONN)
        system = await mavsdk.first_autopilot(timeout_s=10.0)
        if system is None:
            raise SystemExit("找不到飛控（QGC 是否還開著？）")
        log("已連線")
        action = ActionAsync(system)  # action：arm、takeoff、goto、land 等動作
        telemetry = TelemetryAsync(system)  # telemetry：位置、狀態等遙測

        # 1. arm（EKF 還沒準備好會被拒絕，所以重試；MAVSDK 以例外表示失敗）
        for _ in range(30):
            try:
                await action.arm()
                break
            except ActionError as e:
                log(f"arm 被拒絕（{e}），1 秒後重試")
                await asyncio.sleep(1)
        else:
            raise SystemExit("arm 失敗")
        log("已 arm")

        # 2. takeoff：高度達到目標的 95% 就算完成
        await action.set_takeoff_altitude(TAKEOFF_ALT)
        await action.takeoff()
        pos = await wait_for("takeoff", lambda p: p.relative_altitude_m >= TAKEOFF_ALT * 0.95)

        # 3. goto：把「北 20 m、東 20 m」換成經緯度；高度維持（absolute_altitude_m 是 AMSL）
        lat0, lon0 = pos.latitude_deg, pos.longitude_deg  # 起點（起飛點上空）
        lat = lat0 + GOTO_NORTH / M_PER_DEG_LAT
        lon = lon0 + GOTO_EAST / (M_PER_DEG_LAT * math.cos(math.radians(lat0)))
        await action.goto_location(lat, lon, pos.absolute_altitude_m, 0)
        log(f"goto：北 {GOTO_NORTH} m、東 {GOTO_EAST} m")

        def reached(p):
            dn = (p.latitude_deg - lat) * M_PER_DEG_LAT  # 距目標：北向，公尺
            de = (p.longitude_deg - lon) * M_PER_DEG_LAT * math.cos(math.radians(lat0))  # 距目標：東向，公尺
            return math.hypot(dn, de) < 1.0

        await wait_for("goto", reached)

        # 4. 降落：落地後飛控自動 disarm；用 armed 狀態判斷（相對高度落地後可能仍讀到 1 m 以上）
        await action.land()
        log("land 指令已送出")
        async def disarmed():
            async for armed in telemetry.subscribe_armed():
                if not armed:
                    return

        try:
            await asyncio.wait_for(disarmed(), TIMEOUT)
        except asyncio.TimeoutError:
            raise SystemExit("逾時：降落")
        log("降落：完成（已 disarm）")


asyncio.run(main())

w5_velocity_square.py

W5 實驗 2：Guided 模式速度控制（pymavlink）：用速度指令飛正方形，或停止發送觀察逾時。

檔案位置：scripts/w5_velocity_square.py

"""W5 實驗 2：Guided 模式速度控制（pymavlink）：用速度指令飛正方形。

用法：vendor/venv/bin/python -u -I scripts/w5_velocity_square.py [square|timeout]
  square（預設）：起飛後依序往北、東、南、西各飛一邊，回到起點上空後降落。
  timeout：只往東飛 SIDE_SECONDS 秒，然後「停止發送」速度指令 SILENT_SECONDS 秒，
           觀察飛機什麼時候停下來（ArduCopter 的 GUID_TIMEOUT，預設 3 秒）。
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762。
速度指令（SET_POSITION_TARGET_LOCAL_NED 的 vx, vy, vz）必須持續發送，約 10 Hz。
"""
import math
import sys
import time

from pymavlink import mavutil

MODE = sys.argv[1] if len(sys.argv) > 1 else "square"
CONN = "tcp:127.0.0.1:5762"
TAKEOFF_ALT = 10  # 起飛高度，公尺（相對起飛點）
SPEED = 2.0  # 飛行速度，m/s
SIDE_SECONDS = 10  # 每邊飛幾秒（邊長約 SPEED * SIDE_SECONDS = 20 m）
SILENT_SECONDS = 8  # timeout 模式：停止發送後觀察幾秒
SEND_HZ = 10  # 速度指令發送頻率，Hz
GUIDED = 4  # ArduCopter 的 GUIDED 模式編號
LAND = 9  # ArduCopter 的 LAND 模式編號
TIMEOUT = 60  # 起飛與降落最多等幾秒

T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
# type_mask 只啟用速度（vx, vy, vz），忽略位置、加速度、yaw、yaw 速率
VEL_ONLY = (T.POSITION_TARGET_TYPEMASK_X_IGNORE | T.POSITION_TARGET_TYPEMASK_Y_IGNORE
            | T.POSITION_TARGET_TYPEMASK_Z_IGNORE | T.POSITION_TARGET_TYPEMASK_AX_IGNORE
            | T.POSITION_TARGET_TYPEMASK_AY_IGNORE | T.POSITION_TARGET_TYPEMASK_AZ_IGNORE
            | T.POSITION_TARGET_TYPEMASK_YAW_IGNORE | T.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)

# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component,
                               mavutil.mavlink.MAV_DATA_STREAM_ALL, 10, 1)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def command(cmd, *params):
    """送 COMMAND_LONG 並等 COMMAND_ACK；回傳 result（0 = ACCEPTED）。"""
    p = list(params) + [0] * (7 - len(params))  # p：補滿 7 個的 COMMAND_LONG 參數（param1–param7）
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0, *p)
    ack = m.recv_match(type="COMMAND_ACK", condition=f"COMMAND_ACK.command=={cmd}",
                       blocking=True, timeout=5)
    return None if ack is None else ack.result


def set_mode(mode):
    m.mav.set_mode_send(m.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode)


def send_velocity(vx, vy, vz):
    """local NED 速度，m/s：vx 北、vy 東、vz 下（往上要給負數）。"""
    m.mav.set_position_target_local_ned_send(
        0, m.target_system, m.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED,
        VEL_ONLY, 0, 0, 0, vx, vy, vz, 0, 0, 0, 0, 0)


def fly(label, vx, vy, seconds, send=True):
    """以固定頻率發送速度 seconds 秒（send=False 則只觀察、不發送），每秒印位置與地速。"""
    log(label)
    end = time.time() + seconds
    next_send = time.time()
    next_print = time.time()
    while time.time() < end:
        if send and time.time() >= next_send:
            send_velocity(vx, vy, 0)
            next_send += 1 / SEND_HZ
        pos = m.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=0.05)
        if pos is not None and time.time() >= next_print:
            speed = math.hypot(pos.vx, pos.vy)  # 水平地速，m/s
            log(f"  位置 北 {pos.x:6.1f} 東 {pos.y:6.1f} 高 {-pos.z:5.1f} m，地速 {speed:.2f} m/s")
            next_print += 1.0


def wait_disarmed():
    """等 HEARTBEAT 的 base_mode 不再有 ARMED 旗標（落地後飛控自動 disarm）；逾時就結束腳本。
    不用高度判斷：relative_alt 是相對 home 的值，落地後可能仍讀到 1 m 以上。"""
    end = time.time() + TIMEOUT
    while time.time() < end:
        hb = m.recv_match(type="HEARTBEAT", blocking=True, timeout=1)
        if hb is None or hb.get_srcSystem() != m.target_system:  # 連線上也有 GCS（system 255）的 HEARTBEAT，要略過
            continue
        if not hb.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
            log("降落：完成（已 disarm）")
            return
    raise SystemExit("逾時：降落")


def wait_alt(check, label):
    end = time.time() + TIMEOUT
    while time.time() < end:
        g = m.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=1)  # g：GLOBAL_POSITION_INT 訊息
        if g is not None and check(g.relative_alt / 1000):  # relative_alt 單位 mm → m
            log(f"{label}：完成")
            return
    raise SystemExit(f"逾時：{label}")


# 1. 切 GUIDED、arm（EKF 還沒準備好會被拒絕，所以重試）
set_mode(GUIDED)
if m.recv_match(type="HEARTBEAT", condition=f"HEARTBEAT.custom_mode=={GUIDED}",
                 blocking=True, timeout=5) is None:
    raise SystemExit("切 GUIDED 失敗")
log("模式：GUIDED")
for _ in range(30):
    result = command(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 1)
    if result == 0:
        break
    log(f"arm 被拒絕（result={result}），1 秒後重試")
    time.sleep(1)
else:
    raise SystemExit("arm 失敗")
log("已 arm")

# 2. 起飛
command(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, TAKEOFF_ALT)
wait_alt(lambda alt: alt >= TAKEOFF_ALT * 0.95, "takeoff")

# 3. 速度控制
if MODE == "square":
    for name, vx, vy in [("往北", SPEED, 0), ("往東", 0, SPEED), ("往南", -SPEED, 0), ("往西", 0, -SPEED)]:
        fly(f"{name} {SPEED} m/s {SIDE_SECONDS} 秒", vx, vy, SIDE_SECONDS)
    fly("煞車：速度 0 持續發送 3 秒", 0, 0, 3)
else:
    fly(f"往東 {SPEED} m/s {SIDE_SECONDS} 秒", 0, SPEED, SIDE_SECONDS)
    fly(f"停止發送 {SILENT_SECONDS} 秒，觀察何時停下來", 0, 0, SILENT_SECONDS, send=False)

# 4. 降落
set_mode(LAND)
log("模式：LAND")
wait_disarmed()

w6_failsafe_battery.py

W6 實驗 4：觸發低電量 failsafe（pymavlink），觀察飛控自己切到 RTL 並降落。

檔案位置：scripts/w6_failsafe_battery.py

"""W6 實驗 4：觸發低電量 failsafe（pymavlink），觀察飛控自己切到 RTL 並降落。

用法：vendor/venv/bin/python -u -I scripts/w6_failsafe_battery.py
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762。
做法：起飛飛到北 30 m 後，把 BATT_LOW_VOLT 調到比目前電壓高 1 V，等 BATT_LOW_TIMER 秒後飛控判定低電量，
依 BATT_FS_LOW_ACT 動作。結束時還原三個參數。
"""
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
ALT = 10  # 起飛高度，公尺（相對起飛點）
FAR = 30  # 觸發前先飛到北幾公尺，這樣才看得出 RTL 飛回來
GUIDED = 4  # ArduCopter 的 GUIDED 模式編號
TIMEOUT = 120  # 每個階段最多等幾秒
FS_LOW_ACT_RTL = 2  # BATT_FS_LOW_ACT：2 = RTL
LOW_TIMER = 2  # BATT_LOW_TIMER：電壓持續低於門檻幾秒才觸發，秒
MODES = {3: "AUTO", 4: "GUIDED", 5: "LOITER", 6: "RTL", 9: "LAND"}  # ArduCopter 模式編號 → 名稱，只用來印 log

T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component, T.MAV_DATA_STREAM_ALL, 10, 1)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def command(cmd, *params):
    """送 COMMAND_LONG 並等 COMMAND_ACK；回傳 result（0 = ACCEPTED）。"""
    p = list(params) + [0] * (7 - len(params))  # p：補滿 7 個的 COMMAND_LONG 參數（param1–param7）
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0, *p)
    ack = m.recv_match(type="COMMAND_ACK", condition=f"COMMAND_ACK.command=={cmd}",
                       blocking=True, timeout=5)
    return None if ack is None else ack.result


def get_param(name):
    m.mav.param_request_read_send(m.target_system, m.target_component, name.encode(), -1)
    msg = m.recv_match(type="PARAM_VALUE", condition=f"PARAM_VALUE.param_id=='{name}'", blocking=True, timeout=5)
    if msg is None:
        raise SystemExit(f"讀不到參數 {name}")
    return msg.param_value


def set_param(name, value):
    m.mav.param_set_send(m.target_system, m.target_component, name.encode(), value,
                         T.MAV_PARAM_TYPE_REAL32)
    msg = m.recv_match(type="PARAM_VALUE", condition=f"PARAM_VALUE.param_id=='{name}'", blocking=True, timeout=5)
    if msg is None:
        raise SystemExit(f"設定參數 {name} 沒有回應")
    log(f"{name} = {msg.param_value:g}")


def restore(originals):
    """逐一還原參數；某一個失敗不影響其他，最後回報失敗的名稱。"""
    failed = []  # 還原失敗的參數名稱
    for name, value in originals.items():
        try:
            set_param(name, value)
        except SystemExit:
            failed.append(name)
    if failed:
        log(f"警告：這些參數沒有還原成功，請手動檢查：{failed}")


def watch(until):
    """印出模式變化與飛控的 STATUSTEXT，直到 until() 為真；逾時就結束腳本。"""
    end = time.time() + TIMEOUT
    mode = None  # 目前的 custom_mode
    while time.time() < end:
        msg = m.recv_match(type=["HEARTBEAT", "STATUSTEXT"], blocking=True, timeout=1)
        if msg is None:
            continue
        if msg.get_type() == "STATUSTEXT":
            log(f"STATUSTEXT：{msg.text}")
        elif msg.get_srcSystem() == m.target_system:  # 連線上也有 GCS（system 255）的 HEARTBEAT，要略過
            if msg.custom_mode != mode:
                mode = msg.custom_mode
                log(f"模式：{MODES.get(mode, mode)}")
            if until(msg):
                return
    raise SystemExit("逾時")


def disarmed(hb):
    return not hb.base_mode & T.MAV_MODE_FLAG_SAFETY_ARMED


originals = {n: get_param(n) for n in ("BATT_LOW_VOLT", "BATT_LOW_TIMER", "BATT_FS_LOW_ACT")}  # 結束時還原
log(f"原始參數：{originals}")
try:
    set_param("BATT_FS_LOW_ACT", FS_LOW_ACT_RTL)
    set_param("BATT_LOW_TIMER", LOW_TIMER)

    # 1. GUIDED → arm（EKF 還沒好會被拒絕，所以重試）→ takeoff → 飛到北 FAR 公尺
    m.mav.set_mode_send(m.target_system, T.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, GUIDED)
    for _ in range(60):
        if command(T.MAV_CMD_COMPONENT_ARM_DISARM, 1) == 0:
            break
        time.sleep(1)
    else:
        raise SystemExit("arm 失敗")
    log("已 arm")
    command(T.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, ALT)
    end = time.time() + TIMEOUT
    while time.time() < end:
        g = m.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=1)
        if g is not None and g.relative_alt / 1000 >= ALT * 0.95:  # relative_alt 單位 mm → m
            break
    else:
        raise SystemExit("逾時：takeoff")
    log("takeoff 完成")
    m.mav.set_position_target_local_ned_send(
        0, m.target_system, m.target_component, T.MAV_FRAME_LOCAL_NED,
        T.POSITION_TARGET_TYPEMASK_VX_IGNORE | T.POSITION_TARGET_TYPEMASK_VY_IGNORE
        | T.POSITION_TARGET_TYPEMASK_VZ_IGNORE | T.POSITION_TARGET_TYPEMASK_AX_IGNORE
        | T.POSITION_TARGET_TYPEMASK_AY_IGNORE | T.POSITION_TARGET_TYPEMASK_AZ_IGNORE
        | T.POSITION_TARGET_TYPEMASK_YAW_IGNORE | T.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
        FAR, 0, -ALT, 0, 0, 0, 0, 0, 0, 0, 0)
    end = time.time() + TIMEOUT
    while time.time() < end:
        pos = m.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=1)  # pos.x：往北距離，公尺
        if pos is not None and abs(pos.x - FAR) < 1.0:
            break
    else:
        raise SystemExit(f"逾時：飛到北 {FAR} m")
    log(f"已飛到北 {FAR} m")

    # 2. 把低電量門檻調到比目前電壓高 1 V
    status = m.recv_match(type="SYS_STATUS", blocking=True, timeout=5)  # status：SYS_STATUS 訊息
    if status is None:
        raise SystemExit("收不到 SYS_STATUS")
    volt = status.voltage_battery / 1000  # mV → V
    log(f"目前電壓 {volt:.2f} V，把 BATT_LOW_VOLT 調到 {volt + 1:.2f} V")
    set_param("BATT_LOW_VOLT", volt + 1)

    # 3. 不再送任何指令，只觀察飛控自己做什麼，直到 disarm
    watch(disarmed)
    log("結束：已 disarm")
finally:
    restore(originals)

w6_geofence.py

W6 實驗 3：設圓柱 geofence（pymavlink），Guided 往外飛，觀察越界時飛控自己做什麼。

檔案位置：scripts/w6_geofence.py

"""W6 實驗 3：設圓柱 geofence（pymavlink），Guided 往外飛，觀察越界時飛控自己做什麼。

用法：vendor/venv/bin/python -u -I scripts/w6_geofence.py
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762。
做法：圍欄半徑 30 m、高度上限 20 m，越界動作 RTL；起飛到 10 m 後以速度指令往北飛，越界後只觀察。
每次位置更新印出北向距離。結束時還原參數。
注意：
- Guided 的 goto 目標若在圍欄外會被飛控直接拒絕（飛機不動），所以改用速度指令往北飛。
- Home 每次 arm 都會重設，圍欄圓心跟著動；要從起點（北 0 m）起飛才會在北 30 m 越界。
"""
import sys
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
KEEP = "keep" in sys.argv[1:]  # 帶 keep 時結束不還原參數：QGC 只在圍欄啟用時才畫圓，要截圖用
ALT = 10  # 起飛與飛行高度，公尺（相對起飛點）
SPEED = 3  # 往北飛的速度，m/s
SEND_SECONDS = 40  # 速度指令最多送幾秒（飛機越界後飛控接管，模式不再是 GUIDED 就停送）
GUIDED = 4  # ArduCopter 的 GUIDED 模式編號
TIMEOUT = 120  # 每個階段最多等幾秒
FENCE_TYPE_ALT_MAX_CIRCLE = 3  # FENCE_TYPE 是 bitmask：1 = 高度上限、2 = 以 Home 為圓心的圓；3 = 兩個都開
FENCE_ACTION_RTL_OR_LAND = 1  # FENCE_ACTION：1 = RTL（不行就 LAND）
FENCE_RADIUS = 30  # 圍欄半徑，公尺（FENCE_RADIUS 最小值就是 30）
FENCE_ALT_MAX = 20  # 圍欄高度上限，公尺（FENCE_ALT_MAX 範圍 10–1000）
MODES = {3: "AUTO", 4: "GUIDED", 5: "LOITER", 6: "RTL", 9: "LAND"}  # ArduCopter 模式編號 → 名稱，只用來印 log
PARAMS = {"FENCE_ENABLE": 1, "FENCE_TYPE": FENCE_TYPE_ALT_MAX_CIRCLE, "FENCE_ACTION": FENCE_ACTION_RTL_OR_LAND,
          "FENCE_RADIUS": FENCE_RADIUS, "FENCE_ALT_MAX": FENCE_ALT_MAX,
          "AVOID_ENABLE": 0}  # 要設定的參數；AVOID_ENABLE 預設 3（含 UseFence），會在圍欄內 2 m 把速度指令擋下來，要關掉才會越界

T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component, T.MAV_DATA_STREAM_ALL, 10, 1)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def command(cmd, *params):
    """送 COMMAND_LONG 並等 COMMAND_ACK；回傳 result（0 = ACCEPTED）。"""
    p = list(params) + [0] * (7 - len(params))  # p：補滿 7 個的 COMMAND_LONG 參數（param1–param7）
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0, *p)
    ack = m.recv_match(type="COMMAND_ACK", condition=f"COMMAND_ACK.command=={cmd}",
                       blocking=True, timeout=5)
    return None if ack is None else ack.result


def get_param(name):
    m.mav.param_request_read_send(m.target_system, m.target_component, name.encode(), -1)
    msg = m.recv_match(type="PARAM_VALUE", condition=f"PARAM_VALUE.param_id=='{name}'", blocking=True, timeout=5)
    if msg is None:
        raise SystemExit(f"讀不到參數 {name}")
    return msg.param_value


def set_param(name, value):
    m.mav.param_set_send(m.target_system, m.target_component, name.encode(), value,
                         T.MAV_PARAM_TYPE_REAL32)
    msg = m.recv_match(type="PARAM_VALUE", condition=f"PARAM_VALUE.param_id=='{name}'", blocking=True, timeout=5)
    if msg is None:
        raise SystemExit(f"設定參數 {name} 沒有回應")
    log(f"{name} = {msg.param_value:g}")


def restore(originals):
    """逐一還原參數；某一個失敗不影響其他，最後回報失敗的名稱。"""
    failed = []  # 還原失敗的參數名稱
    for name, value in originals.items():
        try:
            set_param(name, value)
        except SystemExit:
            failed.append(name)
    if failed:
        log(f"警告：這些參數沒有還原成功，請手動檢查：{failed}")


originals = {n: get_param(n) for n in PARAMS}  # 結束時還原
log(f"原始參數：{originals}")
try:
    for name, value in PARAMS.items():
        set_param(name, value)

    # 1. GUIDED → arm（EKF 還沒好會被拒絕，所以重試）→ takeoff
    m.mav.set_mode_send(m.target_system, T.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, GUIDED)
    for _ in range(60):
        if command(T.MAV_CMD_COMPONENT_ARM_DISARM, 1) == 0:
            break
        time.sleep(1)
    else:
        raise SystemExit("arm 失敗")
    log("已 arm")
    command(T.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, ALT)
    end = time.time() + TIMEOUT
    while time.time() < end:
        g = m.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=1)
        if g is not None and g.relative_alt / 1000 >= ALT * 0.95:  # relative_alt 單位 mm → m
            break
    else:
        raise SystemExit("逾時：takeoff")
    log("takeoff 完成")

    # 2. 速度指令往北（10 Hz 持續送；速度指令不像 goto 會被預先拒絕，但要關掉 AVOID_ENABLE 才飛得出圍欄），越界後飛控會改模式
    ignore = (T.POSITION_TARGET_TYPEMASK_X_IGNORE | T.POSITION_TARGET_TYPEMASK_Y_IGNORE
              | T.POSITION_TARGET_TYPEMASK_Z_IGNORE | T.POSITION_TARGET_TYPEMASK_AX_IGNORE
              | T.POSITION_TARGET_TYPEMASK_AY_IGNORE | T.POSITION_TARGET_TYPEMASK_AZ_IGNORE
              | T.POSITION_TARGET_TYPEMASK_YAW_IGNORE | T.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)
    log(f"往北 {SPEED} m/s（圍欄半徑 {FENCE_RADIUS} m）")

    # 3. 印模式變化、STATUSTEXT、每 5 m 變化一次的北向距離（離 EKF 原點），直到 disarm
    end = time.time() + TIMEOUT
    send_until = time.time() + SEND_SECONDS  # 速度指令送到這個時刻為止
    last_send = 0  # 上次送速度指令的時刻
    mode = None  # 目前的 custom_mode
    last_north = None  # 上次印出的北向距離（離 EKF 原點，取整到 5 m），公尺
    while time.time() < end:
        now = time.time()
        if mode in (None, GUIDED) and now < send_until and now - last_send >= 0.1:
            last_send = now
            m.mav.set_position_target_local_ned_send(
                0, m.target_system, m.target_component, T.MAV_FRAME_LOCAL_NED, ignore,
                0, 0, 0, SPEED, 0, 0, 0, 0, 0, 0, 0)
        msg = m.recv_match(type=["HEARTBEAT", "STATUSTEXT", "LOCAL_POSITION_NED"], blocking=True, timeout=0.1)
        if msg is None:
            continue
        kind = msg.get_type()  # kind：訊息種類
        if kind == "STATUSTEXT":
            log(f"STATUSTEXT：{msg.text}")
        elif kind == "LOCAL_POSITION_NED":
            north = round(msg.x / 5) * 5  # msg.x：往北距離，公尺
            if north != last_north:
                last_north = north
                log(f"北 {msg.x:.1f} m、高 {-msg.z:.1f} m")
        elif msg.get_srcSystem() == m.target_system:  # 連線上也有 GCS（system 255）的 HEARTBEAT，要略過
            if msg.custom_mode != mode:
                mode = msg.custom_mode
                log(f"模式：{MODES.get(mode, mode)}")
            if not msg.base_mode & T.MAV_MODE_FLAG_SAFETY_ARMED:
                log("結束：已 disarm")
                break
    else:
        raise SystemExit("逾時")
finally:
    if KEEP:
        log(f"keep：不還原參數，圍欄維持啟用；原始值是 {originals}，截圖後請自行設回")
    else:
        restore(originals)

w6_mission_square.py

W6 實驗 1：上傳正方形任務（pymavlink），切 AUTO 執行，觀察進度直到降落。

檔案位置：scripts/w6_mission_square.py

"""W6 實驗 1：上傳正方形任務（pymavlink），切 AUTO 執行，觀察進度直到降落。

用法：vendor/venv/bin/python -u -I scripts/w6_mission_square.py
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762。
任務：起飛 10 m → 四個角（北 20 m 起順時針）→ RTL。座標以腳本啟動時飛機的位置為 home。
上傳流程：MISSION_COUNT → 飛控逐項要（MISSION_REQUEST[_INT]）→ 回 MISSION_ITEM_INT → MISSION_ACK。
"""
import math
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
ALT = 10  # 起飛與航點高度，公尺（相對 home）
SIDE = 20  # 正方形邊長，公尺
M_PER_DEG_LAT = 111_320  # 每度緯度約幾公尺（近似值）
AUTO = 3  # ArduCopter 的 AUTO 模式編號
GUIDED = 4  # ArduCopter 的 GUIDED 模式編號
MISSION_TIMEOUT = 300  # 任務最多等幾秒

T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component, T.MAV_DATA_STREAM_ALL, 10, 1)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def command(cmd, *params):
    """送 COMMAND_LONG 並等 COMMAND_ACK；回傳 result（0 = ACCEPTED）。"""
    p = list(params) + [0] * (7 - len(params))  # p：補滿 7 個的 COMMAND_LONG 參數（param1–param7）
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0, *p)
    ack = m.recv_match(type="COMMAND_ACK", condition=f"COMMAND_ACK.command=={cmd}",
                       blocking=True, timeout=5)
    return None if ack is None else ack.result


def set_mode(mode):
    m.mav.set_mode_send(m.target_system, T.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode)
    hb = m.recv_match(type="HEARTBEAT", condition=f"HEARTBEAT.custom_mode=={mode}", blocking=True, timeout=5)
    if hb is None:
        raise SystemExit(f"切換模式 {mode} 失敗")


# 1. 目前位置當作 home
g = m.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=10)  # g：GLOBAL_POSITION_INT 訊息
if g is None:
    raise SystemExit("收不到 GLOBAL_POSITION_INT")
lat0, lon0 = g.lat / 1e7, g.lon / 1e7  # home 的緯度、經度，度
alt0 = g.alt / 1000  # home 的海拔（AMSL），公尺
log(f"home：{lat0:.6f}, {lon0:.6f}，海拔 {alt0:.1f} m")


def offset(north, east):
    """home 往北 north 公尺、往東 east 公尺的 (緯度, 經度)，單位 1e7 度（MISSION_ITEM_INT 的格式）。"""
    lat = lat0 + north / M_PER_DEG_LAT
    lon = lon0 + east / (M_PER_DEG_LAT * math.cos(math.radians(lat0)))
    return int(lat * 1e7), int(lon * 1e7)


# 2. 任務清單：(command, lat, lon, alt)；frame 除了第 0 項外都用相對 home 的高度
corners = [offset(SIDE, 0), offset(SIDE, SIDE), offset(0, SIDE), offset(0, 0)]
home_lat, home_lon = offset(0, 0)
items = [
    (T.MAV_CMD_NAV_WAYPOINT, T.MAV_FRAME_GLOBAL, home_lat, home_lon, alt0),  # 第 0 項：home
    (T.MAV_CMD_NAV_TAKEOFF, T.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, 0, ALT),
]
items += [(T.MAV_CMD_NAV_WAYPOINT, T.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, la, lo, ALT) for la, lo in corners]
items += [(T.MAV_CMD_NAV_RETURN_TO_LAUNCH, T.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, 0, 0)]
NAMES = {T.MAV_CMD_NAV_WAYPOINT: "WAYPOINT", T.MAV_CMD_NAV_TAKEOFF: "TAKEOFF",
         T.MAV_CMD_NAV_RETURN_TO_LAUNCH: "RTL"}  # 只用來印 log

# 3. 上傳：先清空，再依協定逐項回應
m.mav.mission_clear_all_send(m.target_system, m.target_component)
m.recv_match(type="MISSION_ACK", blocking=True, timeout=5)
m.mav.mission_count_send(m.target_system, m.target_component, len(items))
log(f"MISSION_COUNT：{len(items)} 項")
deadline = time.time() + 30  # 上傳最多等幾秒
while True:
    msg = m.recv_match(type=["MISSION_REQUEST_INT", "MISSION_REQUEST", "MISSION_ACK"],
                       blocking=True, timeout=5)
    if msg is None or time.time() > deadline:
        raise SystemExit("上傳逾時：飛控沒有回應")
    if msg.get_type() == "MISSION_ACK":
        if msg.type != T.MAV_MISSION_ACCEPTED:
            raise SystemExit(f"上傳失敗：MISSION_ACK type={msg.type}")
        log("上傳完成：MISSION_ACK ACCEPTED")
        break
    seq = msg.seq  # seq：飛控現在要第幾項
    cmd, frame, lat, lon, alt = items[seq]
    log(f"  收到 {msg.get_type()} seq={seq} → 回 {NAMES[cmd]}")
    m.mav.mission_item_int_send(m.target_system, m.target_component, seq, frame, cmd,
                                0, 1, 0, 0, 0, 0, lat, lon, alt)  # current=0、autocontinue=1、param1–4=0

# 4. GUIDED 下 arm（EKF 還沒準備好會被拒絕，所以重試），再切 AUTO
set_mode(GUIDED)
for _ in range(30):
    result = command(T.MAV_CMD_COMPONENT_ARM_DISARM, 1)
    if result == 0:
        break
    log(f"arm 被拒絕（result={result}），1 秒後重試")
    time.sleep(1)
else:
    raise SystemExit("arm 失敗")
log("已 arm")
set_mode(AUTO)
# 在地面上切到 AUTO 不會自己起飛（約 10 秒後還會因為沒起飛而自動 disarm），要再送 MISSION_START
log(f"模式：AUTO；MISSION_START 結果：{command(T.MAV_CMD_MISSION_START, 0, 0)}")

# 5. 觀察進度：MISSION_CURRENT（目前第幾項）、MISSION_ITEM_REACHED（到了第幾項）、直到 disarm
end = time.time() + MISSION_TIMEOUT
current = None  # 目前執行第幾項
while time.time() < end:
    msg = m.recv_match(type=["MISSION_CURRENT", "MISSION_ITEM_REACHED", "HEARTBEAT"],
                       blocking=True, timeout=1)
    if msg is None:
        continue
    t = msg.get_type()
    if t == "MISSION_CURRENT" and msg.seq != current:
        current = msg.seq
        log(f"目前執行第 {current} 項")
    elif t == "MISSION_ITEM_REACHED":
        log(f"到達第 {msg.seq} 項")
    elif t == "HEARTBEAT" and msg.get_srcSystem() == m.target_system \
            and not msg.base_mode & T.MAV_MODE_FLAG_SAFETY_ARMED:  # 連線上也有 GCS 的 HEARTBEAT，要略過
        log("任務結束：已 disarm")
        break
else:
    raise SystemExit("逾時：任務沒有在時限內結束")

w6_mission_timeout.py

W6 實驗 2：觀察任務上傳協定的逾時與重送（pymavlink）。

檔案位置：scripts/w6_mission_timeout.py

"""W6 實驗 2：觀察任務上傳協定的逾時與重送（pymavlink）。

用法：vendor/venv/bin/python -u -I scripts/w6_mission_timeout.py
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762。
做法：送 MISSION_COUNT 後完全不回應，印出飛控之後送來的每一則 MISSION_REQUEST[_INT] / MISSION_ACK 與時間，
最後（逾時之後）再回一次 MISSION_ITEM_INT，看飛控怎麼處理遲到的項目。結束時清空任務。
"""
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
COUNT = 3  # 宣告的任務項目數
WATCH_SECONDS = 20  # 不回應時觀察幾秒
T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def drain(seconds):
    """印出接下來 seconds 秒內收到的任務協定訊息與 STATUSTEXT。"""
    end = time.time() + seconds
    while time.time() < end:
        msg = m.recv_match(type=["MISSION_REQUEST", "MISSION_REQUEST_INT", "MISSION_ACK", "STATUSTEXT"],
                           blocking=True, timeout=0.5)
        if msg is None:
            continue
        if msg.get_type() == "STATUSTEXT":
            log(f"STATUSTEXT：{msg.text}")
        elif msg.get_type() == "MISSION_ACK":
            log(f"MISSION_ACK type={msg.type}")
        else:
            log(f"{msg.get_type()} seq={msg.seq}")


m.mav.mission_clear_all_send(m.target_system, m.target_component)
m.recv_match(type="MISSION_ACK", blocking=True, timeout=5)
m.mav.mission_count_send(m.target_system, m.target_component, COUNT)
log(f"送出 MISSION_COUNT = {COUNT}，之後不回應")
drain(WATCH_SECONDS)

# 遲到的項目：第 0 項（home 位置，內容隨便填）
log("逾時後才回 MISSION_ITEM_INT seq=0")
m.mav.mission_item_int_send(m.target_system, m.target_component, 0, T.MAV_FRAME_GLOBAL,
                            T.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, 0, 0, 0)
drain(5)

m.mav.mission_clear_all_send(m.target_system, m.target_component)
ack = m.recv_match(type="MISSION_ACK", blocking=True, timeout=5)
log(f"清空任務：{'無回應' if ack is None else f'MISSION_ACK type={ack.type}'}")

w6_waypoint_mission.py

W6 專案 1：自動飛正方形並回家（pymavlink），含安全參數與驗收。

檔案位置：scripts/w6_waypoint_mission.py

"""W6 專案 1：自動飛正方形並回家（pymavlink）。

用法：vendor/venv/bin/python -u -I scripts/w6_waypoint_mission.py
先執行 scripts/sitl.sh；本腳本連 tcp:127.0.0.1:5762。
流程：設安全參數（geofence + 低電量 RTL）→ 上傳任務 → arm → AUTO 執行 → 落地後印出驗收結果 → 還原參數。
任務：起飛 10 m → 四個角（北 20 m 起順時針）→ RTL。驗收：落地點離 Home 小於 ACCEPT_M 公尺、任務在 TIMEOUT 內結束。
"""
import math
import time

from pymavlink import mavutil

CONN = "tcp:127.0.0.1:5762"
ALT = 10  # 起飛與航點高度，公尺（相對 home）
SIDE = 20  # 正方形邊長，公尺
M_PER_DEG_LAT = 111_320  # 每度緯度約幾公尺（近似值）
AUTO = 3  # ArduCopter 的 AUTO 模式編號
GUIDED = 4  # ArduCopter 的 GUIDED 模式編號
TIMEOUT = 300  # 任務最多等幾秒
ACCEPT_M = 3  # 驗收：落地點離 Home 的水平距離上限，公尺
# 安全參數：圓柱圍欄（半徑 50 m、高度上限 20 m，越界 RTL）＋低電量 RTL
SAFETY = {
    "FENCE_ENABLE": 1,  # 1 = 啟用圍欄
    "FENCE_TYPE": 3,  # bitmask：1 = 高度上限、2 = 以 Home 為圓心的圓；3 = 兩個都開
    "FENCE_ACTION": 1,  # 越界動作：1 = RTL（不行就 LAND）
    "FENCE_RADIUS": 50,  # 圍欄半徑，公尺
    "FENCE_ALT_MAX": 20,  # 圍欄高度上限，公尺
    "BATT_FS_LOW_ACT": 2,  # 低電量動作：2 = RTL
}

T = mavutil.mavlink  # T：MAVLink 常數的命名空間，只為了縮短下面的名稱
# m：MAVLink 連線；m.mav.*_send 送出訊息，m.recv_match 接收訊息
m = mavutil.mavlink_connection(CONN)
m.wait_heartbeat(timeout=10)
m.mav.request_data_stream_send(m.target_system, m.target_component, T.MAV_DATA_STREAM_ALL, 10, 1)
t0 = time.time()  # 腳本開始時刻


def log(msg):
    print(f"[{time.time() - t0:5.1f}s] {msg}")


def command(cmd, *params):
    """送 COMMAND_LONG 並等 COMMAND_ACK；回傳 result（0 = ACCEPTED）。"""
    p = list(params) + [0] * (7 - len(params))  # p：補滿 7 個的 COMMAND_LONG 參數（param1–param7）
    m.mav.command_long_send(m.target_system, m.target_component, cmd, 0, *p)
    ack = m.recv_match(type="COMMAND_ACK", condition=f"COMMAND_ACK.command=={cmd}",
                       blocking=True, timeout=5)
    return None if ack is None else ack.result


def set_mode(mode):
    m.mav.set_mode_send(m.target_system, T.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode)
    hb = m.recv_match(type="HEARTBEAT", condition=f"HEARTBEAT.custom_mode=={mode}", blocking=True, timeout=5)
    if hb is None:
        raise SystemExit(f"切換模式 {mode} 失敗")


def get_param(name):
    m.mav.param_request_read_send(m.target_system, m.target_component, name.encode(), -1)
    msg = m.recv_match(type="PARAM_VALUE", condition=f"PARAM_VALUE.param_id=='{name}'", blocking=True, timeout=5)
    if msg is None:
        raise SystemExit(f"讀不到參數 {name}")
    return msg.param_value


def set_param(name, value):
    m.mav.param_set_send(m.target_system, m.target_component, name.encode(), value, T.MAV_PARAM_TYPE_REAL32)
    msg = m.recv_match(type="PARAM_VALUE", condition=f"PARAM_VALUE.param_id=='{name}'", blocking=True, timeout=5)
    if msg is None:
        raise SystemExit(f"設定參數 {name} 沒有回應")


def upload(items):
    """依 MAVLink 任務協定上傳 items：[(command, frame, lat, lon, alt), ...]。"""
    m.mav.mission_clear_all_send(m.target_system, m.target_component)
    m.recv_match(type="MISSION_ACK", blocking=True, timeout=5)
    m.mav.mission_count_send(m.target_system, m.target_component, len(items))
    deadline = time.time() + 30  # 上傳最多等幾秒
    while True:
        msg = m.recv_match(type=["MISSION_REQUEST_INT", "MISSION_REQUEST", "MISSION_ACK"],
                           blocking=True, timeout=5)
        if msg is None or time.time() > deadline:
            raise SystemExit("上傳逾時：飛控沒有回應")
        if msg.get_type() == "MISSION_ACK":
            if msg.type != T.MAV_MISSION_ACCEPTED:
                raise SystemExit(f"上傳失敗：MISSION_ACK type={msg.type}")
            return
        cmd, frame, lat, lon, alt = items[msg.seq]  # msg.seq：飛控現在要第幾項
        m.mav.mission_item_int_send(m.target_system, m.target_component, msg.seq, frame, cmd,
                                    0, 1, 0, 0, 0, 0, lat, lon, alt)  # current=0、autocontinue=1、param1–4=0


def local_xy():
    """回傳 (北, 東) 離 EKF 原點的距離，公尺；收不到就結束腳本。"""
    pos = m.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=5)  # pos：LOCAL_POSITION_NED 訊息
    if pos is None:
        raise SystemExit("收不到 LOCAL_POSITION_NED")
    return pos.x, pos.y


originals = {n: get_param(n) for n in SAFETY}  # 結束時還原
try:
    # 1. 安全參數
    for name, value in SAFETY.items():
        set_param(name, value)
    log("安全參數已設定：" + "、".join(f"{k}={v}" for k, v in SAFETY.items()))

    # 2. 任務：以目前位置為 home
    g = m.recv_match(type="GLOBAL_POSITION_INT", blocking=True, timeout=10)  # g：GLOBAL_POSITION_INT 訊息
    if g is None:
        raise SystemExit("收不到 GLOBAL_POSITION_INT")
    lat0, lon0 = g.lat / 1e7, g.lon / 1e7  # home 的緯度、經度，度
    alt0 = g.alt / 1000  # home 的海拔（AMSL），公尺

    def offset(north, east):
        """home 往北 north 公尺、往東 east 公尺的 (緯度, 經度)，單位 1e7 度。"""
        lat = lat0 + north / M_PER_DEG_LAT
        lon = lon0 + east / (M_PER_DEG_LAT * math.cos(math.radians(lat0)))
        return int(lat * 1e7), int(lon * 1e7)

    corners = [offset(SIDE, 0), offset(SIDE, SIDE), offset(0, SIDE), offset(0, 0)]
    home_lat, home_lon = offset(0, 0)
    items = [
        (T.MAV_CMD_NAV_WAYPOINT, T.MAV_FRAME_GLOBAL, home_lat, home_lon, alt0),  # 第 0 項：home
        (T.MAV_CMD_NAV_TAKEOFF, T.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, 0, ALT),
    ]
    items += [(T.MAV_CMD_NAV_WAYPOINT, T.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, la, lo, ALT) for la, lo in corners]
    items += [(T.MAV_CMD_NAV_RETURN_TO_LAUNCH, T.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, 0, 0)]
    upload(items)
    log(f"任務已上傳（{len(items)} 項）")

    # 3. arm（EKF 還沒準備好會被拒絕，所以重試）→ AUTO → MISSION_START
    set_mode(GUIDED)
    for _ in range(60):
        if command(T.MAV_CMD_COMPONENT_ARM_DISARM, 1) == 0:
            break
        time.sleep(1)
    else:
        raise SystemExit("arm 失敗")
    home_n, home_e = local_xy()  # 起飛點離 EKF 原點的距離，公尺（驗收用）
    log("已 arm")
    set_mode(AUTO)
    log(f"AUTO；MISSION_START 結果：{command(T.MAV_CMD_MISSION_START, 0, 0)}")

    # 4. 觀察：記錄到達的項目與最遠距離，直到 disarm
    start = time.time()  # 任務開始時刻
    end = start + TIMEOUT
    max_dist = 0.0  # 離 Home 的最大水平距離，公尺
    reached = []  # 到達的項目編號
    while time.time() < end:
        msg = m.recv_match(type=["MISSION_ITEM_REACHED", "LOCAL_POSITION_NED", "HEARTBEAT", "STATUSTEXT"],
                           blocking=True, timeout=1)
        if msg is None:
            continue
        kind = msg.get_type()  # kind：訊息種類
        if kind == "MISSION_ITEM_REACHED":
            reached.append(msg.seq)
            log(f"到達第 {msg.seq} 項")
        elif kind == "STATUSTEXT":
            log(f"STATUSTEXT：{msg.text}")
        elif kind == "LOCAL_POSITION_NED":
            max_dist = max(max_dist, math.hypot(msg.x - home_n, msg.y - home_e))
        elif msg.get_srcSystem() == m.target_system \
                and not msg.base_mode & T.MAV_MODE_FLAG_SAFETY_ARMED:  # 連線上也有 GCS 的 HEARTBEAT，要略過
            break
    else:
        raise SystemExit("逾時：任務沒有在時限內結束")
    duration = time.time() - start  # 任務耗時，秒

    # 5. 驗收
    n, e = local_xy()
    land_dist = math.hypot(n - home_n, e - home_e)  # 落地點離 Home 的水平距離，公尺
    ok = land_dist <= ACCEPT_M and all(i in reached for i in range(2, 6))
    print("—— 驗收 ——")
    print(f"耗時 {duration:.0f} 秒；到達項目 {reached}；最遠離 Home {max_dist:.1f} m；落地點離 Home {land_dist:.1f} m")
    print("結果：" + ("通過" if ok else "未通過"))
finally:
    failed = []  # 還原失敗的參數名稱
    for name, value in originals.items():
        try:
            set_param(name, value)
        except SystemExit:
            failed.append(name)
    log(f"警告：這些參數沒有還原成功，請手動檢查：{failed}" if failed else "參數已還原")