獨立章節 7 — STK 與 gRPC 通聯測試(含 Simulink / MAVLink 整合)
資料夾:
grpc_link/GrpcLink_Lab_Handout.md
實驗單 — STK 與 gRPC 通聯測試
時間:180 分鐘(可拆成兩次)
先備:Python 3.9+、grpcio、protobuf、agi.stk12 或 ansys.stk.core
選配:STK Runtime(第 3 部分起)、MATLAB + Simulink(第 5 部分)、pymavlink(第 6 部分)
第 1、2、6 部分完全不需要 STK。 沒有授權、沒有伺服器也能做完,
而且那三部分正是最常出問題的地方。
部分 0 — 環境(10 分)
cd grpc_link
python grpc_link_api.py
這一支不連 STK,只內省裝好的套件。
檢查點 0.1 兩代 API 的 grpc_port 預設值各是多少?兩個一樣嗎?
檢查點 0.2 執行結果的第四節說「只有新代才有 TLS 通道憑證」。
去原始碼裡找出這個結論是怎麼得到的 —— 它不是寫死的字串。
問題 0.3 為什麼這一支要「內省」而不是把對照表寫死?
提示:想想 Ansys 半年後改了某個方法名會怎樣。
部分 1 — 六層診斷梯(30 分)
1.1 先測一個必定失敗的目標
python grpc_link_probe.py localhost 40704 --no-stk
如果 STK 沒開,你會看到它斷在第 1 層,並印出成因與處置。
檢查點 1.1 它判成哪一個成因?耗時多少?
1.2 造出四種不同的失敗
依序試,把每一次的成因與耗時記下來:
python grpc_link_probe.py 10.255.255.1 40704 --no-stk # 不可路由
python grpc_link_probe.py no-such-host.invalid 40704 --no-stk
python grpc_link_probe.py 127.0.0.1 9 --no-stk # 幾乎不會有人用的埠
檢查點 1.2 填完這張表:
| 目標 | 成因 | 耗時 | 對應的處置 |
|---|---|---|---|
| 不可路由位址 | |||
| 解不出的名稱 | |||
| 關閉的埠 |
問題 1.3 「不可路由」與「關閉的埠」的處置完全不同。
如果程式只說「連不上」,你會先做哪一件?做錯的話會浪費多久?
1.3 造一個「有人聽但不是 gRPC」
開兩個終端機。第一個:
python -c "import socket; s=socket.socket(); s.bind(('127.0.0.1',40704)); s.listen(5); print('listening, 不說話'); input()"
第二個:
python grpc_link_probe.py 127.0.0.1 40704 --no-stk
檢查點 1.4 第 1 層過了嗎?第 2 層呢?成因是什麼?
這是整條梯子最有價值的一格 —— 它是唯一能把「伺服器沒開」
與「開錯了東西」分開的測法,而這兩種的處置完全不同。
1.4 找出那個會誤診的設定
python -c "
import sys; sys.path.insert(0,'.')
import grpc_link_probe as P, socket
def fp():
s=socket.socket(); s.bind(('127.0.0.1',0)); p=s.getsockname()[1]; s.close(); return p
for to in (1.0, 2.0, 3.0, 10.0):
r = P.layer_tcp('127.0.0.1', fp(), timeout=to)
print('timeout %5.1f -> %-18s %8.1f ms' % (to, r['cause'], r['ms']))
"
檢查點 1.5 哪些 timeout 給出 refused?哪些沒有?為什麼?
問題 1.6 這一章原本假設「被拒絕是瞬間回來的」,並用耗時當判準。
從你的量測說明為什麼那個假設是錯的,以及正確的判準應該是什麼。
部分 2 — 驗證段(15 分)
python grpc_link_verify.py
48 項,完全不需要 STK。
檢查點 2.1 第 3 類叫「失敗注入」。挑其中一項,說明它注入了什麼、
以及如果診斷邏輯壞掉,那一項會怎麼紅。
練習 2.2 — 把它弄壞。 打開 grpc_link_probe.py,把 DEFAULT_TCP_TIMEOUT
改成 1.0,再跑一次驗證段。
- 哪幾項變紅了?
- 這說明了什麼?(提示:回想檢查點 1.5)
- 改回來。
提示:想想「最該常跑的檢查」與「最少跑的檢查」。
部分 3 — 接上 STK(25 分)
從這裡開始需要 STK Runtime。
3.1 起伺服器並走完整條梯子
python -c "
from agi.stk12.stkruntime import STKRuntime
app = STKRuntime.StartApplication(grpc_host='127.0.0.1', grpc_port=40704, noGraphics=True)
print('起好了,按 Enter 關掉'); input(); app.ShutDown()
"
另一個終端機:
python grpc_link_probe.py 127.0.0.1 40704
檢查點 3.1 六層各花多久?哪一層最慢?為什麼?
3.2 第 5 層:能力面
看第 5 層印出的 Connect 指令清單。
檢查點 3.2 哪幾條 NACK 了?它們有什麼共同點?
問題 3.3 前五層全綠但某條指令 NACK —— 這算「通聯失敗」嗎?
為什麼程式把第 5 層做成「能力清單」而不是是非題?
3.3 只綁本機 vs 綁所有介面
把上面的 grpc_host 改成 '0.0.0.0' 再起一次,然後從另一台機器
(或同網段的手機熱點)測那個埠。
問題 3.4 綁 0.0.0.0 之後,同網段的任何人能對你的 STK 做什麼?
需要密碼嗎?會留下紀錄嗎?(先讀導論 §3 再回答。)
部分 4 — 成本與批次(30 分)
python grpc_link_transport.py
python grpc_link_bench.py
檢查點 4.1 填表:
| 操作 | 行程內 COM | 跨行程 gRPC | 比值 |
|---|---|---|---|
| 讀一個屬性 | |||
| 送一條 Connect 指令 |
問題 4.2 兩列的比值差很多。用「每次呼叫真正做了多少事」解釋這個差別。
問題 4.3 有人說「改用 gRPC 可以讓腳本跑得更快」。
根據你的量測,這句話在什麼情況下成立?什麼情況下是反的?
那 gRPC 真正換到的是什麼?
4.4 鏈式呼叫
看 grpc_link_bench.py 的第三項。兩段程式語意完全相同,
差別只有 root.CurrentScenario 寫在迴圈裡還是迴圈外。
檢查點 4.4 差幾倍?
問題 4.5 為什麼鏈式那一段的批次等於沒開?
(提示:批次只能攢什麼樣的呼叫?)
4.6 訊息上限
看第四項,它會逐步縮小步長直到撞上限。
檢查點 4.6 在哪一個列數撞上去?約當多少 MB?與 gRPC 的預設上限對得起來嗎?
問題 4.7 兩種處置(分段取 / 把上限開大)的代價各是什麼?
你會選哪一個,為什麼?
部分 5 — 外部模擬驅動 STK(35 分)
5.1 plant 先對閉合式
python grpc_link_realtime.py --plant
檢查點 5.1 擬圓半徑與閉合式 R = V²/(g·tanφ) 差多少?
問題 5.2 為什麼要用閉合式來對,而不是「跑出來看起來像圓就好」?
5.3 注入 STK
python grpc_link_realtime.py
檢查點 5.3 逐點與成批各花多久?快幾倍?
問題 5.4 在你量到的每點成本下,10 Hz 的迴路逐點能塞下幾架載具?
成批呢?
5.5 三個坑
打開 grpc_link_realtime.py,找到 make_realtime_aircraft()。
練習 5.5 把 route.propagate() 那一行註解掉,再跑一次。
錯誤訊息是什麼?它講得清楚嗎?改回來。
問題 5.6 ephemeris_in_latitude_longituide_altitude 這個名字拼錯了。
如果你照正確拼法寫,會發生什麼?你要怎麼才能發現真正的名字?
5.7 Simulink 橋接
先不用 MATLAB:
python grpc_link_simulink.py --fake --no-stk --drop 5 --chunk 10
檢查點 5.7 收到幾點?漏幾包?gRPC 往返幾次?
練習 5.8 把 --chunk 換成 1、5、20、60,記下往返次數與(有接 STK 時的)
每點成本。畫成曲線。
問題 5.9 攢批的代價是什麼?什麼情況下你會選 --chunk 1?
5.10 有 MATLAB 的話
cd F:\GitHub\Smallsat\grpc_link
[t, lat, lon, alt] = grpc_link_plant();
grpc_link_udp_send(0, 24.8, 121.0, 1.5) % 應回 32
注意:
grpc_link_udp_send.m與grpc_link_simulink_build.m
沒有在教材撰寫環境中執行過(當時 MATLAB 無法從命令列啟動)。
檔頭附了檢查清單,請照著走一次,不要假設它們是驗過的。
這也是一個練習:拿到別人的程式碼,先確認它被驗到什麼程度。
練習 5.11 先驗位元組排列:
typecast(double([0 24.8 121.0 1.5]), 'uint8')
list(struct.pack('<4d', 0.0, 24.8, 121.0, 1.5))
兩串 32 個數字要一模一樣。
問題 5.12 如果它們不一樣,而你沒先驗就直接接上 STK,
症狀會長什麼樣?你會往哪裡查?
部分 6 — MAVLink 與四方整合(35 分)
需要
pymavlink(不需要 ArduPilot 本體)。
6.1 MAVLink 模擬器
python grpc_link_ardupilot.py --compare
它用 pymavlink 編出真正的 MAVLink 封包再解回來。
檢查點 6.1 最大位置差多少?預測的量化界多少?
問題 6.2 這個界是怎麼算出來的?(提示:GLOBAL_POSITION_INT 的
lat/lon 欄位型別與單位。)
問題 6.3 程式說「差為 0 也算不通過」。為什麼?
在多數測試裡差為 0 是好消息,這裡為什麼是紅燈?
6.4 四方整合
python grpc_link_integration.py --dry-run
python grpc_link_integration.py --access
檢查點 6.4 三條路的預期差各是多少?為什麼不能用同一個容差?
| 比對 | 實際差 | 預期界 | 這個界從哪來 |
|---|---|---|---|
| 直接 vs Simulink(CSV) | |||
| 直接 vs MAVLink |
檢查點 6.5 三條路在 STK 裡的擬圓半徑一樣嗎?對閉合式差多少?
檢查點 6.6 三條路的可見窗一樣嗎?
問題 6.7 可見窗相同,說明 MAVLink 的 1.5 cm 量化對這個應用不重要。
那什麼樣的應用會讓同一個測試變紅?舉一個具體例子。
6.8 接真的 ArduPilot SITL(選做)
sim_vehicle.py -v ArduPlane --out=udp:127.0.0.1:14550 # ArduPilot 那側
python grpc_link_ardupilot.py --mavlink udpin:127.0.0.1:14550
問題 6.9 換成真 SITL,解碼那一側要改什麼?為什麼?
交件
一份報告,包含:
- 部分 1 的失敗成因表(四種),以及每一種的處置。
- 對問題 1.6 的回答 —— 用你自己的量測數據支持。
- 部分 4 的成本表,以及對問題 4.3 的回答。
- 練習 5.8 的 chunk 曲線,附一句話說明取捨。
- 部分 6 的三條路預期差表,以及對問題 6.3 與 6.7 的回答。
- 一段自己的話:這一章反覆強調「不要給一個聽起來很確定的錯答案」。
從你做過的練習裡挑一個例子說明,並說說它在你自己的程式裡可以怎麼用。
評分
| 項目 | 配分 |
|---|---|
| 四種失敗成因都造得出來、判得對 | 20 |
| 對「耗時不能當判準」的理解(問題 1.6) | 15 |
| 成本量測與對 gRPC 定位的理解(問題 4.3) | 15 |
| chunk 取捨曲線與說明 | 15 |
| 三條路的預期差各自不同、說得出根據 | 20 |
| 對「差為 0 也可疑」的理解(問題 6.3) | 15 |
GrpcLink_Primer.md
基礎導論 — STK 與 gRPC 通聯
0. 這一章要解決的問題
「連不上 STK」是這門課最常收到的一句回報。
它聽起來像一個問題,其實至少是七個,而且從 Python 這一側看起來幾乎一樣:
一個例外、一句英文。於是大家只能亂試 —— 重開 STK、重裝套件、關防火牆、換埠;
碰對了就過,沒碰對就卡住,而且下次再遇到還是一樣亂試。
這一章的主張只有一句:
把「通不通」拆成幾層,每一層只檢查一件事,每一層的失敗恰好對應一個成因。
這樣輸出的就不是「連不上」,而是「第 2 層過、第 3 層不過 → 那個埠有人在聽,
但聽的不是 gRPC 伺服器」。前者只能亂試,後者可以直接處理。
1. gRPC 是什麼,以及 STK 為什麼用它
1.1 一句話
gRPC 讓你像呼叫本機函式一樣,呼叫另一個行程(甚至另一台機器)裡的函式。
底下是三層:
| 層 | 用的東西 | 負責什麼 |
|---|---|---|
| 序列化 | Protocol Buffers | 把參數與回傳值變成緊湊的二進位 |
| 傳輸 | HTTP/2 | 多工、串流、標頭壓縮 |
| 傳送 | TCP | 可靠、有序的位元組流 |
這三層是分開的,而這正是診斷梯能成立的理由:TCP 通了不代表 HTTP/2 握得成手,
HTTP/2 握成了不代表對面是 STK。每一層都能單獨測。
1.2 STK 為什麼要它
STK 的舊介面是 Windows 的 COM。COM 很快(同一個行程,沒有序列化),
但它綁在 Windows、綁在同一台機器。而現代的用法是:
- 把 STK 放進容器,在叢集上跑幾十個案例
- 控制端在 Linux、在筆電、在 CI,STK 在別處
- 讓 MATLAB、C++、飛控軟體都能驅動同一個 STK
gRPC 給的是位置自由。這點很重要,因為它常被說成別的東西 ——
見 §4 的量測。
1.3 三種接法
| 接法 | 傳輸 | 有介面 | 跨機器 | 可容器化 | 什麼時候用 |
|---|---|---|---|---|---|
| STK Engine | 行程內 | 否 | 否 | 否 | 本機自動化,要最低延遲 |
| STK Desktop + COM | COM(pid) | 是 | 否 | 否 | 要看畫面,人機共用場景 |
| STK Desktop + gRPC | gRPC | 是 | 是 | 否 | 要看畫面,但控制端在別台 |
| STK Runtime | 只有 gRPC | 否 | 是 | 是 | 無介面批次、遠端佈署 |
預設埠是 40704。 記住這個數字 —— 流傳的範例常誤寫成 50051,
那是 gRPC 教學的慣用埠,不是 STK 的。
1.4 兩代 Python API
同一台機器上可以同時裝兩代,而且方法名一個都不通用:
| 舊代 | 新代(PySTK) | |
|---|---|---|
| 套件 | agi.stk12 |
ansys.stk.core |
| 命名 | PascalCase(沿用 COM) | snake_case |
| 啟動 | STKRuntime.StartApplication(...) |
STKRuntime.start_application(...) |
| 根物件 | NewObjectRoot() |
new_object_root() |
| TLS | 沒有 | grpc_channel_credentials |
最後一列是選 API 的決定性理由:舊代只有 insecure_channel。
要讓 gRPC 跨出本機,用新代。
2. 六層診斷梯
第 0 層 Python 依賴 grpcio / protobuf / STK API 在不在
第 1 層 TCP 可達 連得上那個 host:port 嗎
第 2 層 gRPC 通道就緒 HTTP/2 握手成功嗎
第 3 層 STK 交握 拿得到 application 物件嗎
第 4 層 物件模型往返 建場景、讀回名字
第 5 層 Connect 能力面 這個 host 支援哪些指令
每一層對應的成因:
| 成因 | 攔在哪一層 | 特徵 |
|---|---|---|
| grpcio / protobuf 沒裝 | 0 | 例外訊息直接講缺哪個模組 |
| gRPC 伺服器沒開 | 1 | TCP 被拒絕 |
| 防火牆丟包 / host 不存在 | 1 | TCP 逾時 |
| 埠有人聽但不是 gRPC | 2 | TCP 通、通道不就緒 |
| 伺服器只綁 127.0.0.1 | 1(從遠端測才看得到) | 本機通、遠端被拒 |
| 授權或模組不足 | 3 / 4 | 交握過了,物件模型呼叫才炸 |
| 這個 host 不支援該指令 | 5 | 前五層全綠,指令仍 NACK |
最後兩列是這一章最想教的事:通聯成功不等於功能齊全。
前四層全綠、腳本照樣跑不動,是完全正常的狀況。
2.1 第 1 層的關鍵:拒絕與逾時是兩件事
- 被拒絕(收到 RST):封包到得了那台機器,那個埠沒人在聽 → 去開伺服器
- 逾時(沒有任何回應):封包被丟掉了 → 去查防火牆 / 路由
兩者都是 OSError。把它們合併成一句「連不上」,就是把最有價值的一位資訊丟掉。
2.2 一個實際踩到的坑:不能用「快不快」來分
直覺上「被拒是瞬間、逾時要等滿」。這個直覺在 Windows 上是錯的。
實測(回文迴路):
| 設定的 timeout | 關閉的埠 | 不可路由位址 |
|---|---|---|
| 1.0 s | 1011 ms → 誤判成逾時 | 1006 ms |
| 3.0 s | 2052 ms(被拒) | 3003 ms |
| 10.0 s | 2040 ms(被拒) | 10000 ms |
| 20.0 s | 2039 ms(被拒) | 20000 ms |
作業系統回報「連線被拒」本身就要約 2040 ms(重送 SYN),而且與 timeout 無關。
對照組則一律剛好等滿 timeout。
所以正確的判準是例外型別(ConnectionRefusedError vs socket.timeout),
不是耗時。而且推出一條硬規則:
timeout 只要低於作業系統回報「被拒」所需的時間,
「伺服器沒開」就會被誤診成「防火牆丟包」。
這個誤診會把人推向完全錯誤的處置。所以本章的預設 timeout 訂在 10 秒,
而當餘裕不足又真的逾時,程式會回報「分不出來」而不是給一個確定的錯答案。
寧可說測不出來。
3. 安全:這件事要先講
STK 舊代 Python API 的 gRPC 通道是 grpc.insecure_channel ——
沒有 TLS、沒有驗證。而伺服器常被設成綁 0.0.0.0(所有介面)。
兩件事加起來的意思是:
能連到那個埠的人,就能完全操控那個 STK ——
建物件、改場景、讀資料、關掉它。沒有密碼,沒有紀錄。
所以:
- 在個人機器上做實驗:綁
127.0.0.1,不要綁0.0.0.0。 - 要跨機器:用新代 API 的
grpc_channel_credentials開 TLS,
或把它放在只有你進得去的網段/VPN 裡。 - 不要因為「反正只是教學場景」就開在系上的網路上 ——
那台機器上的授權與資料不只你的。
4. 成本模型:gRPC 到底慢在哪
4.1 一個常見說法是反的
「gRPC 比舊版的 COM 介面快上數倍」——這句話在 STK 的架構文章裡很常見。
實測同一組操作:
| 操作 | 行程內 COM | 跨行程 gRPC | 比值 |
|---|---|---|---|
| 讀一個屬性 | 0.047 ms | 1.70 ms | gRPC 慢約 36 倍 |
| 送一條 Connect 指令 | 16.3 ms | 14.0 ms | 幾乎相同 |
兩列的差別解釋了全部:
- 讀屬性幾乎不做事,量到的幾乎全是傳輸成本(序列化 + TCP 往返)。
- Connect 指令要 STK 真的去建物件,十幾毫秒都花在 STK 自己身上,
傳輸那 1.7 ms 沉在雜訊裡。
正確的說法:gRPC 的成本只在細粒度存取上顯現;而它換到的不是速度,
是位置自由。拿速度當理由選 gRPC,會在第一支逐點存取的腳本上被打回來。
4.2 所以寫法要改三件事
單次往返約 1.7 ms。結論很清楚:壓低往返次數,而不是讓每次往返更快。
(a) 迴圈裡不要放屬性存取。 這一條最隱蔽:
# 每一圈的 .CurrentScenario 都是一次往返
for i in range(n):
root.CurrentScenario.ShortDescription = "x %d" % i
# 提到迴圈外,只取一次
scen = root.CurrentScenario
for i in range(n):
scen.ShortDescription = "x %d" % i
兩段語意完全相同。在本機沒有差別,在 gRPC 上差好幾倍 ——
而且沒有任何工具會警告你。
(b) 大量寫入用批次。 new_grpc_call_batcher() 把沒有回傳值的呼叫攢起來一次送
(單趟上限 10000 筆)。代價寫在 API 自己的文件裡:批次會改變例外行為,
錯誤變成非同步冒出來、不一定落在出錯那一行。所以批次是最後才加的效能手段,
不是開發時的預設。
注意批次與 (a) 是同一件事的兩面:鏈式呼叫裡的取值會把已攢的批次沖掉,
所以不先做 (a),(b) 等於沒開。
(c) 大量讀取一次取一段。 但有上限,見下節。
4.3 訊息大小上限
gRPC 對單一訊息有接收上限,預設 4 MiB(gRPC 自己的預設值,不是 STK 訂的)。
DataProvider 一次取太多列就會撞上去:
| 步長 | 列數 | 約當大小 | 預設上限 | 上限開到 64 MB |
|---|---|---|---|---|
| 10 s | 60,481 | 0.5 MB | 過(0.80 s) | 過(1.01 s) |
| 2 s | 302,401 | 2.4 MB | 過(3.79 s) | 過(4.50 s) |
| 1 s | 604,801 | 4.8 MB | 撞上限 | 過(8.67 s) |
| 0.1 s | 6,048,002 | 48.4 MB | — | 過(108.8 s) |
處置兩條,代價不一樣:
- 分段取(改步長或切時間區間):資料量不變,訊息變小。推薦。
- 把上限開大:訊息過得去了,但整個酬載要同時擺在記憶體裡 ——
實測取 48 MB 酬載時伺服器端工作集漲到約 2.5 GB、等近兩分鐘。
它把一個乾淨的錯誤換成一次記憶體壓力。
5. 外部模擬驅動 STK
這是 gRPC 真正的用途:讓外部的東西算動力學,STK 算它擅長的
(可見度、鏈路、覆蓋、干擾)。
STK 這一側的機制是 Realtime 傳播器:星曆不由 STK 自己算,由外部餵進去。
5.1 三個實測到的坑
坑 1:飛機沒有 propagator,飛機有 route。
衛星才用 propagator。切換用 set_route_type(PropagatorType.REAL_TIME)。
坑 2:propagate() 要先呼叫,不是最後。
它的作用是「選定即時傳播器」,不是「把已加好的點跑一遍」。順序顛倒的話,
第一次加點就會收到 Realtime propagator must be selected first.
坑 3:point builder 的屬性名在庫裡拼錯了。
point_builder.ephemeris_in_latitude_longituide_altitude
^^^^^^^^^^ longituide
套件裡就是這樣拼的。照正確拼法寫會拿到 AttributeError,
而你會以為是自己記錯 API。這種東西只有內省裝好的套件才找得到。
5.2 成批注入決定能不能做即時
逐點 add_position 一次一趟往返;add_position_batch 一次送完。
實測 60 點:逐點約 1.1–1.6 ms/點,成批約 3 ms 總共 —— 快 20–34 倍。
10 Hz 的迴路每幀 100 ms 預算:
| 情境 | 每幀往返 | 每幀成本 | 結果 |
|---|---|---|---|
| 1 架、逐點 | 1 | 約 1.5 ms | 綽綽有餘 |
| 60 架、逐點 | 60 | 約 90 ms | 貼著預算,會掉幀 |
| 60 架、成批 | 1 | 約 3 ms | 綽綽有餘 |
成批不是微調,是能不能做即時的分界。
5.3 時間一定要用絕對時戳
不要用 +N sec 這種相對時間,也不要靠迴圈的節奏。外部迴路的牆鐘會抖,
而 STK 該用的是模擬時間。兩者混在一起,算出來的可見度窗會隨 CPU 負載變動 ——
而那種誤差事後查不出來。
6. 整合測試:為什麼「跑起來沒報錯」不算驗過
把 MAVLink 模擬器、Simulink、PySTK 接起來之後,每一段都會說自己成功。
整合的錯誤恰好都落在接縫上:單位換到一半、時間基準不一致、
座標系在某一段被悄悄改掉。
這一章的做法是:同一條軌跡走三條不同的路進 STK,再對帳。
而關鍵在於,每一條路的預期差是不一樣的,而且事先算得出來:
| 比對 | 預期差 | 為什麼 |
|---|---|---|
| 直接 vs Simulink(二進位 UDP) | 0 | float64,真正無損 |
| 直接 vs Simulink(CSV) | ≤ 約 0.15 mm | CSV 用 %.9f 寫經緯度,文字格式就是量化 |
| 直接 vs MAVLink | 0 < d ≤ 約 1.5 cm | GLOBAL_POSITION_INT 的 lat/lon 是 1e-7 度的整數 |
用同一個容差套三條路,等於什麼都沒驗。
而 MAVLink 那一列的兩邊都要驗:
- 超過 1.5 cm → 真的有 bug
- 剛好是 0 → 一樣可疑,表示量化沒發生,那條路可能根本沒走 MAVLink 編碼
「差為零」在多數測試裡是好消息,在這裡是紅燈。這是整合測試與
「跑起來沒報錯」最根本的差別。
6.1 最後要問「那這個差重不重要」
1.5 cm 聽起來小,但「小」要相對於用途才有意義。所以最後對三架載具
各算一次對地面站的可見窗 —— 三條逐段相同,就證明 MAVLink 的量化
對這個應用不重要。這是個量出來的結論,不是假設;
換成需要公分級的應用,同一個測試會立刻變紅。
7. 名詞
| 名詞 | 意思 |
|---|---|
| gRPC | Google 開發的遠端程序呼叫框架,走 HTTP/2 + Protocol Buffers |
| Protocol Buffers | 一種緊湊的二進位序列化格式 |
| STKRuntime | STK 的無介面執行檔,只透過 gRPC 提供服務 |
| PySTK | 新代 Python API(ansys.stk.core) |
| Realtime 傳播器 | 星曆由外部逐點餵入的傳播器 |
| GrpcCallBatcher | 把無回傳值的呼叫攢成一批送出的機制 |
| MAVLink | 無人載具常用的輕量遙測/控制協定 |
| SITL | Software In The Loop,飛控軟體在環模擬 |
| degE7 | MAVLink 的經緯度整數編碼,單位 1e-7 度 |
8. 延伸
- 把伺服器放進容器,從另一台機器連 —— 這時 §3 的安全一節不再是理論。
- 用
--chunk掃一遍,把攢批的延遲與往返數的取捨畫成曲線。 - 換成真的 ArduPilot SITL:解碼那一側不必改,因為吃的是同一套 pymavlink。
- 把 plant 換成真正的 6 自由度模型 —— 行程分離的架構下,下游一行都不用動。
GrpcLink_Simulink.md
MATLAB / Simulink 經 Python gRPC 串流至 STK — 架構說明
本文說明如何把 Simulink 的模擬狀態即時送進 STK,讓 STK 去算它擅長的事
(可見度、鏈路、覆蓋、干擾),而動力學留在 Simulink。
文中每一個數字都標了量或推:
- 量 = 在本章程式上實測得到,重跑即可複現。
- 推 = 依據文件或原理推得,尚未實測。
會這樣標,是因為這條管線最容易出錯的地方不是程式寫不出來,
而是把沒驗過的事當成驗過的。
1. 資料流
+------------------------+ +-------------------------+ +------------------------+
| MATLAB / Simulink | | Python 橋接程式 | | STK (Runtime/Desktop) |
| | | | | |
| plant(動力學) | UDP | UDP 接收 | gRPC | Realtime 傳播器 |
| -> t, lat, lon, alt | =====> | 解封包、攢批 | =====> | 星曆點 |
| | 32 B/幀| 轉絕對時戳 | HTTP/2 | -> 可見度/鏈路/覆蓋 |
| grpc_link_udp_send.m | :41000 | grpc_link_simulink.py | :40704 | |
+------------------------+ +-------------------------+ +------------------------+
步長 dt 攢 chunk 幀送一次 1 次 add_position_batch
三段各自負責一件事,而且介面是資料,不是函式呼叫。這讓任何一段
都能單獨換掉或單獨測試 —— 本章的 --fake 就是把第一段換成合成發送端,
於是整條鏈在沒有 MATLAB 的機器上也測得動。
2. 為什麼不讓 Simulink 直接呼叫 Python
最短的接法是在 MATLAB Function 區塊裡用 MATLAB 內建的 Python 介面
(pyenv / py.模組.函式)直接呼叫 PySTK,少一個行程、少一個埠。
不建議,而且在本章的撰寫環境上根本不可行。
MATLAB 的行內 Python 介面只支援它那一版明確列出的 Python 版本區間,
而 PySTK 要的是較新的 Python。兩個版本窗不重疊時,這條路整條消失,
而症狀是 pyenv 設定失敗 —— 一個完全看不出跟 STK 有關的錯誤。
撰寫環境實測(量):MATLAB R2021b,pyenv 的 Version 為空字串,
即未設定任何 Python 環境;同一台機器上的 PySTK 跑在 Python 3.13。
行程分離多一個埠,換到三件事:
| 說明 | |
|---|---|
| 版本解耦 | MATLAB 用它的,Python 用它的,互不牽動 |
| 可單獨測試 | 沒有 MATLAB 也能測整條鏈(--fake) |
| 可換任一端 | 之後換成 ROS 2、遙測 UDP、硬體迴路,只改發送端 |
3. 各段的契約
3.1 Simulink 端
責任:算出狀態,丟出去。不碰 STK,不知道 STK 存在。
- plant 的權威實作是
grpc_link_plant.m(3 自由度協調轉彎)。
它與 Python 側的grpc_link_realtime.plant()逐點吻合到
5×10⁻¹⁴ 度(量,浮點捨入量級)。 - Simulink 模型(
grpc_link_simulink_build.m)刻意不重做 plant,
而是用 From Workspace 餵那條軌跡。理由:再做一份就有三份 plant 要互相
交叉驗證,而它們的離散化只要有一點不同就對不上 ——
於是「介接對不對」會被「三份 plant 為什麼不一樣」淹掉。
要換成真正的 6 自由度模型,就把那一格換掉,其餘不動。 - 解算器設
FixedStepDiscrete,步長 = dt。 - 要開
EnablePacing。 不開的話 Simulink 用最快速度跑完,幾秒噴完全部
封包 —— 那是在測吞吐量,不是測即時串流,兩者的結論完全不同。
模型由 grpc_link_simulink_build.m 用腳本建出來,不是附一個 .slx。
理由在那支檔頭寫得很清楚,一句話:.slx 是二進位,git diff 只會說
「二進位檔不同」,助教無法在不開 MATLAB 的情況下檢查模型對不對。
四個區塊,各自只做一件事:
| 區塊 | 型別 | 做什麼 |
|---|---|---|
SimTime |
Clock | 提供模擬時間(不是牆鐘) |
Plant |
From Workspace | 餵 plant 算好的 lat / lon / alt |
SendUDP |
MATLAB Function | 呼叫 grpc_link_udp_send,送 32-byte 封包 |
Logged |
To Workspace | 在 MATLAB 這側留一份,好與 Python 對照 |
時間走 Clock,不跟資料混在一起。 把
[t lat lon alt]整個丟給
From Workspace 不會得到四個元素 —— From Workspace 是把第一欄當時間、
其餘當資料,所以輸出只有三個元素,函式裡的u(4)越界。固定尺寸訊號的
越界在編譯期就會被抓,而症狀是一句看不出原因的
Errors occurred during parsing of MATLAB function。
用 Clock 還順帶把「時間來源是模擬時間」這件事變成模型上看得見的東西。
跑完之後 MATLAB 這側的結果:
第三格才是重點:它把軌跡對外部閉合式 R = V²/(g·tanφ) 畫出來,
而不是「看起來像個圓就算了」。
3.2 線上格式
4 個 float64,little-endian,共 32 bytes: t(秒) lat(deg) lon(deg) alt(km)
代價要講清楚:UDP 不保證送達也不保證順序。因此橋接一律用封包裡的 t
MATLAB 的 typecast(double([...]),'uint8') 與 Python 的 struct.pack('<4d',...)
在 x86 上應逐位元相同(推)。請自己對一次,方法寫在
grpc_link_udp_send.m 的檔頭 —— 位元組錯位會讓軌跡變成亂數,
而症狀看起來像「STK 收到了但畫錯」。
3.3 橋接端
責任:收、攢、轉時戳、送進 STK。不做物理。
- 攢批:收滿
--chunk幀才送一次add_position_batch。
實測 117 點、chunk 20 → 6 次 gRPC 往返(量)。 - 絕對時戳:把
t轉成 STK 的絕對時間字串,不用相對時間、
也不靠迴圈的節奏。外部迴路的牆鐘會抖,而 STK 該用的是模擬時間;
兩者混在一起,算出來的可見度窗會隨 CPU 負載變動,而那種誤差事後查不出來。 - 漏包統計:用
t的缺口算,並以注入漏包驗證過統計本身
(刻意丟 3 包,回報 3 包,量)。
3.4 STK 端
責任:收星曆,算分析。
用 Realtime 傳播器。三個實測到的坑(量,全部在
grpc_link_realtime.py 的檔頭有完整說明):
- 飛機沒有
propagator,飛機有route。 衛星才用propagator。
切換用set_route_type(PropagatorType.REAL_TIME)。 propagate()要先呼叫,不是最後。 它的作用是「選定即時傳播器」。
順序顛倒會收到Realtime propagator must be selected first.- point builder 的屬性名在庫裡拼錯了:
ephemeris_in_latitude_longituide_altitude(longituide)。
不是筆誤,套件裡就是這樣拼的。
3.5 雙向:讓 STK 的判斷回頭改變飛行
上面的模型是單向的:From Workspace 餵預算好的軌跡,STK 算出來的東西
影響不了飛行。grpc_link_simulink_bidir.m 把 plant 做成模型裡的狀態,
並把回饋接進控制律,迴路才真的閉起來。
StateDelay(1/z)是故意放在迴路裡的:它把代數迴路打斷,
也讓「這一步用的是上一步的狀態」在圖上看得見。
方程只有一份
把 plant 搬進 Simulink 的直覺做法是在模型裡再寫一次方程。那會很糟 ——
教材裡就會有三份 plant(Python、grpc_link_plant.m、Simulink),
它們的離散化只要有一點不同就對不上,於是「介接對不對」這個問題
會被「三份 plant 為什麼不一樣」淹掉。
所以單步被抽到 grpc_link_step.m,grpc_link_plant.m 與 Simulink 的
PlantStep 區塊都呼叫它。「兩邊一致」因此是結構上保證的,
不是每次都要重新量的巧合。
怎麼驗一個閉迴路
兩件事要分開驗:
(a) 退化情形要回到已知答案。 把回饋強制成「永遠可用」,
控制律就退回固定坡度角,模型的軌跡必須與 grpc_link_plant() 逐點相同。
實測 400 步,差 0.000e+00 度 —— 逐位元相同。
(b) 真的閉起來時,軌跡必須不一樣。 前面全過但軌跡沒變就白做了。
實測 400 步、43.4 s 牆鐘(10 倍實時),閉迴路與開迴路的
最大差 35.5 km。
節奏與掉包
Simulink 這一側是主時鐘,不能停下來等網路。所以 grpc_link_udp_recv.m
的規則是:不阻塞、只取最新一筆、沒有就沿用上一次並把年齡加上去。
沿用是刻意的 —— 回饋掉一拍就把 link_ok 當成 false,會讓飛機因為
一個掉包而亂轉。而 age_s 一定要回傳出去:老掉的回饋和沒有回饋是兩回事,
控制律有權知道自己用的是多舊的資訊。把 age 藏起來是這類橋接最常見的坑 ——
迴路看起來在跑,其實用的是三十秒前的判斷,而且沒有任何跡象。
實測 Python 那側收到 367 / 400 步(UDP 掉包),而軌跡沒有因此發散,
正是因為沿用 + 年齡這個設計。
4. 節奏:這才是 gRPC 在這裡的重點
一次 gRPC 往返約 1.70 ms(讀屬性,回文迴路,量)。
逐點注入約 1.06–1.57 ms/點(量)。成批 60 點約 3 ms 總共,
即快 20–34 倍(量,同一次執行內比較)。
10 Hz 的迴路每幀有 100 ms 預算:
| 情境 | 每幀往返數 | 每幀成本 | 結果 |
|---|---|---|---|
| 1 架、逐點 | 1 | 約 1.5 ms | 綽綽有餘 |
| 60 架、逐點 | 60 | 約 90 ms | 貼著預算,會掉幀 |
| 60 架、成批 | 1 | 約 3 ms | 綽綽有餘 |
所以成批不是微調,而是能不能做即時的分界。
攢批的代價是延遲:最多 chunk × dt。--chunk 1 就退回逐點。
橋接結束時會印出實際往返數與每點成本,讓這個取捨可以量,而不是憑感覺調。
4.1 「gRPC 比 COM 快上數倍」是反的
這句話在 STK 架構文章裡很常見。實測(量,grpc_link_transport.py):
| 操作 | 行程內 COM | 跨行程 gRPC | 比值 |
|---|---|---|---|
| 讀一個屬性 | 0.047 ms | 1.70 ms | gRPC 慢約 36 倍 |
| 送一條 Connect 指令 | 16.3 ms | 14.0 ms | 幾乎相同 |
兩列的差別解釋了全部:讀屬性幾乎不做事,量到的幾乎全是傳輸成本;
Connect 指令要 STK 真的去建物件,十幾毫秒都花在 STK 自己身上,
傳輸那 1.7 ms 沉在雜訊裡。
正確的說法:gRPC 的成本只在細粒度存取上顯現;而 gRPC 換到的不是速度,
是位置自由 —— 跨機器、跨平台、進容器。拿速度當理由選 gRPC,
會在第一支逐點存取的腳本上被打回來。
4.2 讀回來也有上限
gRPC 對單一訊息有接收上限,預設 4 MiB(gRPC 自己的預設值,不是 STK 訂的)。
DataProvider 一次取太多列就會撞上去。實測(一週分析期間、Classical Elements,量):
| 步長 | 列數 | 約當大小 | 預設上限 | 上限開到 64 MB |
|---|---|---|---|---|
| 10 s | 60481 | 0.5 MB | 過(0.80 s) | 過(1.01 s) |
| 2 s | 302401 | 2.4 MB | 過(3.79 s) | 過(4.50 s) |
| 1 s | 604801 | 4.8 MB | 撞上限 | 過(8.67 s) |
| 0.1 s | 6048002 | 48.4 MB | — | 過(108.8 s) |
處置兩條,代價不一樣:
- 分段取(改步長或切時間區間):資料量不變,訊息變小。推薦。
- 把上限開大:訊息過得去,但整個酬載要同時擺在記憶體裡。
實測取 48 MB 酬載時伺服器端工作集漲到約 2.5 GB、等近兩分鐘(量)。
它把一個乾淨的錯誤換成一次記憶體壓力。
5. 啟動順序
順序有講究,弄反了會看到誤導的錯誤:
- 先啟動 STK 的 gRPC 伺服器。
- STK Runtime:STKRuntime.start_application(grpc_host="0.0.0.0", grpc_port=40704)
- 或 STK Desktop:啟動時帶/grpcServer On /grpcHost <host> /grpcPort <port> - 跑通聯測試。
python grpc_link_probe.py
—— 不要跳過這一步。它會在三十秒內告出是網路、交握、還是能力的問題,
而這三種的處置完全不同。 - 啟動 Python 橋接。
python grpc_link_simulink.py --udp --wait 120
它會綁 UDP 埠並等封包。首包預設等 60 秒,因為 MATLAB 冷啟動要
二三十秒 —— 用短超時會在 MATLAB 還在開機時就放棄,然後回報
「收到 0 點」,看起來像介接壞了(這是實測踩到的,量)。 - 最後跑 Simulink。
6. 座標系:要不要轉換
Realtime 傳播器的 point builder 直接吃經緯高
(ephemeris_in_latitude_longituide_altitude),所以 plant 若本來就輸出 LLA,
不需要轉換。這是最省事、也最不容易錯的一條路,本章走的就是它。
其他情況:
| plant 輸出 | 建議 |
|---|---|
| LLA(經緯高) | 直接送。本章的做法 |
| ECEF / 地固座標 | 用 ephemeris_in_central_body_fixed_frame 的 add_position(t,x,y,z),同樣不必自己轉 |
| ECI / 慣性座標 | 用 ephemeris_in_central_body_inertial_frame |
| ENU / 本地切平面 | 要自己轉,STK 這邊沒有對應入口。轉換點放在 Simulink 或橋接都可以,但要固定在一處 |
一條通則:能讓 STK 收原生座標系,就不要自己轉。 自己寫的座標轉換
是這類管線最常見的靜默錯誤來源 —— 它不會報錯,只會讓軌跡偏掉,
而偏掉的軌跡在 3D 視窗裡看起來仍然「像在飛」。
高度的基準面也要對齊:point builder 分了
..._altitude(參考橢球)與 ..._altitude_above_mean_sea_level_ /
..._above_terrain 三種入口。選錯的症狀是整條軌跡上下平移幾十公尺 ——
對可見度影響不大,對地形遮蔽的判斷影響很大。
7. 接上分析(例:與地面站的視線通斷)
注入之後,STK 這側就是一般的分析,沒有任何「即時」的特別之處:
fac = root.current_scenario.children.new(STKObjectType.FACILITY, "GS")
fac.position.assign_geodetic(24.8, 121.0, 0.0)
acc = ac.get_access_to_object(fac)
acc.compute_access()
iv = acc.computed_access_intervals.to_array(0, -1) # 每段可見窗
注意一個順序問題:注入完要先 route.propagate(),再算 access。
沒有重新傳播的話,access 看到的是舊星曆 —— 而它不會報錯,
只會給你一組對不上的可見窗。
8. 驗證狀態一覽
| 項目 | 狀態 |
|---|---|
| Python 側整條鏈(合成發送端 → UDP → 攢批 → 統計) | 已實測,含注入漏包驗證統計 |
| 漏包統計 | 已實測(丟 3 包,報 3 包) |
grpc_link_plant.m 與 Python plant 一致 |
已實測,差 5×10⁻¹⁴ 度 |
plant 對閉合式 R = V²/(g·tanφ) |
已實測,差 0.017% |
| Realtime 注入 + 讀回(PySTK over gRPC) | 已實測,讀回 61 列 |
| 逐點 vs 成批注入 | 已實測,20–34 倍 |
| COM vs gRPC 成本 | 已實測 |
| 訊息上限與記憶體代價 | 已實測 |
| 六層通聯梯的四種失敗成因 | 已實測(拒絕/逾時/DNS/非 gRPC) |
grpc_link_simulink_build.m 建出模型並跑完 |
已實測(MATLAB R2021b,4 個區塊) |
grpc_link_simulink_bidir.m 閉迴路 |
已實測,退化情形差 0.000e+00,閉迴路差 35.5 km |
grpc_link_udp_recv.m 收回饋 |
已實測,367/400 步,掉包不發散 |
| 地形載入與基準面偏移 | 已實測,偏移 10.82 m,雜訊 2.79 m |
| RF 鏈路預算對閉合式 FSPL | 已實測,最大差 0.0000 dB |
grpc_link_udp_send.m 實際送出封包 |
已實測,單獨呼叫回傳 32 bytes |
| MATLAB↔Python 位元組排列一致 | 已實測,橋接解出來的值對得上 |
| Simulink → UDP → 橋接 全程 | 已實測,3157 幀 0 漏包,壓成 64 次 gRPC 往返 |
從命令列跑這些腳本時有一個容易誤判的地方:matlab.exe 不加 -wait
會立刻返回,於是在它還沒跑完就去讀輸出檔,看起來會像 MATLAB 啟動失敗。
加上 -wait 即可。
(順帶一提,這台機器上 exit / quit 會被 STK 裝的 MATLAB Interface
啟動掛鉤弄出 Unrecognized function or variable 'Settings'。
那只影響退出,不影響腳本本身 —— 腳本的輸出在錯誤之前就寫完了。)
三個在真的跑起來之後才抓到的錯,每一個都已修進 .m 並寫進註解:
Clock沒有DecimationFactor這個參數(是Decimation,而且不必設)。- From Workspace 把矩陣第一欄當時間,所以輸出只有三個元素 ——
原本的u(4)越界,而症狀是一句看不出原因的
Errors occurred during parsing of MATLAB function。改用 Clock 供時間。 sim()回傳Simulink.SimulationOutput,To Workspace 的變數掛在它身上,
不會自動進 base workspace。用evalin('base', ...)會以為模擬失敗,
其實封包早就送出去了。
這三個都是「人工審閱看不出來、跑一次就現形」的那種錯 ——
也正是為什麼上面那張表要分「已實測」與「未執行」,而不是只寫「完成」。
9. 一份流傳範例的逐項校正
網路上關於「PySTK over gRPC 外部控制」的範例有一批共同的錯。
下表是拿裝好的套件內省得到的結果(量,grpc_link_api.py
每次執行都會重新查,所以它不會過期):
| 範例常寫的 | 實際 |
|---|---|
gRPC 埠 50051 |
40704。50051 是 gRPC 教學的慣用埠,不是 STK 的 |
attach_to_application(host=, port=) |
關鍵字是 grpc_host= / grpc_port= |
stk.AgESTKObjectType.eAircraft |
stkobjects.STKObjectType.AIRCRAFT |
AgEPropagatingMode.ePropagatingModeExternal |
沒有這個列舉 |
aircraft.propagator |
飛機沒有 propagator,飛機有 route |
set_propagating_mode(...) |
set_route_type(PropagatorType.REAL_TIME) |
add_position_llh(...) |
add_position(...),且要先取到 point builder |
attitude.add_ypr(...) |
沒有 add_ypr;姿態走 VehicleAttitudeExternal 一族 |
| 「STK 已移除 COM/AgUiAutomation 依賴」 | 不成立。agi.stk12 仍可用、AgUiAutomation.dll 仍在、AttachToApplication(pid=...) 純 COM 附著仍支援 |
| 「gRPC 比 COM 快上數倍」 | 反的(見 §4.1) |
| 「大量座標要成批送」 | 這一項是對的,而且庫裡有現成入口 add_position_batch |
root.current_time = <時間字串> 這一項是對的:該屬性存在且可寫。
這張表不是為了挑錯,它是這一章的動機:上面每一項都不會被通聯測試的
前五層攔下來。 網路全綠、交握全綠、物件模型全綠,然後在第一行業務邏輯
炸掉 —— 這就是為什麼第 6 層(能力面)必須存在,也是為什麼驗證段
不能只驗連線。
10. 檔案
| 檔案 | 作用 |
|---|---|
grpc_link_probe.py |
六層通聯診斷梯 |
grpc_link_transport.py |
三種傳輸的定位與 COM/gRPC 成本對照 |
grpc_link_bench.py |
往返延遲、批次、鏈式打斷、訊息上限 |
grpc_link_api.py |
兩代 API 對照與流傳範例校正(純內省,不連 STK) |
grpc_link_realtime.py |
Realtime 注入、plant、閉合式對照 |
grpc_link_simulink.py |
UDP → gRPC 橋接(--fake / --csv / --udp) |
grpc_link_plant.m |
plant 的 MATLAB 版(已與 Python 側對照) |
grpc_link_udp_send.m |
MATLAB 側 UDP 發送(未執行) |
grpc_link_simulink_build.m |
以腳本建出 Simulink 模型(未執行) |
grpc_link_verify.py |
驗證段 |
GrpcLink_UAV_SWIL_Gap.md
無人機 SWIL 測試平台 — 現況盤點與缺口分析
本文回答一個問題:如果要用 STK + MAVLink + MATLAB/Simulink + PySTK
搭一套無人機 SWIL 測試平台,目前這份教材裡的東西撐得起多少?還缺什麼?
寫法上只有一條規則:每一項都標明「已實測」或「未做」,不寫「應該可以」。
已實測的附上量到的數字,未做的寫清楚缺什麼、補起來大概要動到哪裡。
一、現在真的有什麼(全部實測)
| 能力 | 狀態 | 量到的數字 |
|---|---|---|
| gRPC 通聯診斷(六層梯) | 已實測 | 四種失敗成因(拒絕/逾時/DNS/非 gRPC)各自造得出來、判得對 |
| 兩代 Python API 對照 | 已實測 | 純內省,8 組改名 + 5 項流傳範例校正 |
| 傳輸成本 COM vs gRPC | 已實測 | 讀屬性 0.047 vs 1.70 ms(gRPC 慢 36 倍);Connect 指令 16.3 vs 14.0 ms(幾乎相同) |
| 單一訊息上限 | 已實測 | 預設 4 MiB;302,401 列過、604,801 列撞;開到 64 MB 的代價是伺服器工作集約 2.5 GB |
| Realtime 星曆注入(單向) | 已實測 | 逐點 1.06 ms/點;成批 60 點約 3 ms(快 20–34 倍) |
| Simulink → UDP → gRPC → STK | 已實測 | 3,157 幀、0 漏包、chunk 50 壓成 64 次 gRPC 往返 |
| MATLAB/Python 模型等價 | 已實測 | 逐點差 5×10⁻¹⁴ 度(浮點捨入) |
| MAVLink 編解碼(合成 SITL) | 已實測 | 量化界 1.5 cm(degE7),實測最大差 0.73 cm |
| 三路對帳(直接/Simulink/MAVLink) | 已實測 | 每條路的預期差各自事先算出;STK 內擬圓半徑三條完全相同 |
| 單一地面站可見窗 | 已實測 | 三條路的可見窗逐段相同 |
| 閉迴路(STK 判斷回頭改飛行) | 已實測 | 本地回饋 1.04 ms/步 → 約 61 Hz;STK access 模式見 §四之 11 |
這一組能證明的事:工具鏈接得起來、每一段的成本量得出來、
這一組不能證明的事:任何關於真實飛行、真實環境、真實無線電的事。
下面逐條說。
二、簡報大綱宣稱的能力 vs 現況
| 大綱宣稱 | 現況 | 差距 |
|---|---|---|
| ArduPilot / PX4 SITL 執行核心導航與任務調度 | 外部 MAVLink 模擬器(7 種訊息、多機) | 協定與訊息面已驗;飛控行為仍完全未驗 |
| 高精度空氣動力學、6DOF Plant Model | 已實作並驗過(配平、模態、能量) | 係數是代表值,非風洞/飛測值;無風 |
| 自訂控制演算法(LQR / H-∞) | 一條 if | 只是為了證明迴路會動,不是控制設計 |
| 多機協同(分配 / 避撞 / 隊形) | 已實作並驗過 | 分配對窮舉、避撞對注入、隊形對閉合式 |
| 台灣高精度 DEM 地形 | 已載入並驗過(GeoTIFF,30 m) | 地形跟隨與避障仍未做 |
| STK RF 鏈路預算 | 已建並驗過(2.4 GHz,FSPL 對閉合式差 0.0000 dB) | 無多路徑、無大氣以外的環境效應 |
| 閉迴路(Simulink 真的雙向) | 已實測(閉迴路與開迴路差 35.5 km) | 控制律仍是玩具 |
| EO/IR 或雷達動態 FOV、海面目標偵搜 | 完全沒有 | 未做 |
| 多機協同(Swarm)即時 SWIL | 已量到 8 架 | 上限是結構性的(見下);協同邏輯未做 |
| 低延遲、高併發 | 部分成立,且說法要修正 | 見 §三 |
| 虛擬環境完成 90% 極端情境 | 沒有依據 | 這個數字無法從現況支持 |
| HIL(硬體在環) | 完全沒有 | 目前全是 SIL |
這一輪關掉的三個缺口,以及各自量到什麼
地形(grpc_link_terrain.py)。STK 直接吃 GeoTIFF(TIFF_TERRAIN_FILE),
不必轉 DTED。載入 0.64 s,回報的涵蓋範圍與檔案逐位相符。
而對帳挖出一個比預期更重要的結論:載入時宣告的基準面(橢球 vs 平均海平面)
造成的系統性偏移實測是 10.82 m,而內插雜訊只有 2.79 m —— 差一個數量級,
所以偏移不是雜訊。但光問 STK 是問不出哪個才對的:你宣告什麼,
它就照什麼回讀,兩個自洽組合的平均差都是 −0.01 m。
正確答案只能來自 DEM 本身的來源說明(SRTM 一類是 EGM96/平均海平面)。
協同(grpc_link_swarm.py)。三件事各有一個可以證明的判準:
- 任務分配:Hungarian 對窮舉法 120/120 一致 —— 最佳性有證明。
而貪婪最近鄰平均貴 4.8%、最差 52.3%,120 個案例裡 48 個不是最優。
「要不要換成最佳化」只能這樣回答。 - 避撞:偵測與化解分開驗 —— 兩者都表現成「沒有警報」,
混在一起驗會把「沒偵測到」誤判成「化解成功」。
注入必定相撞的航線要偵測到、必定安全的要不誤報、化解後垂直間距 = 層距。 - 隊形:
v_i = ω r_i是閉合式。5 機橫隊在 1 km 轉彎中內外側需速差 48%;
速度包絡 12–25 m/s 下最小轉彎半徑 720 m —— 這是硬限制,
控制律再好也繞不過,超出就只能鬆開隊形。
六自由度(grpc_link_sixdof.py)。三層驗證,由硬到軟:
| 層 | 驗什麼 | 實測 |
|---|---|---|
| 數學恆等式 | 四元數範數 | 2.1×10⁻¹⁵(2000 步 RK4,不做每步正規化) |
| 配平 | 合力與合力矩為零 | 殘差 5.8×10⁻¹³ —— 解到機器精度 |
| 模態 | 對飛行力學的解析近似 | 短/長週期頻率比 11.4,阻尼 0.53 / 0.036 |
係數是代表值,不是任何真機的風洞或飛測結果。 所以驗的是動力學自洽
與模態結構,不是這架飛機的操穩品質 —— 把代表值的結果講成真機性能,
是這類模型最常見的誤用。
而能量檢查挖出一個真錯:殘差 3.5×10⁻³「看起來很小」,但步長細化顯示
它不收斂(收斂比 1.00)。查出來是檢查本身重算升力係數時漏了俯仰率項。
改用功-能定理之後降到 3.9×10⁻⁶,收斂比 3.25 / 3.64(二階)。
離散誤差與模型錯誤都給你一個小數字,但對步長的反應完全不同。
多機閉迴路(grpc_link_fleetloop.py)。STK 判定鏈路中斷 → 送真的
MAV_CMD_NAV_RETURN_TO_LAUNCH 回模擬器 → 載具掉頭飛回家。
整條迴路跨行程、跨協定。
行為證據(3 架、STK 真的算 access、遮蔽角 8 度):
| 載具 | RTL 時離家 | 最近 | 淨降 |
|---|---|---|---|
| 1 | 647 m | 32 m | 616 m |
| 2 | 621 m | 31 m | 590 m |
| 3 | 656 m | 31 m | 625 m |
證據看行為,不看旗標 —— 模擬器的心跳把 RTL 與 LOITER 送成同一個
custom_mode,「模式變了」在協定上看不出來;但「它回到離家 31 公尺」看得出來。
旗標只證明指令被收下,回家證明它被執行了。
閉迴路的成本(對照開迴路):
| 架數 | 幀數 | 回饋 ms/架 | 每幀 ms | 上限 Hz |
|---|---|---|---|---|
| 1 | 195 | 2.16 | 2.2 | 462 |
| 2 | 378 | 1.85 | 3.7 | 270 |
| 4 | 768 | 1.64 | 6.6 | 152 |
每幀成本與架數線性(2.2 → 3.7 → 6.6 ms),因為閉迴路不能攢批、
而且每架要依序等。不過要注意:這裡的單次 access 只要 1.6–2.4 ms,
而 grpc_link_closedloop.py 量到的是 13 ms ——
那個成本不是常數,它隨已累積的星曆成長(access 對整段星曆重算)。
兩個數字都是真的,條件不同。
這一節踩到的五個坑,形狀都一樣
沒有一個會報錯,全部只表現成「行為看起來怪怪的」——
而且五個都會讓人先去懷疑控制律:
- 遮蔽角 25 度 → 觸發距離只有 322 m,回程被待命繞圈的擺盪蓋掉。
- 遮蔽角 2 度 → 觸發距離 2,577 m,載具要飛 127 秒而只收 45 秒,
RTL 一次都沒觸發。門檻要跟觀測時間對得起來。 not ok判斷 → 把「算不出來」讀成「斷了」。None與False
在布林上都是假,但只有False該觸發 RTL。- 守衛設在點數 → 5 Hz 下 10 點只有 2 秒星曆,access 照樣算不動。
守衛要設在跨度,不是點數 —— 取樣率一變,點數的意義就變了。 LineOfSight掛在兩端 → 只從載具移除沒有用,地面站那一個照樣擋。
連帶量到一個高度邊界:90 m 時 access 恆為 0 段,100 m 起才有 ——
而模擬器的預設巡航高度正好是 90 m,卡在邊界上。
另外有一個指標自己的教訓:「有沒有飛回家」這個判準改了三次。
前兩次都在修「距離序列長得好不好看」,第三次才想通真正要問的是
「它有沒有回到家」。RTL 之後載具必須先轉向,那幾秒距離本來就會增加,
所以「單調下降」從一開始就是錯的要求。
一個指標連調三次,通常代表它量的不是你真正在問的東西。
MAVLink 來源與多機(grpc_link_mavsim.py)。改用一個外部的多機 MAVLink
模擬器取代 ArduPilot SITL。訊息面從一種變成七種(HEARTBEAT、GLOBAL_POSITION_INT、
GPS_RAW_INT、ATTITUDE、VFR_HUD、SYS_STATUS、HOME_POSITION),而且是多機。
解碼的獨立對照:從位置序列算出的地速 vs VFR_HUD 報的地速 ——
兩個不同的 MAVLink 欄位,三架都差 ±0.00 m/s。
多機的擴展成本因此從外插變成量出來的(1 / 2 / 4 / 8 架):
| 架數 | 點數 | gRPC 往返 | ms/點 | 每架往返 |
|---|---|---|---|---|
| 1 | 130 | 13 | 0.505 | 13.0 |
| 2 | 242 | 26 | 0.165 | 13.0 |
| 4 | 448 | 48 | 0.169 | 12.0 |
| 8 | 896 | 96 | 0.122 | 12.0 |
兩個結論:
- 往返次數與架數嚴格線性(每架 12–13 次,比值 1.08)。
攢批壓得掉同一架的連續點,壓不掉跨載具的 ——
因為add_position_batch掛在某一架的 point builder 上。
這是結構決定的下界,不是實作差。 - 每點成本反而隨架數下降(0.505 → 0.122 ms),固定開銷被分攤了。
所以多機的限制不在每點成本,在每幀要送幾次往返。
換算成上限(單次往返 1.7 ms):10 Hz 約 58 架、50 Hz 約 11 架。
這與先前「60 架貼著預算」的外插吻合,但現在它站在結構性理由上。
仍然沒補到的:這不是真的自駕韌體 —— 沒有 EKF、沒有控制迴路調校、
沒有失效保護行為。「飛控行為」那一欄還是未驗,不可宣稱等同 SITL。
(這個界線是模擬器作者自己寫下的,本教材照搬,不稀釋。)
RF 鏈路(grpc_link_comm.py)。2.4 GHz / 1 W / 115.2 kbps / G/T 5 dB/K。
STK 的自由空間損耗與閉合式 20log10(4πdf/c) 在 6.18–16.12 km 上
最大差 0.0000 dB —— 逐項對得上,才有資格相信含大氣與地形時的數字。
而算出來的餘裕是 50.8–59.1 dB,這本身就是結論:這條鏈路在視線內
不會因為距離而斷(要吃掉 51 dB 得把距離拉到 346 倍)。
限制因素不是距離,是遮蔽 —— 這也解釋了為什麼先前用仰角門檻當
「斷鏈」的近似還堪用:它其實是在近似遮蔽,不是在近似鏈路預算。
雙向 Simulink(grpc_link_simulink_bidir.m)。plant 變成模型裡的狀態,
回饋接進控制律。實測 400 步、43.4 s 牆鐘(10 倍實時),
閉迴路與開迴路的軌跡最大差 35.5 km。
Python 那側收到 367 步,STK access 模式每步 15.9 ms(比單向貴 24 倍),
STK 的判斷與本地近似不一致 2.2%。
退化情形的交叉驗證也保住了:把回饋強制成「永遠可用」,
模型的軌跡與 grpc_link_plant() 逐位元相同(400 步,差 0.000e+00)——
因為兩者呼叫同一份 grpc_link_step.m,不是第三份實作。
三、三個要修正的技術說法
這三點不是措辭問題 —— 照原文講,懂的人會當場質疑。
1.「STK 已移除舊版 COM / AgUiAutomation 依賴」— 不成立
實測:agi.stk12 仍可正常 import、AgUiAutomation.dll 仍在安裝目錄、
STKDesktop.AttachToApplication(pid=...) 這條純 COM 路徑仍支援。
新舊兩代 API 可以裝在同一台機器上並存。
改成:「新增 gRPC 路徑,讓自動化能跨機器、跨平台、進容器;
COM 仍在,本機且要最低延遲時它仍是最快的選擇。」
2.「gRPC 比 COM 快上數倍」— 反的
實測見 §一。細粒度存取上 gRPC 慢 36 倍;重操作上兩者幾乎相同,
因為成本由 STK 自己主導、傳輸沉在雜訊裡。
改成:「gRPC 換到的是位置自由(跨機器、容器化、多語言),
不是速度。它的成本只在細粒度存取上顯現,而那可以用批次 API 壓下去
(實測 20–34 倍)。」
3.「低延遲、高併發,支持蜂群即時 SWIL」— 目前只有一半
單向串流確實便宜(0.65 ms/點,攢批之後),但那個便宜來自攢批。
閉迴路不能攢批 —— 下一步要等這一步的答案。所以
「10 Hz 塞得下 60 架」這個結論在閉迴路下不成立,而 SWIL 的價值正在閉迴路。
改成:把單向(串流展示)與雙向(閉迴路驗證)分開講,各自給數字。
四、缺口清單(依「補起來要動多少」排序)
容易(既有章節已有現成作法,搬過來即可)
-
~~台灣地形 DEM~~ — 已完成,見上。剩下的是把地形跟隨與避障
做進控制律,以及取得涵蓋任務區的地形檔(本教材不附)。 -
~~RF 鏈路預算~~ — 已完成,見上。剩下的是多路徑與干擾。
-
EO/IR 或雷達載荷 —
eoir_earlywarning/與radar_bistatic/
兩章有完整的感測器建模與 FOV 分析,直接掛到無人機物件上即可。
中等(要新寫,但路徑清楚)
-
真實 ArduPilot / PX4 SITL — 已改用外部 MAVLink 模擬器(見上),
訊息面與多機都補上了,但飛控行為仍完全未驗
(飛行模式切換、任務執行、失效保護、EKF 收斂)。
要補這一塊只能接真的 SITL;解碼那一側不必改,
因為吃的是同一套 pymavlink,換的只有連線字串。 -
6DOF 氣動模型 — 目前是 3 自由度運動學。這台機器上
Aerospace Blockset 授權是有的,所以路徑存在;要做的是建模與風場。
注意:換掉 plant 之後,現在那個「MATLAB 與 Python 逐點吻合到 5e-14」
的交叉驗證會失效,需要換一個對照基準(例如配平條件的解析解)。 -
~~多機 / 蜂群~~ — 開迴路 8 架、閉迴路 4 架,協同三項(分配 / 避撞 /
隊形)已實作並驗過。仍缺的是任務級的協同驗證:
目前驗的是「分配是最優的」「衝突偵測得到」「隊形速度算得對」,
還沒有驗「整隊在一個真實任務裡表現如何」。 -
滑動視窗的閉迴路 — 見下一條,這是這次工作發現的真問題。
需要設計決策(不是寫程式就能解)
-
閉迴路的 STK 回饋成本會隨時間成長 —
compute_access()每一步都對
整段星曆重算,所以成本隨已注入的點數成長(O(n²))。
短測試看不出來,長任務會崩。解法是滑動視窗:只保留最近 N 秒的星曆
來算 access。但那會改變 access 的語意(看不到更早的通聯窗),
是個要明講的取捨,不是實作細節。 -
回饋忠實度 vs 迴路頻率 — 本檔提供兩種模式:
STK 真的算(忠實、貴)與本地近似(便宜、失真)。
選哪個取決於要驗什麼,而且必須量兩者判斷不一致的次數 ——
「便宜的那個夠不夠用」只能這樣回答。 -
即時性只有軟即時 — Simulink 的 pacing 貼牆鐘,但沒有 RTOS、
沒有時間戳同步、沒有延遲上界保證。要談軍規級 HIL,這一層必須補。
目前的設計刻意用絕對時戳而非牆鐘,所以模擬結果不受迴路抖動影響 ——
這是對的方向,但不等於即時性保證。 -
HIL 完全沒有 — 目前全是 SIL。要接真實飛控板,需要處理
序列埠/CAN、時間同步、以及硬體的失效注入。這是另一個層級的工作。
環境模型尚未觸及
- 風場與微氣候、電磁干擾與蓄意干擾、GNSS 拒止環境、多路徑。
這些在大綱裡都被列為賣點,但現況一項都沒有。
STK 有部分能力(干擾分析),但需要各自建模。
五、建議的最小可信示範(MVP)
如果目標是「做出一個別人會信的 demo」,建議的最短路徑是往深做一條,
不要往廣鋪四條:
- 載入一塊台灣地形(不必全島),
- 掛上一個真的 Comm 鏈路(天線 + 功率 + 餘裕門檻),
- 跑真的 ArduPilot SITL,
- 閉迴路:地形遮蔽造成鏈路餘裕不足 → 飛控自動返航,
- 全程量:回饋延遲、迴路頻率、以及STK 的判斷與本地近似不一致的次數。
這條路徑之所以可信,是因為它把「STK 算了 Simulink 算不出來的東西,
而且那個結果真的改變了飛行」這件事變成在軌跡上無法造假的證據。
其餘的能力(EO/IR、雷達、蜂群)都可以在這個骨架上加,
但先有骨架。
六、這份盤點本身的限制
- 所有數字都在單一機器、回文迴路上量到。跨機器的數字一定更差,
而且目前沒有量過。 - STK 版本與模組以實測環境為準;不同模組組合的行為可能不同。
- 「已實測」指的是在本教材的測試條件下跑過,不代表在任務級場景下成立。
圖
grpc_link/arch_dataflow.png
grpc_link/arch_fleet_cost.png
grpc_link/arch_ladder.png
grpc_link/arch_loop.png
grpc_link/arch_space_to_uav.png
grpc_link/grpc_link.png
grpc_link/sim_bidir_diagram.png
grpc_link/sim_model_diagram.png
grpc_link/sim_run_results.png
grpc_link/stk_multiview.png
程式碼
grpc_link/aero_de_deck.py
# -*- coding: utf-8 -*-
"""對外簡報:航太產業無人機研發摔機之外的選擇 —— 從太空到無人載具。
純資料,沒有副作用,不 import STK —— `tools/make_pptx.py` 會 import 它。
## 這一份與另外兩份的差別
grpc_deck.py 章節簡報。給修課的人,假設讀者會去跑那些程式。
uav_de_deck.py 無人機的數位工程。聚焦在 grpc_link 這一章的結果。
本檔 **貫穿線**:同一套方法論從衛星走到無人載具。
所以本檔的主角不是任何單一結果,而是**同一種失敗在兩個領域重複出現**。
每一組對照都來自這個教材庫裡真的記錄過的東西(各章的 findings 與檔頭),
不是為了寫簡報編出來的類比。
## 刻意不放的東西
本教材會發佈,所以不放「降低 X% 成本」「縮短 Y% 週期」這類無法從現況
簡報只負責把**可量測的事實**與**明確的邊界**講清楚。
"""
DECK = dict(
eyebrow="數位工程 · 從立方衛星到無人載具 · STK + MAVLink + Simulink + PySTK",
title="摔機之外的選擇",
subtitle="同一套驗證方法,從太空任務走到無人載具",
thesis="航太的模擬做了幾十年,而無人機研發仍然靠摔。"
"差別不在工具有多貴,在**有沒有把「這個結果憑什麼可信」寫成檢查**。"
"這份簡報講的是那套檢查長什麼樣,以及它從衛星搬到無人機時"
"**一條都沒有改**。",
scenario=[
("方法論來源", "16 週立方衛星課程 + 七個獨立章節,全部 STK 實測"),
("太空側題材", "雙基地雷達、紅外預警、入軌與交會迴避、地形傳播、"
"次軌道飛行、地月轉移"),
("無人載具側", "通聯診斷、地形遮蔽、RF 鏈路、GPS 可用度、"
"多機協同、六自由度"),
("共用的三條規矩", "ACK 不等於生效;判準要對外部來源;失敗要用注入來驗"),
("介接", "PySTK 走 gRPC;各段以**資料**為介面,不是函式呼叫"),
],
focus=[
"衛星任務裡學到的坑,在無人機上會不會重演?—— 會,而且形狀一模一樣。",
"「模擬跑起來很像」與「模擬可信」差在哪?差在有沒有可以證明的判準。",
"工具會 ACK、會回讀、會給你一個數字 —— 哪一種情況下這些都不算數?",
"把環境模型放進閉迴路要付多少延遲?本地近似便宜多少、失真多少?",
"這套東西**做不到**什麼?—— 這一頁比前面幾頁重要。",
],
# 圖的順序是**敘事順序**,不是產生順序:
# 先立論(貫穿線)、再給架構、再給工具面、最後才給量測結果。
figures=[
("arch_space_to_uav.png",
"同一種失敗在兩個領域重複出現 —— 右欄是無人載具這一章真的踩到的,"
"左欄是先前章節記錄下來的"),
("arch_dataflow.png",
"三段各做一件事,介面是**資料**不是函式呼叫 —— "
"所以換掉任一段,其他兩段不動"),
("arch_ladder.png",
"六層診斷梯:每一層的失敗恰好對應一個成因,"
"所以輸出不是「連不上」而是「斷在第幾層」"),
("stk_multiview.png",
"STK 多視角 —— 地面軌跡與斷鏈段、GPS 天空圖、仰角時序、"
"六小時的定位幾何(資料由 STK 計算)"),
("sim_model_diagram.png",
"Simulink 單向模型:時間走 Clock、狀態走 From Workspace,"
"以腳本建出(可 diff、可審)"),
("sim_bidir_diagram.png",
"Simulink 雙向模型:`1/z` 的狀態延遲把迴路閉起來,"
"回饋接進控制律"),
("arch_loop.png",
"為什麼攢批在單向有效、在閉迴路無效 —— "
"閉迴路下一步要等這一步的答案"),
("arch_fleet_cost.png",
"多機的上限是**結構**決定的:往返數與架數線性,"
"而閉迴路連攢批都沒有"),
("grpc_link.png",
"四個量出來的結論:傳輸成本、即時預算、訊息上限、"
"以及「判準是例外型別不是耗時」"),
("sim_run_results.png",
"飛行軌跡對**外部閉合式**的偏差(±6.3 m)—— 不是拿模型對自己"),
],
results_caption="全部為實測;單一機器、回文迴路。跨機器數字必然更差且尚未量過",
results=[
("同一種失敗", "太空側(既有章節)", "無人載具側(本章)"),
("ACK 了但沒生效",
"岸際雷達:SetHeightAboveGround 有 ACK,回讀仍是 False",
"存取約束掛在**兩端**,只移除一端沒有用"),
("失效時不報錯,直接停住",
"物件離開地形檔範圍 -> ComputeAccess 再也不回來",
"星曆太短 -> access 回空清單,被讀成「鏈路斷了」"),
("通過檢查卻毫無影響",
"TIREM 地表電性:範圍檢查過、回讀正確,輸出差 0.000 dB",
"指令送進沒人聽的埠(推播埠 vs 綁定埠)"),
("自洽但都錯",
"次軌道:兩個量用同一套錯記帳算,互相對照一定過",
"能量檢查重算係數、漏了俯仰率項 —— 殘差不隨步長收斂"),
("編出來的公式通過了驗證",
"推進週:阻力 Δv 是一條擬合指數,連自己的說明都對不上",
"6DOF 係數是**代表值**,已明確標註非風洞/飛測"),
("多條路互相驗證",
"地月轉移:解析 / 純 Python 數值 / STK Astrogator 三路一致",
"直接 / Simulink / MAVLink 三路進 STK,**預期差各自算出**"),
],
findings=[
"A|**同一種失敗會在兩個領域重演,而且沒有一個會報錯。** "
"右邊那一欄不是類比,是這一章真的踩到的;左邊那一欄是"
"先前章節記錄下來的。工具不會告訴你它算的不是你以為的東西。",
"B|**判準要對外部來源。** 地月轉移拿公開的影響球半徑與 LOI 值對;"
"無人機這邊拿自由空間損耗的閉合式(差 0.0000 dB)、GPS 標稱軌道半徑"
"(差 0.004%)、以及 DOP 的數學恆等式(殘差 8.9e-16)。"
"拿模型對自己只證明它自洽。",
"C|**失敗要用注入來驗。** 「偵測得到」與「沒有東西可偵測」"
"都表現成沒有警報 —— 所以要造出真的失敗:四種通聯失效、"
"對撞航線、退化幾何、刻意漏包、把舊公式塞回去。",
"D|**一個「很小」的殘差可能藏著真錯。** 6DOF 的能量殘差 3.5e-3"
"看起來很小,但不隨步長收斂 —— 那不是離散誤差。"
"改用功-能定理後降到 3.9e-6,收斂比 3.25/3.64(二階)。"
"**離散誤差與模型錯誤都給你一個小數字。**",
"E|**這套東西不能取代什麼,要先講。** 它把鏈路、地形、GPS、"
"多機衝突的失效提前到地面上重現;它**不**驗真實韌體行為、"
"不驗真機操穩品質、沒有硬體在環、沒有電磁層建模。"
"一份只講得到的東西的簡報,在評審席上撐不過三個問題。",
],
verify=(136, 0, 0),
verify_note=[
"其中多項是**失敗注入**:造出真的失敗(拒絕/逾時/DNS/非 gRPC 伺服器、"
"對撞航線、退化幾何、漏包、把舊公式塞回去),看診斷指不指得對。",
"另有 27 項**需要 STK**,驗的是 STK 自己:GPS almanac 給的星座"
"(對公開標稱值,不對 STK 自己)、Realtime 注入的往返完整性、"
"存取約束有沒有生效,以及**閉迴路有沒有真的改變飛行**。",
],
run=[
"python grpc_link_probe.py 通聯診斷(不需 STK)",
"python grpc_link_fleetloop.py 多機閉迴路 + 返航行為證據",
"python grpc_link_swarm.py 分配(對窮舉)/ 避撞 / 隊形",
"python grpc_link_sixdof.py 配平、模態、能量收斂",
"python grpc_link_verify.py 136 項(不需 STK)",
"python grpc_link_stk_verify.py 27 項(需 STK)",
],
files=[
("**做得到**", "把鏈路、地形、GPS、多機衝突的失效提前到地面上重現"),
("**做不到**(一)", "真實自駕韌體行為:EKF、失效保護、任務狀態機"),
("**做不到**(二)", "真機操穩品質 —— 6DOF 用代表性係數,非風洞值"),
("**做不到**(三)", "硬體在環:目前全是軟體在環,沒有時序保證"),
("**做不到**(四)", "多路徑、蓄意干擾、GNSS 拒止的電磁層建模"),
("需要自備", "STK 授權;地形 DEM;MAVLink 模擬器(皆不隨教材附上)"),
("最短可信路徑", "一塊地形 + 一條真 Comm 鏈路 + 真 SITL + 閉迴路返航"),
],
)
grpc_link/grpc_deck.py
# -*- coding: utf-8 -*-
"""獨立章節 7 的簡報內容。
純資料,沒有副作用,不 import STK —— `tools/make_pptx.py` 會 import 它。
數字全部來自本章各模組的實際輸出(grpc_link_*.json 與各支的終端輸出)。
**這份簡報刻意不含業務效益頁。** 原始需求的大綱第五部分是
「預期效益與產業價值」,本檔把它換成「量得出來的能力與缺口」——
因為這份教材會發佈,而可量測的事實比價值主張耐得住檢驗,
也才擋得住評審席上「這個數字哪來的」這個問題。
"""
DECK = dict(
eyebrow="獨立章節 7 · STK + PySTK(gRPC)+ Simulink + MAVLink",
title="工具鏈本身也要驗",
subtitle="無人機 SWIL:把「連不上」拆成六層,把「夠快嗎」變成數字",
thesis="「連不上 STK」聽起來像一個問題,實際上至少是七個,"
"而從 Python 這一側看起來幾乎一樣。這一章把整條工具鏈拆開來量 ——"
"包含推翻三個廣為流傳、但實測站不住的說法。",
scenario=[
("STK 側", "STK Runtime(無介面),gRPC 伺服器預設埠 40704"),
("Python API", "agi.stk12(舊代)與 ansys.stk.core / PySTK(新代)並存"),
("飛行模型", "3 自由度協調轉彎;閉合解 R = V²/(g·tanφ) 可獨立對照"),
("Simulink", "模型以腳本建(非 .slx):Clock + From Workspace + "
"MATLAB Function + To Workspace"),
("線上格式", "4 個 float64 little-endian,32 bytes:t / lat / lon / alt"),
("MAVLink", "pymavlink 編出真實 GLOBAL_POSITION_INT(lat/lon 為 1e-7 度整數)"),
("閉迴路", "STK 算地面站可見性或 GPS 定位品質,回頭改變飛行"),
("GPS 星座", "STK 內建 almanac,實測 32 個 PRN(非標稱 Walker 近似)"),
],
focus=[
"「連不上」有七個成因,而處置完全不同 —— 怎麼在三十秒內分辨?",
"gRPC 真的比 COM 快嗎?快在哪、慢在哪、差幾倍?",
"一次能從 STK 讀回多少資料?撞到上限會怎樣?把上限開大的代價是什麼?",
"同一條軌跡走三條路進 STK,它們應該差多少?—— 而差為零為什麼也算不通過?",
"閉迴路比單向貴多少?把環境模型放進迴路 vs 在本地近似,失真多少?",
],
figures=[
("grpc_link.png",
"四個量出來的結論 —— 傳輸成本、即時預算、訊息上限、"
"以及「判準是例外型別不是耗時」"),
("sim_model_diagram.png",
"Simulink 模型(由 grpc_link_simulink_build.m 以腳本建出,可 diff、可審)"),
("sim_run_results.png",
"MATLAB 側跑滿 1.1 圈:地面軌跡、爬升、以及對閉合式的偏差(±6.3 m)"),
],
results_caption="全部為實測,單一機器、回文迴路;跨機器數字必然更差且尚未量過",
results=[
("量", "結果", "說明"),
("讀屬性:行程內 COM vs gRPC", "0.047 vs 1.70 ms", "gRPC 慢 36 倍"),
("Connect 指令:COM vs gRPC", "16.3 vs 14.0 ms", "幾乎相同,成本在 STK 自己"),
("gRPC 單一訊息上限", "4 MiB(302,401 列過 / 604,801 列撞)", "gRPC 預設值"),
("上限開到 64 MB 取 48 MB", "108.8 s,伺服器工作集約 2.5 GB",
"把乾淨的錯誤換成記憶體壓力"),
("Realtime 注入:逐點 vs 成批", "1.06 ms/點 vs 60 點約 3 ms", "快 20–34 倍"),
("Simulink → UDP → gRPC → STK", "3,157 幀 0 漏包,壓成 64 次往返",
"chunk 50"),
("MAVLink 量化界(預測 vs 實測)", "1.50 cm vs 0.73 cm", "degE7 整數編碼"),
("閉迴路:STK 算 vs 本地近似", "13.2 ms(8–11 Hz)vs 1.04 ms(61 Hz)",
"貴 17–21 倍;兩者判斷不一致 4.2%"),
("GPS 軌道半徑(STK almanac)", "26,560.99 km vs 公開 26,560 km", "差 0.004%"),
("GPS 可用度 @ 遮蔽角 30°", "可見 5–7 顆,HDOP 1.16–4.19,可用 96.4%",
"開闊天空 10° 則 100%"),
("DOP 恆等式殘差", "8.9×10⁻¹⁶", "GDOP²=PDOP²+TDOP²"),
],
findings=[
"A|**「gRPC 比 COM 快上數倍」是反的。** 細粒度存取慢 36 倍,"
"重操作幾乎相同。gRPC 換到的是**位置自由**,不是速度。"
"(「STK 已移除 COM 依賴」也不成立。)",
"C|**「被拒絕是瞬間回來的」在 Windows 上是錯的。** 實測要約 2,040 ms,"
"所以 timeout < 2 s 會把「伺服器沒開」誤診成「防火牆丟包」。"
"判準必須是**例外型別**,不是耗時 —— 這是本章自己的 bug。",
"D|**通聯成功不等於功能齊全。** Runtime 上設定類指令全 ACK,"
"Units_Get 與 ReportCreate 一律 NACK。第六層回報的是能力清單,"
"不是是非題。",
"E|**整合測試的價值在於每條路的預期差不同,而且事先算得出來。** "
"CSV 走 1e-9 度、MAVLink 走 1e-7 度,兩界比值正好 100。而 MAVLink 那條"
"**差為零也算不通過**。",
"F|**閉迴路不能攢批**,而攢批是單向便宜的唯一原因。"
"開迴路每架 12-13 次往返、8 架很輕鬆;閉迴路每架每幀一次查詢"
"且要依序等,每幀 2.2 / 3.7 / 6.6 ms(1 / 2 / 4 架)。",
"G|**證據看行為,不看旗標。** 模擬器把 RTL 與 LOITER 送成同一個"
"custom_mode,「模式變了」在協定上看不出來;但三架都從 620-660 m"
"回到離家 31 m,那看得出來。旗標只證明指令被收下。",
],
verify=(111, 0, 0),
verify_note=[
"封包格式、攢批算術、DOP 的數學恆等式都不在 STK 裡 ——"
"綁上授權只會讓最該常跑的檢查變成最少跑的。",
"其中第 3 與第 5 類是**失敗注入**:真的造出四種失敗"
"(拒絕 / 逾時 / DNS / 非 gRPC 伺服器),看診斷指到對的成因。",
"另有 27 項**需要 STK**(grpc_link_stk_verify.py),驗的是 STK 自己:"
"almanac 給的星座對不對(對公開標稱值,不對 STK 自己)、Realtime 注入的"
"往返完整性、存取約束有沒有生效,以及最重要的 ——"
"**閉迴路有沒有真的改變飛行**。前面全過但軌跡沒變,就白做了。",
],
run=[
"python grpc_link_probe.py 六層通聯診斷(不需 STK)",
"python grpc_link_transport.py COM vs gRPC 成本",
"python grpc_link_bench.py 延遲、批次、訊息上限",
"python grpc_link_integration.py --access 四方整合 + 可見窗",
"python grpc_link_gps.py 雙向子項:GPS 可用度",
"python grpc_link_terrain.py --dem <路徑> 地形載入與基準面對帳",
"python grpc_link_comm.py RF 鏈路預算對閉合式 FSPL",
"python grpc_link_mavsim.py --scaling 多機開迴路的擴展成本",
"python grpc_link_fleetloop.py 多機閉迴路 + RTL 行為證據",
"python grpc_link_verify.py 驗證段 136 項(不需 STK)",
"python grpc_link_stk_verify.py STK 驗證段 27 項(需 STK)",
],
files=[
("grpc_link_probe.py / _api.py", "六層診斷梯;兩代 API 對照(純內省)"),
("grpc_link_bench.py / _transport.py", "延遲、批次、訊息上限;COM vs gRPC"),
("grpc_link_integration.py", "四方整合:每條路的預期差各自算出"),
("grpc_link_closedloop.py / _gps.py", "雙向:鏈路可用性與 GPS 定位品質"),
("grpc_link_terrain.py / _comm.py", "真實 DEM 與 RF 鏈路預算"),
("grpc_link_mavsim.py / _fleetloop.py", "MAVLink 模擬器;多機開/閉迴路"),
("*.m(6 支)", "MATLAB / Simulink 側(含雙向模型)"),
("GrpcLink_*.md(4 份)", "導論、實驗單、架構說明、SWIL 缺口分析"),
],
)
grpc_link/grpc_link_api.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 兩代 Python API 對照,以及一份網路範例的逐項校正。
## 為什麼這一支要存在
STK 的 Python API 有**兩代**,而且它們在同一台機器上可以同時裝著:
agi.stk12 舊代。命名沿用 COM 的 PascalCase(StartApplication)
ansys.stk.core 新代(PySTK)。命名改成 Python 慣用的 snake_case
(start_application)
兩代都走 gRPC、都連同一個伺服器、埠號預設值也一樣,**但方法名一個都不通用**。
於是最常見的卡關長這樣:照著某篇文章抄一段程式,裡面混了兩代的名字,
跑起來是 `AttributeError`,而錯誤訊息完全沒有提到「你混用了兩代 API」。
更棘手的是**看起來對但其實不存在**的名字。生成式工具產出的 STK 程式碼
尤其容易這樣 —— 它會造出符合命名習慣、語意也講得通、但庫裡沒有的成員。
這種程式碼讀起來比真的還順,只有跑下去才會炸。
## 所以本檔用內省,不用記憶
底下每一條對照都是**去裝好的套件裡查**,不是抄文件。含意:
- 它不會過期。Ansys 哪天改名,這支就變紅,而不是靜靜地錯下去。
- 它在**沒有 STK 授權、沒有伺服器**的機器上就能跑完 —— 純內省,不連線。
## 一份實際校正過的範例
本檔附了一份流傳中的「PySTK over gRPC 外部控制」範例的逐項校正
(`MYTHS`)。**七項裡有六項在裝好的套件裡不存在。** 逐項如下:
| 範例寫的 | 實際 |
|---|---|
| 埠號 `50051` | **`40704`**。50051 是 gRPC 教學的慣用埠,不是 STK 的 |
| `attach_to_application(host=, port=)` | 關鍵字是 `grpc_host=` / `grpc_port=` |
| `stk.AgESTKObjectType.eAircraft` | `stkobjects.STKObjectType.AIRCRAFT` |
| `AgEPropagatingMode.ePropagatingModeExternal` | 沒有這個列舉 |
| `aircraft.propagator` | **飛機沒有 propagator,飛機有 `route`** |
| `set_propagating_mode(...)` | 飛機用 `set_route_type(PropagatorType.REAL_TIME)` |
| `add_position_llh(...)` | `add_position(...)`,而且要先取到 point builder |
唯一對的那一項是它最後的建議:**大量座標要成批送**。而它沒講的是
庫裡早就有現成的成批入口(`add_position_batch`),而且實測快 20-30 倍
(見 `grpc_link_realtime.py`)。
這張表不是為了挑錯。它是這一章的**動機**:上面七項裡,
沒有任何一項會被「通聯測試」的前五層攔下來 —— 網路全綠、
交握全綠、物件模型全綠,然後在第一行業務邏輯炸掉。
**這就是為什麼第 5 層(能力面)必須存在,而且為什麼驗證段不能只驗連線。**
python grpc_link_api.py
離開碼 0 = 對照表與裝好的套件一致
"""
import io
import json
import os
import sys
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
API_OUT = os.path.join(HERE, "grpc_link_api.json")
# STK gRPC 伺服器的預設埠。兩代 API 的 grpc_port 預設值都是它。
STK_DEFAULT_GRPC_PORT = 40704
# gRPC 慣用的教學埠 —— **不是** STK 的。流傳的範例常誤用這個。
GRPC_TUTORIAL_PORT = 50051
# (說明, 舊代路徑, 新代路徑)。路徑寫成 "模組:屬性.屬性"。
RENAMES = [
("啟動 Runtime 伺服器",
"agi.stk12.stkruntime:STKRuntime.StartApplication",
"ansys.stk.core.stkruntime:STKRuntime.start_application"),
("接上既有伺服器",
"agi.stk12.stkruntime:STKRuntime.AttachToApplication",
"ansys.stk.core.stkruntime:STKRuntime.attach_to_application"),
("取得物件模型根",
"agi.stk12.stkruntime:STKRuntimeApplication.NewObjectRoot",
"ansys.stk.core.stkruntime:STKRuntimeApplication.new_object_root"),
("關閉連線",
"agi.stk12.stkruntime:STKRuntimeApplication.ShutDown",
"ansys.stk.core.stkruntime:STKRuntimeApplication.shutdown"),
("取得批次器",
"agi.stk12.stkruntime:STKRuntimeApplication.NewGrpcCallBatcher",
"ansys.stk.core.stkruntime:STKRuntimeApplication.new_grpc_call_batcher"),
("設 gRPC 選項",
"agi.stk12.stkruntime:STKRuntimeApplication.SetGrpcOptions",
"ansys.stk.core.stkruntime:STKRuntimeApplication.set_grpc_options"),
("送 Connect 指令",
"agi.stk12.stkobjects:AgStkObjectRoot.ExecuteCommand",
"ansys.stk.core.stkobjects:STKObjectRoot.execute_command"),
("新場景",
"agi.stk12.stkobjects:AgStkObjectRoot.NewScenario",
"ansys.stk.core.stkobjects:STKObjectRoot.new_scenario"),
]
# 流傳範例裡的名字 -> 實際該用什麼。(說明, 不存在的路徑, 正確的路徑 或 None)
MYTHS = [
("物件型別列舉",
"ansys.stk.core:AgESTKObjectType",
"ansys.stk.core.stkobjects:STKObjectType"),
("傳播模式列舉",
"ansys.stk.core:AgEPropagatingMode",
None),
("飛機的傳播器",
"ansys.stk.core.stkobjects:Aircraft.propagator",
"ansys.stk.core.stkobjects:Aircraft.route"),
("切換成外部驅動",
"ansys.stk.core.stkobjects:Aircraft.set_propagating_mode",
"ansys.stk.core.stkobjects:Aircraft.set_route_type"),
("注入一點經緯高",
"ansys.stk.core.stkobjects:PropagatorRealtimeDeticPoints.add_position_llh",
"ansys.stk.core.stkobjects:PropagatorRealtimeDeticPoints.add_position"),
]
# 只有新代才有的能力。**這一條是選 API 的決定性理由**(見導論的安全一節)。
NEW_ONLY = [
("TLS 通道憑證",
"ansys.stk.core.stkruntime:STKRuntime.attach_to_application",
"grpc_channel_credentials"),
]
def resolve(path):
"""把 "模組:屬性.屬性" 解出來。回傳 (找到嗎, 物件 或 None)。
用 importlib 而不是 eval —— 路徑字串來自本檔的表格,但
「拿字串當程式碼執行」在教材裡是壞示範,而且 importlib 的
失敗訊息清楚得多。
"""
import importlib
mod_name, _, attr_path = path.partition(":")
try:
obj = importlib.import_module(mod_name)
except ImportError:
return False, None
for part in attr_path.split("."):
if not part:
continue
if not hasattr(obj, part):
return False, None
obj = getattr(obj, part)
return True, obj
def check_ports():
"""兩代 API 的 grpc_port 預設值都要是 40704,而且都不是 50051。"""
import inspect
rows = []
for label, path in (
("舊代 agi.stk12",
"agi.stk12.stkruntime:STKRuntime.AttachToApplication"),
("新代 PySTK",
"ansys.stk.core.stkruntime:STKRuntime.attach_to_application")):
found, fn = resolve(path)
if not found:
rows.append((label, None, "套件沒裝"))
continue
port = inspect.signature(fn).parameters["grpc_port"].default
rows.append((label, port, "" if port == STK_DEFAULT_GRPC_PORT
else "與 %d 不符" % STK_DEFAULT_GRPC_PORT))
return rows
def check_renames():
"""兩代的名字各自都要在。回傳 [(說明, 舊在嗎, 新在嗎), ...]。"""
return [(label, resolve(old)[0], resolve(new)[0])
for label, old, new in RENAMES]
def check_myths():
"""流傳的名字要**不在**,而校正後的名字要**在**。
注意這一項的判準是反的:`ok` 代表「錯的確實不存在、對的確實存在」。
如果哪天 Ansys 真的加了 `add_position_llh`,這一項會變紅 ——
那是對的,因為到時候這張校正表就該改了。
"""
rows = []
for label, bad, good in MYTHS:
bad_found = resolve(bad)[0]
good_found = resolve(good)[0] if good else None
ok = (not bad_found) and (good is None or good_found)
rows.append((label, bad, bad_found, good, good_found, ok))
return rows
def check_new_only():
"""新代才有的參數。回傳 [(說明, 新代有嗎, 舊代有嗎), ...]。"""
import inspect
rows = []
for label, path, param in NEW_ONLY:
found, fn = resolve(path)
has_new = bool(found) and param in inspect.signature(fn).parameters
old = path.replace("ansys.stk.core", "agi.stk12").replace(
"attach_to_application", "AttachToApplication")
ofound, ofn = resolve(old)
has_old = bool(ofound) and param in inspect.signature(ofn).parameters
rows.append((label, param, has_new, has_old))
return rows
def main():
print("=" * 78)
print(" STK Python API 兩代對照 —— 全部靠內省裝好的套件,不連 STK")
print("=" * 78)
bad = 0
print()
print(" 一、gRPC 預設埠(流傳的範例常誤寫成 %d)" % GRPC_TUTORIAL_PORT)
for label, port, note in check_ports():
if port is None:
print(" %-16s (略過:%s)" % (label, note))
continue
mark = "OK" if not note else "**不符**"
print(" %-16s grpc_port 預設 = %-6s %s %s"
% (label, port, mark, note))
if note:
bad += 1
print()
print(" 二、同一件事的兩代名字(兩邊都要在)")
for label, old_ok, new_ok in check_renames():
if not (old_ok and new_ok):
bad += 1
print(" %-16s 舊代 %s 新代 %s"
% (label, "有" if old_ok else "**缺**",
"有" if new_ok else "**缺**"))
print()
print(" 三、流傳範例的逐項校正(錯的要不在,對的要在)")
for label, badp, badf, goodp, goodf, ok in check_myths():
print(" %-14s %s" % (label, "OK" if ok else "**表要改了**"))
print(" 寫的 %-62s %s"
% (badp.split(":")[-1], "竟然存在!" if badf else "不存在(如預期)"))
if goodp:
print(" 實際 %-62s %s"
% (goodp.split(":")[-1], "存在" if goodf else "**也不存在**"))
else:
print(" 實際 (沒有對應的列舉 —— 概念本身就不是這樣做的)")
if not ok:
bad += 1
print()
print(" 四、只有新代才有的能力")
for label, param, has_new, has_old in check_new_only():
print(" %-14s %-28s 新代 %s 舊代 %s"
% (label, param, "有" if has_new else "**缺**",
"有" if has_old else "沒有"))
if not has_new or has_old:
# 舊代若哪天也加了,這張表同樣要改
bad += 1
print(" -> 舊代只有 insecure_channel;要讓 gRPC 跨出本機,用新代。")
io.open(API_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"default_port": STK_DEFAULT_GRPC_PORT,
"ports": [[a, b, c] for a, b, c in check_ports()],
"renames": [[a, b, c] for a, b, c in check_renames()],
"myths": [[a, b, c, d, e, f]
for a, b, c, d, e, f in check_myths()],
"new_only": [[a, b, c, d]
for a, b, c, d in check_new_only()]},
ensure_ascii=False, indent=1) + "\n")
print()
if bad:
print(" **%d 項與裝好的套件不符** —— 對照表需要更新。" % bad)
else:
print(" 對照表與裝好的套件一致。")
print(" 結果已存到 %s" % os.path.basename(API_OUT))
return 1 if bad else 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_ardupilot.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — ArduPilot SITL / MAVLink 併入同一條管線。
## 為什麼要有這一支
前一節讓 Simulink 驅動 STK。但飛控軟體在環(SITL)才是這類工具鏈真正的
用途:讓**真正的自動駕駛儀**飛,再讓 STK 去算它算不了的東西。
ArduPilot 的 SITL 說的是 **MAVLink**,所以這一支做的就是
「MAVLink 進、STK 出」。
而它與 Simulink 那一節共用**同一個橋接類別**(`grpc_link_simulink.Bridge`)。
這不是節省程式碼,是這一章的架構主張在自我驗證:
> 介面是資料,不是函式呼叫。所以換掉資料來源,下游完全不動。
`Bridge` 一行都不必改,就從吃 Simulink 的 32-byte 封包變成吃 MAVLink ——
如果當初把 STK 的呼叫混進 UDP 解析裡,這一節就得重寫一遍。
## 三方整合測試:同一條軌跡,四個系統,最後在 STK 裡對帳
```
同一份 plant(協調轉彎,已對過閉合式)
|
+-----------+-----------+
| |
MAVLink 編碼 32-byte 封包
(pymavlink) (Simulink 側)
| |
UDP :14550 UDP :41000
| |
+-----------> Bridge <--+
|
gRPC :40704
|
STK
```
兩條路餵同一條軌跡,最後比對。**而它們不該完全相等** ——
差多少是可以事先算出來的,這才是這個測試有意義的地方(見下一節)。
## MAVLink 的整數量化:一個可以事先算出來的誤差
`GLOBAL_POSITION_INT` 的欄位是整數:
lat, lon int32,單位 **1e-7 度**
alt int32,單位 **公釐**(平均海平面)
relative_alt int32,單位 公釐(相對起飛點)
vx, vy, vz int16,單位 cm/s
hdg uint16,單位 0.01 度
所以走 MAVLink 的軌跡**必然**被量化到 1e-7 度。換成地面距離:
緯度 1e-7 度 = 1e-7 x 111,320 m / 度 約 **1.11 cm**
經度 1e-7 度 x cos(25 度) 約 **1.01 cm**
也就是說,MAVLink 路徑與 Simulink 路徑的差**應該落在約 1.2 cm 以內,
而且不應該是零**。這個界是**預測**出來的,不是量完才回頭湊的 ——
所以驗證段用它當判準:
- 差得**比 1.2 cm 大** -> 真的有 bug(欄位錯位、單位搞錯、座標系不對)
- 差**剛好是 0** -> 也可疑:表示量化沒發生,可能根本沒走 MAVLink 編碼
**把「應該有的誤差」也一起驗**,是這個測試與「跑起來沒報錯」的差別。
## SITL 不在這台機器上,那還驗了什麼
撰寫環境沒有 ArduPilot SITL 的執行檔,但**裝了 pymavlink**。
所以 `--fake-sitl` 用 pymavlink 自己去**編碼並送出真正的 MAVLink 封包**:
- **驗到了**:MAVLink 的線上格式、欄位單位、解碼路徑、量化界、整條橋接。
這些正是會出錯的地方。
- **沒驗到**:ArduPilot 本身的行為(飛行模式、任務、失效保護),
以及真實 SITL 的訊息節奏與訊息種類。
換句話說,**協定這一側是真的驗過的,飛控那一側沒有。**
接真的 SITL 要改的只有一個參數(連線字串),因為解碼吃的是
同一套 pymavlink。
## 比合成來源更進一步:外部 MAVLink 模擬器
`grpc_link_mavsim.py` 接一個**外部的多機 MAVLink 模擬器**,
訊息面從這裡的一種(GLOBAL_POSITION_INT)變成七種
(加上 HEARTBEAT、ATTITUDE、GPS_RAW_INT、VFR_HUD、SYS_STATUS、HOME_POSITION),
而且是多機。它補到的與沒補到的都寫在那一支的檔頭 ——
**仍然不是真的自駕韌體**,飛控行為那一欄還是未驗。
本檔的 `--fake-sitl` 留著,因為它不需要任何外部東西就能驗解碼路徑;
要更接近真實就用 `grpc_link_mavsim.py`。
## 接真的 ArduPilot SITL
# ArduPilot 那一側(需自行取得 ArduPilot 原始碼)
sim_vehicle.py -v ArduPlane --out=udp:127.0.0.1:14550
# 這一側
python grpc_link_ardupilot.py --mavlink udpin:127.0.0.1:14550
用法:
python grpc_link_ardupilot.py --fake-sitl --no-stk # 端到端自測,不連 STK
python grpc_link_ardupilot.py --fake-sitl # 併入 STK
python grpc_link_ardupilot.py --compare # 三方整合對帳(不連 STK)
python grpc_link_ardupilot.py --mavlink udpin:127.0.0.1:14550
"""
import io
import json
import math
import os
import sys
import threading
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_realtime as R # noqa: E402
import grpc_link_simulink as S # noqa: E402
MAVLINK_IN = "udpin:127.0.0.1:14550"
MAVLINK_OUT = "udpout:127.0.0.1:14550"
AP_OUT = os.path.join(HERE, "grpc_link_ardupilot.json")
# GLOBAL_POSITION_INT 的刻度。**這些是 MAVLink 規格值,不是本章訂的。**
DEG_E7 = 1e-7 # lat / lon 的單位(度)
MM = 1e-3 # alt / relative_alt 的單位(公尺)
M_PER_DEG_LAT = 111320.0 # 緯度一度約當的地面距離(m),用於換算量化界
# 量化界(公尺):緯度方向 1e-7 度。經度方向再乘 cos(lat),所以緯度這一項是上界。
QUANT_M = DEG_E7 * M_PER_DEG_LAT
def quantization_bound_m(lat_deg=R.LAT0):
"""MAVLink 整數量化造成的最大位置誤差(公尺)。
**事先算得出來**,所以可以當驗證判準,而不是量完才回頭湊一個容差。
兩個方向各自的半量化誤差,取向量和的上界。
"""
dlat = DEG_E7 * M_PER_DEG_LAT
dlon = DEG_E7 * M_PER_DEG_LAT * math.cos(math.radians(lat_deg))
return math.hypot(dlat, dlon)
# ------------------------------------------------------- 合成 SITL(發送端)
def fake_sitl(n=None, hz=20.0, target=MAVLINK_OUT):
"""用 pymavlink 把 plant 編成**真正的 MAVLink** 封包送出去。
這不是「假裝送 MAVLink」—— 位元組是 pymavlink 產生的,
所以解碼那一側面對的是真的線上格式。沒驗到的只有 ArduPilot 的行為。
回傳送出的訊息數。
"""
from pymavlink import mavutil
t, lat, lon, alt_km = R.plant(n=n or R.N_STEPS)
conn = mavutil.mavlink_connection(target, source_system=1)
sent = 0
for i in range(len(t)):
alt_mm = int(round(alt_km[i] * 1000.0 / MM)) # km -> m -> mm
conn.mav.global_position_int_send(
int(t[i] * 1000.0), # time_boot_ms
int(round(lat[i] / DEG_E7)), # lat, degE7
int(round(lon[i] / DEG_E7)), # lon, degE7
alt_mm, # alt, mm (AMSL)
alt_mm, # relative_alt, mm
0, 0, 0, # vx, vy, vz cm/s
65535) # hdg 未知
sent += 1
time.sleep(1.0 / hz)
return sent
# ------------------------------------------------------- 資料來源(接收端)
def source_mavlink(device=MAVLINK_IN, expect=None, idle_timeout=3.0,
first_timeout=60.0):
"""從 MAVLink 收 GLOBAL_POSITION_INT,yield (t, lat, lon, alt_km)。
與 UDP 那條路一樣,**首包另計超時** —— 真的 SITL 從啟動到開始送
位置訊息要十幾秒,用短超時會在它還在初始化時就放棄。
只挑 GLOBAL_POSITION_INT:SITL 會噴幾十種訊息,
全收下來只會讓下游去猜哪個是位置。**在來源就篩掉**,下游才單純。
"""
from pymavlink import mavutil
conn = mavutil.mavlink_connection(device)
got = 0
try:
while expect is None or got < expect:
to = first_timeout if got == 0 else idle_timeout
msg = conn.recv_match(type="GLOBAL_POSITION_INT",
blocking=True, timeout=to)
if msg is None:
break
got += 1
yield (msg.time_boot_ms / 1000.0,
msg.lat * DEG_E7,
msg.lon * DEG_E7,
msg.alt * MM / 1000.0) # mm -> m -> km
finally:
try:
conn.close()
except Exception: # noqa: BLE001
pass
# ------------------------------------------------------- 三方對帳
def compare_paths(n=None):
"""同一條 plant 走 MAVLink 與走 Simulink 封包,比對結果。
回傳 (最大位置差公尺, 預測的量化界, MAVLink 收到幾點)。
這一項是「整合測試」真正的內容:不是「兩條路都沒報錯」,
而是**兩條路的差落在事先算得出來的界內,而且不為零**。
"""
n = n or 60
recv = []
th = threading.Thread(target=fake_sitl, kwargs={"n": n, "hz": 200.0},
daemon=True)
th.start()
for frame in source_mavlink(expect=n, first_timeout=20.0, idle_timeout=2.0):
recv.append(frame)
t, lat, lon, _alt = R.plant(n=n)
worst = 0.0
for i, (_tm, la, lo, _al) in enumerate(recv):
if i >= len(lat):
break
dlat = (la - lat[i]) * M_PER_DEG_LAT
dlon = (lo - lon[i]) * M_PER_DEG_LAT * math.cos(math.radians(lat[i]))
worst = max(worst, math.hypot(dlat, dlon))
return worst, quantization_bound_m(), len(recv)
def main():
use_stk = "--no-stk" not in sys.argv
chunk = 10
if "--chunk" in sys.argv:
chunk = int(sys.argv[sys.argv.index("--chunk") + 1])
print("=" * 78)
print(" ArduPilot / MAVLink -> STK(與 Simulink 共用同一個橋接)")
print("=" * 78)
if "--compare" in sys.argv:
worst, bound, ngot = compare_paths()
print()
print(" 三方對帳:同一份 plant,一條走 MAVLink 編解碼,一條直接用")
print(" MAVLink 收到 %d 點" % ngot)
print(" 最大位置差 %.4f m" % worst)
print(" 預測的量化界 %.4f m(lat/lon 各 1e-7 度)" % bound)
ok = 0.0 < worst <= bound * 1.05
print(" 判定:%s" % ("通過" if ok else "**不通過**"))
print(" 差要落在界內(不然是 bug),而且**不能是 0**")
print(" —— 是 0 就表示量化沒發生,可能根本沒走 MAVLink 編碼。")
io.open(AP_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"worst_m": worst, "bound_m": bound, "points": ngot,
"pass": ok}, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(AP_OUT))
return 0 if ok else 1
fake = "--fake-sitl" in sys.argv
device = MAVLINK_IN
if "--mavlink" in sys.argv:
device = sys.argv[sys.argv.index("--mavlink") + 1]
app = pts = route = ac = root = None
if use_stk:
try:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
root = app.new_object_root()
root.new_scenario("ArduPilotBridge")
t_all, _, _, _ = R.plant()
ts = R.timestamps(t_all)
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"'
% (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
ac, route, pts = R.make_realtime_aircraft(root, "ArduPilotUAV")
print(" STK 已就緒:%s(Realtime route)" % ac.instance_name)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:100])
print(" 改用 --no-stk 只驗解碼與橋接,或先跑 grpc_link_probe.py。")
return 1
bridge = S.Bridge(pts, chunk)
n_expect = None
if fake:
n_expect = R.N_STEPS
threading.Thread(target=fake_sitl, daemon=True).start()
print(" 來源:合成 SITL(pymavlink 編出真正的 MAVLink,送 %s)" % MAVLINK_OUT)
else:
print(" 來源:%s(等 ArduPilot SITL)" % device)
print(" 首包最多等 60 秒 —— 真的 SITL 初始化要十幾秒")
t0 = time.time()
for frame in source_mavlink(device, expect=n_expect):
bridge.feed(frame)
bridge.flush()
rep = bridge.report()
rep["wall_s"] = time.time() - t0
rep["quant_bound_m"] = quantization_bound_m()
print()
print(" 收到 %d 點,漏 %d 包" % (rep["points"], rep["gaps"]))
print(" gRPC 往返 %d 次(攢批把 %d 點壓成 %d 次)"
% (rep["grpc_calls"], rep["points"], rep["grpc_calls"]))
print(" MAVLink 量化界 %.4f m(整數欄位造成,無法消除)"
% rep["quant_bound_m"])
if use_stk:
print(" 注入耗時 %.3f s,%.3f ms/點" % (rep["inject_s"],
rep["ms_per_point"]))
route.propagate()
rb_lat, rb_lon = R.readback(ac, root.current_scenario)
r_stk = R.fitted_radius(rb_lat, rb_lon)
r_ref = R.turn_radius()
rep["r_stk_m"], rep["r_closed_m"] = r_stk, r_ref
print()
print(" 端到端驗證:STK 裡讀回 %d 列,擬圓半徑 %.1f m" % (len(rb_lat), r_stk))
print(" 閉合式 %.1f m,差 %.3f%%"
% (r_ref, abs(r_stk - r_ref) / r_ref * 100))
print(" -> plant -> MAVLink -> 橋接 -> gRPC -> STK 全程走完,")
print(" 再與**外部閉合式**對照。")
io.open(AP_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(rep, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(AP_OUT))
if app is not None:
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_bench.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 延遲、批次與訊息上限量測。
## 為什麼通聯測試要量效能
因為 gRPC 這條路**會通,但會慢**,而慢法跟本機 COM 完全不同:
每一次屬性存取都是一次網路往返。一個在本機跑三秒的腳本,
搬到 gRPC 上可能跑三分鐘 —— 程式一個字都沒改,而且**不會有任何錯誤訊息**。
所以「通了」之後真正要問的是:一次往返多久?哪些寫法會把往返數乘上去?
本檔量四件事,每一件都對應一個實際會踩到的坑。
### 一、單次往返延遲
拿一個**不快取**的屬性讀取當標尺。回文迴路上是幾十微秒到幾毫秒;
真正的意義不是這個數字本身,而是把它乘上腳本的呼叫次數 ——
一支做一萬次設定的腳本,在 0.2 ms/次 之下就是 2 秒,
在 2 ms/次 之下就是 20 秒。
### 二、批次(GrpcCallBatcher)
STK 提供 `NewGrpcCallBatcher()`:把**沒有回傳值**的呼叫(設屬性、
無回傳的方法)攢起來一次送。單趟最多 10000 筆。
代價寫在 API 自己的文件裡,而且很重要:**批次會改變例外行為** ——
錯誤變成非同步冒出來,不一定落在出錯的那一行。所以批次是
**最後才加的效能手段**,不是開發時的預設。
### 三、鏈式呼叫會悄悄毀掉批次
這是本檔最想量的東西,因為它完全看不出來。批次只能攢
「沒有回傳值」的呼叫,而**任何取值都會先把已攢的送出去**。
於是:
root.CurrentScenario.ShortDescription = a # CurrentScenario 是一次取值
root.CurrentScenario.LongDescription = b # 又一次
每一行都被中間那個 `.CurrentScenario` 打斷,批次等於沒開。把它提出來:
scen = root.CurrentScenario # 取值一次
scen.ShortDescription = a # 這兩行攢得起來
scen.LongDescription = b
兩段程式在本機跑起來沒有差別,在 gRPC 上差好幾倍 ——
而**沒有任何工具會警告你**。本檔把這個差距量出來。
### 四、訊息大小上限
gRPC 對單一訊息有接收上限,預設 **4 MiB**(這是 gRPC 自己的預設值,
不是 STK 訂的)。DataProvider 一次取回太多列就會撞上去。
實測(一週分析期間、Classical Elements、逐步縮小步長):
步長 10 s 60481 列 約 0.5 MB 過
步長 2 s 302401 列 約 2.4 MB 過
步長 1 s 604801 列 約 4.8 MB **撞上限**
處置有兩條,**代價不一樣**:
- **分段取**(改步長或切時間區間):資料量不變,訊息變小。推薦。
- **把上限開大**(`grpc_max_message_size`):訊息過得去了,但整個
酬載要同時擺在記憶體裡。實測把上限開到 64 MB 去取 48 MB 的酬載,
伺服器端的工作集漲到約 2.5 GB,而且要等近兩分鐘。
**它把一個乾淨的錯誤換成了一次記憶體壓力。**
python grpc_link_bench.py # 自己啟一個 Runtime 來量
python grpc_link_bench.py --attach # 量已經在跑的伺服器
python grpc_link_bench.py --quick # 少跑幾輪
"""
import io
import json
import os
import sys
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
BENCH_PORT = 40704
BENCH_OUT = os.path.join(HERE, "grpc_link_bench.json")
# gRPC 的預設單一訊息接收上限(位元組)。這是 gRPC 的文件值,
# 不是本教材量出來的 —— 量出來的是「撞到它的那個列數」。
GRPC_DEFAULT_MAX_MESSAGE = 4 * 1024 * 1024
EPOCH = "1 Jan 2026 00:00:00.000"
def scenario(root, days=7):
"""一顆圓軌道衛星、一個地面站,供後面四項量測共用。"""
root.NewScenario("GrpcBench")
for cmd in (
"SetUnits / km",
'SetAnalysisTimePeriod * "%s" "+%d days"' % (EPOCH, days),
"New / */Satellite BenchSat",
"SetState */Satellite/BenchSat Classical TwoBody "
'UseScenarioInterval 60 ICRF "%s" 7000 0 28.5 0 0 0' % EPOCH,
"New / */Facility BenchFac",
"SetPosition */Facility/BenchFac Geodetic 24.8 121.0 0.0"):
root.ExecuteCommand(cmd)
return root.CurrentScenario
# ------------------------------------------------------------ 一、往返延遲
def bench_roundtrip(scen, n=300):
"""單次 get-property 的往返時間(ms)。
量的是**不快取**的屬性:`InstanceName` 每次都真的走一趟。
量之前先丟掉前幾次 —— 第一次呼叫含通道暖機,把它算進平均會偏高。
"""
for _ in range(min(10, n)):
_ = scen.InstanceName
t0 = time.time()
for _ in range(n):
_ = scen.InstanceName
return (time.time() - t0) / n * 1000.0
# ------------------------------------------------------------ 二、批次
def bench_batching(app, scen, n=400):
"""同樣 n 次設屬性:不批次 vs 批次。回傳 (不批次秒, 批次秒)。
選 `Epoch` 這種純量屬性,而不是會觸發重新傳播的東西 ——
不然量到的是 STK 的計算時間,不是通聯的往返時間。
"""
fac = scen.Children.Item("BenchFac")
t0 = time.time()
for i in range(n):
fac.HeightAboveGround = 0.001 * (i % 11)
plain = time.time() - t0
batcher = app.NewGrpcCallBatcher()
t0 = time.time()
with batcher:
for i in range(n):
fac.HeightAboveGround = 0.001 * (i % 11)
batched = time.time() - t0
return plain, batched
# ------------------------------------------------------------ 三、鏈式打斷
def bench_chaining(app, root, n=300):
"""批次開著,鏈式寫法 vs 提出中間物件。回傳 (鏈式秒, 提出秒)。
**兩段的語意完全相同**,差別只有 `root.CurrentScenario` 寫在
迴圈裡還是迴圈外。鏈式那一段裡的取值會把批次沖掉,所以它
等於沒開批次 —— 這就是要量出來的東西。
"""
batcher = app.NewGrpcCallBatcher()
t0 = time.time()
with batcher:
for i in range(n):
# 每一圈的 .CurrentScenario 都是一次取值 -> 沖掉批次
root.CurrentScenario.ShortDescription = "chained %d" % i
chained = time.time() - t0
batcher2 = app.NewGrpcCallBatcher()
t0 = time.time()
with batcher2:
scen = root.CurrentScenario # 取值一次,移到迴圈外
for i in range(n):
scen.ShortDescription = "hoisted %d" % i
hoisted = time.time() - t0
return chained, hoisted
# ------------------------------------------------------------ 四、訊息上限
def bench_payload(scen, sat, steps=(10.0, 2.0, 1.0)):
"""逐步縮小 DataProvider 步長,直到撞上訊息上限。
回傳 [(步長, 列數, 位元組估計, 秒數 或 None, 錯誤訊息), ...]。
撞到就停 —— 再往下只會更大,不會有新資訊。
"""
dp = sat.DataProviders.Item("Classical Elements").Group.Item("ICRF")
rows = []
for step in steps:
t0 = time.time()
try:
res = dp.Exec(scen.StartTime, scen.StopTime, step)
vals = res.DataSets.GetDataSetByName("Semi-major Axis").GetValues()
dt = time.time() - t0
rows.append((step, len(vals), len(vals) * 8, dt, ""))
except Exception as e: # noqa: BLE001
rows.append((step, None, None, None, str(e)[:120]))
break
return rows
# ------------------------------------------------------------ 主流程
def connect(attach=False, port=BENCH_PORT, max_message=0):
"""拿到一個 STKRuntime application。attach=True 就不自己啟。"""
from agi.stk12.stkruntime import STKRuntime
if attach:
return STKRuntime.AttachToApplication(grpc_host="localhost",
grpc_port=port,
grpc_max_message_size=max_message)
return STKRuntime.StartApplication(grpc_host="0.0.0.0", grpc_port=port,
grpc_timeout_sec=90, noGraphics=True,
grpc_max_message_size=max_message)
def main():
attach = "--attach" in sys.argv
quick = "--quick" in sys.argv
n_rt, n_b = (60, 100) if quick else (300, 400)
steps = (10.0, 2.0) if quick else (10.0, 2.0, 1.0)
print("=" * 76)
print(" STK gRPC 通聯測試 — 延遲與批次量測")
print("=" * 76)
try:
app = connect(attach)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:100])
print(" 先跑 grpc_link_probe.py 找出斷在哪一層。")
return 1
if app is None:
print(" 接不上 STK(回 None)。先跑 grpc_link_probe.py。")
return 1
root = app.NewObjectRoot()
scen = scenario(root)
sat = root.GetObjectFromPath("*/Satellite/BenchSat")
out = {}
rt = bench_roundtrip(scen, n_rt)
out["roundtrip_ms"] = rt
print()
print(" 一、單次往返(get-property x %d)" % n_rt)
print(" %.3f ms/次 -> 一萬次呼叫要 %.1f 秒" % (rt, rt * 10000 / 1000))
plain, batched = bench_batching(app, scen, n_b)
out["batch"] = {"n": n_b, "plain_s": plain, "batched_s": batched}
print()
print(" 二、批次(設屬性 x %d)" % n_b)
print(" 不批次 %.3f s 批次 %.3f s 快 %.1f 倍"
% (plain, batched, plain / batched if batched else float("nan")))
chained, hoisted = bench_chaining(app, root, n_rt)
out["chaining"] = {"n": n_rt, "chained_s": chained, "hoisted_s": hoisted}
print()
print(" 三、鏈式呼叫打斷批次(兩段語意相同,x %d)" % n_rt)
print(" 鏈式 %.3f s 提出中間物件 %.3f s 差 %.1f 倍"
% (chained, hoisted, chained / hoisted if hoisted else float("nan")))
print()
print(" 四、訊息大小上限(gRPC 預設 %d MiB)"
% (GRPC_DEFAULT_MAX_MESSAGE // (1024 * 1024)))
pay = bench_payload(scen, sat, steps)
out["payload"] = [{"step_s": a, "rows": b, "bytes": c, "secs": d, "err": e}
for a, b, c, d, e in pay]
for step, rows, nbytes, secs, err in pay:
if rows is None:
print(" 步長 %5.1f s **撞上限** %s" % (step, err))
else:
print(" 步長 %5.1f s %8d 列 約 %5.2f MB %6.2f s"
% (step, rows, nbytes / 1e6, secs))
io.open(BENCH_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(out, ensure_ascii=False, indent=1) + "\n")
print()
print(" 量測值已存到 %s" % os.path.basename(BENCH_OUT))
try:
app.ShutDown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_closedloop.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 雙向(閉迴路)實驗。
## 為什麼單向不夠
`grpc_link_simulink.py` 是**單向**的:Simulink 算、STK 收。STK 在那條鏈裡
只是一個顯示與分析的終點,它算出來的東西影響不了飛行。
而 SWIL 真正的價值在**閉迴路** —— STK 算出來的環境判斷要能改變飛行決策:
鏈路斷了 -> 自動返航
地形擋住 -> 拉高
感測器看不到目標 -> 改變航向
所以這一支把迴路接起來:
```
Simulink --UDP 41000--> 橋接 --gRPC--> STK(注入位置)
^ | |
| | 算鏈路可用性
+-----UDP 41001----------+<-----------------+
回饋
```
## 閉迴路的代價,而且它是可以量的
單向串流每點約 0.65 ms(實測)。閉迴路多了三段:回傳的 UDP、
STK 的計算、以及**等待** —— 而等待是最貴的,因為單向可以攢批,
閉迴路不行:**你必須等這一步的答案才能算下一步。**
攢批是單向串流能便宜的唯一原因。閉迴路把它拿掉了,所以
「10 Hz 塞得下 60 架」那個結論在閉迴路下完全不成立。
本檔把閉迴路的每步成本量出來,再換算成它撐得住的迴路頻率。
## 回饋量選什麼
選**地面站的鏈路可用性**,因為它有三個好處:
1. 它是 STK 真正擅長、而 Simulink 算不出來的東西(地球曲率、地形、天線)。
2. 它的變化是**離散事件**(通/斷),所以閉迴路的效果在軌跡上看得見。
3. 它對應一個真實需求:斷鏈自動返航。
實作上分兩種模式。它們的差別**不只是快慢,是回饋到底由誰算**:
- `--mode access`:每一步呼叫 STK 的 `compute_access()`。
**回饋真的來自 STK** —— 含存取約束、地球曲率,若載入地形還含遮蔽。
代價是每一步都要對**整段星曆**重算,成本隨已注入的點數成長。
- `--mode local`:橋接自己用球面幾何算仰角。**回饋不再來自 STK**,
STK 只負責存星曆。成本固定且極低。
這正是每個 SWIL 系統都要做的取捨:**把環境模型放進迴路(貴、忠實),
還是在本地近似(便宜、失真)**。本檔不只量兩者的成本,還量它們
**判斷不一致的次數** —— 因為「便宜的那個夠不夠用」只能這樣回答,
不能用講的。
一個刻意不做的事:本檔**不**假裝「用 STK 算幾何仰角」。
STK 沒有便宜的單點仰角入口(`CentralBody` 沒有大地座標轉換,
`create_one_point_access` 吃的是物件路徑),硬湊一個出來只會讓
「回饋來自 STK」這句話變成半真半假。要嘛真的走 access,要嘛承認是本地算的。
## 控制律:斷鏈就反向轉彎
刻意選最簡單、在軌跡上一眼看得出來的:
鏈路可用 -> 維持坡度角 +phi
鏈路中斷 -> 坡度角翻成 -phi(朝回地面站的方向轉)
它不是好的返航律(真的返航要算航向誤差),但它讓「STK 的判斷真的
改變了飛行」這件事在圖上無法造假 —— 軌跡會在斷鏈點折返。
## 沒有 MATLAB 也要能跑
`--fake` 用 Python 實作 Simulink 那一側的閉迴路(同一組協調轉彎方程),
所以整條迴路在沒有 MATLAB 的機器上就能驗。
**驗到的是迴路本身與 STK 這一側;沒驗到的是 Simulink 的求解器行為。**
python grpc_link_closedloop.py --fake # 自跑閉迴路(需 STK)
python grpc_link_closedloop.py --fake --mode local # 便宜的本地近似
python grpc_link_closedloop.py --compare # 量兩種模式的不一致
python grpc_link_closedloop.py --fake --no-stk # 只驗迴路,不連 STK
python grpc_link_closedloop.py --udp # 等 Simulink 接進來
"""
import io
import json
import math
import os
import socket
import struct
import sys
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_realtime as R # noqa: E402
import grpc_link_simulink as S # noqa: E402
FB_PORT = 41001 # 回饋的 UDP 埠(狀態走 41000)
FB_FRAME = "<3d" # t, link_ok(0/1), elevation_deg
FB_SIZE = struct.calcsize(FB_FRAME)
CL_OUT = os.path.join(HERE, "grpc_link_closedloop.json")
# 地面站:放在起點西南方,讓飛機轉到遠側時仰角會掉下去
GS_LAT, GS_LON, GS_ALT_KM = 24.76, 120.96, 0.0
# 地面站的遮蔽角(mask angle)。**10 度不是為了讓實驗好看而挑的**:
# 那是地面站常用的最小工作仰角 —— 再低就會吃到地形、多路徑與大氣折射。
# 實測這條航路的仰角落在 6.97 - 27.35 度(中位 11.38),所以 10 度會產生
# 真實的通斷;若改成 3 度則全程可用,閉迴路等於沒作用。
MIN_ELEV_DEG = 10.0
BANK_DEG = R.BANK_DEG # 正常轉彎坡度角
N_STEPS = 400
DT = 1.0
assert FB_SIZE == 24, "回饋封包必須是 24 bytes"
# ------------------------------------------------------------ 幾何(不含 STK)
def elevation_deg(lat, lon, alt_km, gs_lat=GS_LAT, gs_lon=GS_LON,
gs_alt_km=GS_ALT_KM):
"""地面站看載具的仰角(度)。球面地球,含地球曲率。
這是**本機算的**,只在 `--no-stk` 或拿來對照時用。
真正的回饋要由 STK 給 —— 不然這一章就變成「自己算完再問自己」。
"""
re_km = R.RE_M / 1000.0
p1 = _ecef(gs_lat, gs_lon, gs_alt_km, re_km)
p2 = _ecef(lat, lon, alt_km, re_km)
d = [p2[i] - p1[i] for i in range(3)]
up = _ecef(gs_lat, gs_lon, gs_alt_km + 1.0, re_km)
u = [up[i] - p1[i] for i in range(3)]
nu = math.sqrt(sum(x * x for x in u))
nd = math.sqrt(sum(x * x for x in d))
if nd == 0.0:
return 90.0
cosz = sum(d[i] * u[i] for i in range(3)) / (nu * nd)
return 90.0 - math.degrees(math.acos(max(-1.0, min(1.0, cosz))))
def _ecef(lat, lon, alt_km, re_km):
la, lo = math.radians(lat), math.radians(lon)
r = re_km + alt_km
return (r * math.cos(la) * math.cos(lo),
r * math.cos(la) * math.sin(lo),
r * math.sin(la))
# ------------------------------------------------------------ Simulink 側(合成)
class Plant:
"""協調轉彎,但**坡度角由回饋決定** —— 這就是閉迴路的那一半。
方程與 `grpc_link_realtime.plant()` 相同(顯式歐拉、平板地球),
差別只有 bank 不再是常數。開迴路(永遠 link_ok)時兩者應逐點相同,
而驗證段正是這樣驗的。
"""
def __init__(self, lat0=R.LAT0, lon0=R.LON0, alt0_m=R.ALT0, v=R.V_TAS,
gamma_deg=R.GAMMA_DEG, dt=DT):
self.lat, self.lon, self.h = lat0, lon0, alt0_m
self.psi, self.v, self.dt = 0.0, v, dt
self.cg = math.cos(math.radians(gamma_deg))
self.sg = math.sin(math.radians(gamma_deg))
self.t = 0.0
def step(self, bank_deg):
"""走一步。回傳 (t, lat, lon, alt_km) —— 與 UDP 封包同格式。"""
out = (self.t, self.lat, self.lon, self.h / 1000.0)
dn = self.v * self.cg * math.cos(self.psi) * self.dt
de = self.v * self.cg * math.sin(self.psi) * self.dt
self.lat += math.degrees(dn / R.RE_M)
self.lon += math.degrees(
de / (R.RE_M * math.cos(math.radians(self.lat))))
self.h += self.v * self.sg * self.dt
self.psi += (R.G0 / self.v) * math.tan(math.radians(bank_deg)) * self.dt
self.t += self.dt
return out
def guidance(link_ok, bank_deg=BANK_DEG):
"""控制律:斷鏈就把坡度角翻過來。
**刻意簡單。** 真正的返航要算航向誤差、要有遲滯、要防止在門檻上顫振。
這裡要的是「STK 的判斷確實改變了飛行」這件事在軌跡上無法造假。
"""
return bank_deg if link_ok else -bank_deg
# ------------------------------------------------------------ STK 側
class StkLoop:
"""注入一點,然後問 STK 一個問題。回傳 (link_ok, elev_deg, 耗時秒)。"""
def __init__(self, mode="local"):
self.mode = mode
self.app = self.root = self.ac = self.route = self.pts = None
self.fac = None
self.n = 0
def start(self):
from ansys.stk.core.stkruntime import STKRuntime
from ansys.stk.core.stkobjects import STKObjectType
self.app = STKRuntime.start_application(
grpc_host="0.0.0.0", grpc_port=R.RT_PORT,
grpc_timeout_sec=90, no_graphics=True)
self.root = self.app.new_object_root()
self.root.new_scenario("ClosedLoop")
ts = R.timestamps([0.0, N_STEPS * DT])
self.root.execute_command('SetAnalysisTimePeriod * "%s" "%s"'
% (ts[0], ts[-1]))
self.root.execute_command("SetUnits / km")
# 時間切成場景起點起算的秒數。access 回傳的區間因此是數字,
# 可以直接比大小 —— 用預設的日期字串比較跨分鐘就會錯,而且很安靜。
self.root.units_preferences.set_current_unit("DateFormat", "EpSec")
self.ac, self.route, self.pts = R.make_realtime_aircraft(
self.root, "ClosedLoopUAV")
self.fac = self.root.current_scenario.children.new(
STKObjectType.FACILITY, "LoopGS")
self.fac.position.assign_geodetic(GS_LAT, GS_LON, GS_ALT_KM)
# **把遮蔽角設成 STK 的存取約束。** 少了這一步,兩種模式量的
# 根本不是同一件事:STK 的 access 預設是**純視線**,不含仰角限制,
# 所以在這段距離上幾乎全程可通,翻轉永遠是 0;而本地模式用的是
# 仰角 >= 10 度。少了這條約束**不會報錯** ——
# 只會讓閉迴路看起來沒作用。
from ansys.stk.core.stkobjects import AccessConstraintType
c = self.fac.access_constraints.add_constraint(
AccessConstraintType.ELEVATION_ANGLE)
c.enable_minimum = True
c.minimum = MIN_ELEV_DEG
return self
def tick(self, frame):
"""注入一點並取得回饋。回傳 (link_ok, elev_deg, 這一步花的秒數)。
`elev` 在 access 模式下是本地算的**參考值**(不參與決策),
留著是為了和 STK 的判斷對帳 —— 見 `--compare`。
"""
t0 = time.time()
t, lat, lon, alt = frame
# **時間用秒,不用日期字串。** 場景的 DateFormat 已切成 EpSec,
# 而 `add_position` 的時間參數走的就是那個單位 —— 繼續丟絕對日期
# 字串會拿到 `Invalid date argument`。
# 單位偏好改的不只是輸出格式,**也改了 API 接受什麼**。
self.pts.add_position(t, lat, lon, alt)
self.n += 1
elev = elevation_deg(lat, lon, alt)
if self.mode == "access":
# **回饋真的來自 STK。** 代價:每一步都要重新傳播整段星曆再
# 重算 access,所以成本隨已注入的點數成長 —— 這一行就是
# 本檔要量的東西,而不是要迴避的東西。
link_ok = self._stk_link_ok(t)
else:
# 便宜的那條:本地球面幾何。**STK 只負責存星曆,不參與決策。**
link_ok = elev >= MIN_ELEV_DEG
return link_ok, elev, time.time() - t0
def _stk_link_ok(self, t_sec):
"""問 STK:**在 t_sec 這一刻**這條鏈路通不通。
兩個坑,都是實際踩到的:
1. `route.propagate()` 一定要在 `compute_access()` 之前。
沒有重新傳播的話,access 看到的是舊星曆,而且**它不會報錯**,
只會給一組對不上的可見窗。
2. **`compute_access()` 回傳的是整段星曆的所有可見窗,
不是「此刻通不通」。** 寫成 `len(rows) > 0` 的話,只要
出現過一次可見窗就永遠是 True —— 閉迴路因此完全不作用,
而它**不會報錯**,只會讓翻轉次數停在 0。
(本檔的「翻轉 0 次」警告就是為了抓這種事。)
時間格式刻意先切成 `EpSec`(場景起點起算的秒),這樣回傳的
區間就是數字,可以直接比大小 —— 用預設的日期字串去比,
跨分鐘、跨小時都會出錯,而且錯得很安靜。
"""
self.route.propagate()
acc = self.ac.get_access_to_object(self.fac)
acc.compute_access()
try:
rows = acc.computed_access_interval_times.to_array(0, -1)
except Exception: # noqa: BLE001
return False
for r in rows:
try:
a, b = float(r[0]), float(r[1])
except (TypeError, ValueError):
continue
if a - 1e-6 <= t_sec <= b + 1e-6:
return True
return False
def finish(self):
try:
self.route.propagate()
except Exception: # noqa: BLE001
pass
def shutdown(self):
if self.app is not None:
try:
self.app.shutdown()
except Exception: # noqa: BLE001
pass
# ------------------------------------------------------------ 迴路
def run_loop(stk, n=N_STEPS, send_udp=False, recv_udp=False,
fb_port=FB_PORT, state_port=S.UDP_PORT):
"""跑閉迴路。回傳 (軌跡, 每步耗時清單, 翻轉次數)。
`send_udp`/`recv_udp` 打開時,狀態與回饋走真的 UDP(給 Simulink 用);
否則 plant 直接在本行程裡跑(`--fake`)。
"""
sock_out = sock_in = None
if send_udp or recv_udp:
sock_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
if recv_udp:
sock_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock_in.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock_in.bind(("0.0.0.0", state_port))
sock_in.settimeout(60.0)
plant = Plant(dt=DT)
track, costs, elevs = [], [], []
link_ok, flips, prev = True, 0, True
disagree = 0
for i in range(n):
if recv_udp:
try:
data, _a = sock_in.recvfrom(4096)
except socket.timeout:
break
if len(data) != S.FRAME_SIZE:
continue
frame = struct.unpack(S.FRAME, data)
sock_in.settimeout(5.0)
else:
frame = plant.step(guidance(link_ok))
track.append(frame)
if stk is None:
t0 = time.time()
elev = elevation_deg(frame[1], frame[2], frame[3])
link_ok = elev >= MIN_ELEV_DEG
costs.append(time.time() - t0)
else:
link_ok, elev, dt_s = stk.tick(frame)
costs.append(dt_s)
# 本地近似在同一點上的判斷。access 模式下它**不參與決策**,
# 純粹拿來和 STK 的判斷對帳 —— 「便宜的那個夠不夠用」
# 只能這樣回答,不能用講的。
local_ok = elev >= MIN_ELEV_DEG
if local_ok != link_ok:
disagree += 1
elevs.append(elev)
if link_ok != prev:
flips += 1
prev = link_ok
if send_udp:
sock_out.sendto(struct.pack(FB_FRAME, frame[0],
1.0 if link_ok else 0.0,
0.0 if elev != elev else elev),
("127.0.0.1", fb_port))
for s in (sock_out, sock_in):
if s is not None:
s.close()
return track, costs, flips, disagree, elevs
def summarise(costs):
"""回傳 (平均 ms, 中位數 ms, 最大 ms, 撐得住的迴路頻率 Hz)。"""
if not costs:
return 0.0, 0.0, 0.0, 0.0
ms = sorted(c * 1000.0 for c in costs)
avg = sum(ms) / len(ms)
med = ms[len(ms) // 2]
return avg, med, ms[-1], (1000.0 / ms[-1] if ms[-1] > 0 else float("inf"))
def main():
use_stk = "--no-stk" not in sys.argv
mode = "local"
if "--mode" in sys.argv:
mode = sys.argv[sys.argv.index("--mode") + 1]
n = N_STEPS
if "--steps" in sys.argv:
n = int(sys.argv[sys.argv.index("--steps") + 1])
from_udp = "--udp" in sys.argv
print("=" * 78)
print(" 雙向(閉迴路)實驗 — STK 的判斷回頭改變飛行")
print("=" * 78)
print(" 回饋量:%s 地面站 %.3f N %.3f E 斷鏈門檻仰角 %.1f deg"
% (mode, GS_LAT, GS_LON, MIN_ELEV_DEG))
print(" 控制律:鏈路可用 -> 坡度 +%.0f deg;中斷 -> %.0f deg"
% (BANK_DEG, -BANK_DEG))
stk = None
if use_stk:
try:
stk = StkLoop(mode).start()
print(" STK 已就緒:%s" % stk.ac.instance_name)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:100])
print(" 改用 --no-stk 只驗迴路(回饋改用本機球面幾何)。")
return 1
else:
print(" **--no-stk:回饋改用本機球面幾何,不是 STK 算的。**")
t0 = time.time()
track, costs, flips, disagree, elevs = run_loop(
stk, n, send_udp=from_udp, recv_udp=from_udp)
wall = time.time() - t0
if stk is not None:
stk.finish()
avg, med, worst, hz = summarise(costs)
print()
print(" 跑了 %d 步,牆鐘 %.2f s" % (len(track), wall))
print(" 每步回饋成本 平均 %.3f ms 中位 %.3f ms 最差 %.3f ms"
% (avg, med, worst))
print(" -> 以最差值算,閉迴路撐得住約 %.1f Hz" % hz)
print(" 鏈路狀態翻轉 %d 次" % flips)
if stk is not None and mode == "access":
print()
print(" 忠實度對帳:同一條軌跡上,**STK 的 access 與本地仰角近似**")
print(" 不一致 %d / %d 步(%.1f%%)"
% (disagree, len(track), 100.0 * disagree / max(1, len(track))))
if elevs:
print(" 仰角範圍 %.2f - %.2f deg(門檻 %.1f)"
% (min(elevs), max(elevs), MIN_ELEV_DEG))
print(" -> 不一致率就是「用便宜的那個要付多少失真」的答案。")
print(" 它不為零很正常:STK 的 access 還含存取約束與橢球體,")
print(" 本地那一版是球面近似。**重點是這個數字有被量出來。**")
if flips == 0:
print(" **一次都沒翻 —— 這代表閉迴路沒有真的作用**,")
print(" 可能是地面站位置或門檻讓鏈路全程可用/全程中斷。")
# 與單向串流對照:單向可以攢批,閉迴路不行
print()
print(" 對照:單向串流每點約 0.65 ms(攢批 50 點一次往返,實測)。")
if avg > 0:
print(" 閉迴路每步 %.3f ms —— **貴 %.0f 倍**,"
% (avg, avg / 0.65))
print(" 而且貴的原因不只是多一趟:**閉迴路不能攢批**,")
print(" 因為下一步要等這一步的答案。")
result = {"mode": mode, "steps": len(track), "wall_s": wall,
"disagree": disagree,
"avg_ms": avg, "median_ms": med, "worst_ms": worst,
"loop_hz": hz, "flips": flips, "used_stk": bool(stk),
"gs": [GS_LAT, GS_LON, GS_ALT_KM], "min_elev_deg": MIN_ELEV_DEG,
"track": track}
io.open(CL_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(result, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(CL_OUT))
if stk is not None:
stk.shutdown()
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_comm.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — RF 鏈路預算:把「斷鏈」從仰角門檻換成真的餘裕。
## 這一節補的缺口
前面幾節判斷「鏈路通不通」用的是**仰角門檻**:超過 10 度就算通。
那是個幾何條件,不是無線電條件。真實的斷鏈取決於接收端的
**訊雜比有沒有超過解調門檻**,而那牽涉到功率、天線、距離、頻率、
大氣、以及(載入地形之後)遮蔽。
所以這一支建一條真的鏈路:無人機上的發射機、地面站的接收機,
讓 STK 去算鏈路預算,再用**餘裕**而不是仰角來判斷通斷。
## 判準不對 STK 自己:拿自由空間路徑損耗對
鏈路預算最容易出錯的地方是單位與方向(dB 加成 dBm、增益寫成損耗)。
而這種錯**不會讓數字變得離譜**,只會讓它偏個十幾 dB —— 看起來還很合理。
所以本檔的驗證不是「C/N 看起來合理」,而是拿**自由空間路徑損耗的閉合式**
去對 STK 算出來的值:
FSPL(dB) = 20 log10(d) + 20 log10(f) + 20 log10(4 pi / c)
在沒有大氣、沒有遮蔽、等向天線的設定下,STK 報的接收功率應該等於
Pr = Pt + Gt + Gr - FSPL
而這是**可以逐項對的**。對得上,才有資格相信含大氣與地形時的數字。
## 與地形的關係
載入地形之後,山擋住視線時 access 直接消失 —— 鏈路預算連算都不用算。
所以地形與鏈路預算是**兩層不同的門檻**:
地形遮蔽 -> 有沒有視線(0 或 1)
鏈路餘裕 -> 有視線的時候,訊號夠不夠強(連續值)
把兩者混在一起(例如只用仰角門檻)會同時失去兩邊的資訊:
看不到「明明有視線但訊號不夠」,也看不到「訊號本來夠但被山擋住」。
python grpc_link_comm.py 建鏈路、算預算、對 FSPL
python grpc_link_comm.py --dem <路徑> 再比對「有地形 vs 沒地形」
"""
import io
import json
import math
import os
import sys
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_realtime as R # noqa: E402
COMM_OUT = os.path.join(HERE, "grpc_link_comm.json")
PORT = 40704
C_LIGHT = 299792458.0 # m/s(定義值)
# 無人機遙測鏈路的典型參數。**都是設計點,不是量出來的** ——
# 標在這裡是為了讓人能改,而不是為了假裝它們有權威性。
FREQ_HZ = 2.4e9 # 2.4 GHz
TX_POWER_W = 1.0 # 1 W = 30 dBm
DATA_RATE = 115200.0 # bps
RX_GT = 5.0 # 接收端 G/T(dB/K)
EBNO_MIN_DB = 10.0 # 解調門檻(BPSK 加編碼的常見值)
def fspl_db(range_km, freq_hz=FREQ_HZ):
"""自由空間路徑損耗(dB)。**閉合式,不經 STK。**
FSPL = 20 log10(4 pi d f / c)。寫成這個形式而不是拆成三項,
是為了少一次湊常數的機會 —— 那個 32.44 的常數每次都有人記錯單位。
"""
d_m = range_km * 1000.0
return 20.0 * math.log10(4.0 * math.pi * d_m * freq_hz / C_LIGHT)
def dbw(watts):
return 10.0 * math.log10(watts)
def build_link(root, scen, uav_name="CommUAV", gs_name="CommGS",
gs_lat=None, gs_lon=None, gs_alt_km=0.0):
"""建無人機的發射機與地面站的接收機。回傳 (uav, gs) 的路徑。
用 Connect 而不是物件模型:comm 物件的屬性樹很深,
而 `week11_comm` 已經有一份驗過的 Connect 寫法,照它的形狀走
比重新摸索物件模型可靠。**注意天線型別要先設** ——
改 Antenna 會換掉天線物件,之前寫的 DesignFrequency 會跟著消失
(這是 week11 記下來的坑)。
"""
gs_lat = R.LAT0 - 0.04 if gs_lat is None else gs_lat
gs_lon = R.LON0 - 0.04 if gs_lon is None else gs_lon
uav = "*/Aircraft/%s" % uav_name
gs = "*/Facility/%s" % gs_name
cmds = [
'New / */Facility %s' % gs_name,
'SetPosition %s Geodetic %.6f %.6f %.6f' % (gs, gs_lat, gs_lon, gs_alt_km),
# --- 無人機上的發射機 ---
'New / %s/Sensor UavAnt' % uav,
'New / %s/Sensor/UavAnt/Transmitter UavTx' % uav,
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model Complex_Transmitter_Model' % uav,
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model.Frequency %.1f' % (uav, FREQ_HZ),
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model.Power %.3f' % (uav, TX_POWER_W),
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model.DataRate %.1f' % (uav, DATA_RATE),
# 天線型別先設,再設它的參數 —— 順序顛倒會被洗掉
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model.AntennaControl.Antenna Isotropic' % uav,
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model.AntennaControl.Antenna.DesignFrequency %.1f' % (uav, FREQ_HZ),
'Transmitter %s/Sensor/UavAnt/Transmitter/UavTx SetValue Model.Modulator BPSK' % uav,
# --- 地面站的接收機 ---
'New / %s/Sensor GsAnt' % gs,
'New / %s/Sensor/GsAnt/Receiver GsRx' % gs,
'Receiver %s/Sensor/GsAnt/Receiver/GsRx SetValue Model Simple_Receiver_Model' % gs,
'Receiver %s/Sensor/GsAnt/Receiver/GsRx SetValue Model.GOverT %.2f' % (gs, RX_GT),
# `Model.AutoTrackFrequency` 是 **Complex** 接收機才有的屬性,
# 在 Simple_Receiver_Model 上會 NACK。簡單接收機本來就跟著
# 發射頻率走,不需要也不能設。
]
ack = nack = 0
fails = []
for c in cmds:
try:
root.execute_command(c)
ack += 1
except Exception as e: # noqa: BLE001
nack += 1
fails.append((c[:90], str(e)[:70]))
return uav, gs, ack, nack, fails
def link_budget(root, tx_path, rx_path, start, stop, step=10.0):
"""讀鏈路預算。回傳 (欄位字典, 列數)。
走 access 的 "Link Information" data provider —— 那是 STK 把
整條鏈路的每一項(EIRP、路徑損耗、接收功率、C/N、Eb/No)
攤開來的地方。**逐項都能對**,而只讀一個 C/N 就對不了。
"""
tx = root.get_object_from_path(tx_path)
rx = root.get_object_from_path(rx_path)
acc = tx.get_access_to_object(rx)
acc.compute_access()
dp = acc.data_providers.item("Link Information")
# **`is_group` 不可靠**:實測「Link Information」的 is_group 是真,
# 但物件上根本沒有 `group` 屬性(DataProviderTimeVarying)。
# 所以改用 hasattr 判斷 —— 問物件「你有沒有這個東西」,
# 比問它「你是不是那一類」可靠。
if hasattr(dp, "group"):
try:
dp = dp.group.item(0)
except Exception: # noqa: BLE001
pass
res = dp.execute(start, stop, step)
out = {}
for name in ("Time", "Range", "EIRP", "Rcvd. Frequency",
"Free Space Loss", "Rcvd. Iso. Power", "C/N", "Eb/No"):
try:
out[name] = list(res.data_sets.get_data_set_by_name(name).get_values())
except Exception: # noqa: BLE001
pass
n = min((len(v) for v in out.values()), default=0)
return out, n
def main():
dem = None
if "--dem" in sys.argv:
dem = sys.argv[sys.argv.index("--dem") + 1]
print("=" * 78)
print(" RF 鏈路預算 — 把「斷鏈」從仰角門檻換成真的餘裕")
print("=" * 78)
print(" 設計點:%.2f GHz · %.1f W(%.1f dBW)· %.0f bps · G/T %.1f dB/K"
% (FREQ_HZ / 1e9, TX_POWER_W, dbw(TX_POWER_W), DATA_RATE, RX_GT))
print(" 解調門檻 Eb/No >= %.1f dB" % EBNO_MIN_DB)
try:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0", grpc_port=PORT,
grpc_timeout_sec=90,
no_graphics=True)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:110])
return 1
root = app.new_object_root()
root.new_scenario("CommLink")
scen = root.current_scenario
t, lat, lon, alt = R.plant(n=200)
stamps = R.timestamps(t)
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"' % (stamps[0], stamps[-1]))
root.execute_command("SetUnits / km")
if dem:
try:
from ansys.stk.core.stkobjects import TerrainFileType
earth = scen.terrain.get_item_by_name("Earth")
tobj = earth.terrain_collection.add(
dem, TerrainFileType.TIFF_TERRAIN_FILE)
tobj.use_terrain = True
print(" 地形已載入:%s" % os.path.basename(dem))
print(" **注意**:這一步會把航路限制在地形檔的涵蓋範圍內 ——")
print(" 出界時 STK 的 ComputeAccess 不會回來(見 grpc_link_terrain.py)。")
except Exception as e: # noqa: BLE001
print(" 地形載入失敗:%s" % str(e)[:100])
dem = None
ac, route, pts = R.make_realtime_aircraft(root, "CommUAV")
pts.add_position_batch(stamps, list(lat), list(lon), list(alt))
route.propagate()
uav, gs, ack, nack, fails = build_link(root, scen)
print()
print(" 建鏈路:%d 條 ACK / %d 條 NACK" % (ack, nack))
for c, m in fails[:6]:
print(" NACK %s" % c)
print(" %s" % m)
if nack:
print(" **有 NACK,先修掉再談數字**")
app.shutdown()
return 1
tx_path = "%s/Sensor/UavAnt/Transmitter/UavTx" % uav
rx_path = "%s/Sensor/GsAnt/Receiver/GsRx" % gs
try:
cols, n = link_budget(root, tx_path, rx_path,
scen.start_time, scen.stop_time, 10.0)
except Exception as e: # noqa: BLE001
print(" 讀不到鏈路預算:%s" % str(e)[:140])
app.shutdown()
return 1
if not n:
print(" 鏈路預算是空的 —— 可能整段都沒有存取窗。")
app.shutdown()
return 1
print(" 鏈路預算 %d 列,欄位:%s" % (n, ", ".join(sorted(cols))))
# ---- 獨立對照:自由空間路徑損耗 ----
rng = cols.get("Range")
fsl = cols.get("Free Space Loss")
worst = None
if rng and fsl:
diffs = []
for i in range(min(len(rng), len(fsl))):
want = fspl_db(float(rng[i]))
got = abs(float(fsl[i])) # STK 報成負值(損耗)
diffs.append(got - want)
worst = max(abs(d) for d in diffs)
print()
print(" **獨立對照:STK 的自由空間損耗 vs 閉合式 20log10(4 pi d f / c)**")
print(" 距離 %.2f - %.2f km" % (min(map(float, rng)), max(map(float, rng))))
print(" 最大差 %.4f dB(平均 %+.4f dB)"
% (worst, sum(diffs) / len(diffs)))
print(" 這一項對不上就不必看後面 —— 單位或方向搞錯了。")
ebno = cols.get("Eb/No")
if ebno:
vals = [float(x) for x in ebno]
ok = sum(1 for v in vals if v >= EBNO_MIN_DB)
print()
print(" Eb/No %.2f - %.2f dB;超過門檻 %.1f dB 的比例 %.1f%%(%d/%d)"
% (min(vals), max(vals), EBNO_MIN_DB,
100.0 * ok / len(vals), ok, len(vals)))
margin_lo = min(vals) - EBNO_MIN_DB
print(" 餘裕 %.2f - %.2f dB"
% (margin_lo, max(vals) - EBNO_MIN_DB))
if margin_lo > 20.0:
print()
print(" **餘裕有 %.0f dB —— 這本身就是結論。**" % margin_lo)
print(" 這條鏈路在視線內根本不會因為距離而斷:要把 %.0f dB 吃掉,"
% margin_lo)
print(" 距離得拉到 %.0f 倍(每四倍距離才掉 12 dB)。"
% (10 ** (margin_lo / 20.0)))
print(" 所以在這個設計點下,**限制因素不是距離,是遮蔽** ——")
print(" 而遮蔽只有載入地形之後才算得出來(見 grpc_link_terrain.py)。")
print(" 用仰角門檻當「斷鏈」的近似之所以還堪用,原因也在這裡:")
print(" 它其實是在近似遮蔽,不是在近似鏈路預算。")
out = {"freq_hz": FREQ_HZ, "tx_power_w": TX_POWER_W,
"data_rate": DATA_RATE, "rx_gt": RX_GT,
"ebno_min_db": EBNO_MIN_DB, "rows": n,
"fspl_worst_diff_db": worst,
"terrain": os.path.basename(dem) if dem else None,
"columns": {k: [float(x) for x in v] for k, v in cols.items()
if k != "Time"}}
io.open(COMM_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(out, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(COMM_OUT))
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_diagrams.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 架構圖。
## 為什麼自己畫,不用自動佈局
Simulink 的 `arrangeSystem` 與各種流程圖工具都會**自動排版**,
而自動排版的產物在投影片上常常是:線穿過標籤、方塊疊在一起、
箭頭指向兩個方塊之間的空白。那在螢幕上還看得懂,投影出來就不行了。
所以這一支的每一個方塊都有**明確的座標**。多寫幾行,換到的是
「印出來一定不會疊」。
## 五張圖各自回答什麼
(1) 整體資料流 這條鏈有哪幾段,介面是什麼
(2) 六層診斷梯 「連不上」的七個成因各自被哪一層攔下
(3) 單向 vs 閉迴路 為什麼攢批在一邊有效、另一邊無效
(4) 多機的成本結構 上限是結構決定的,不是實作差
(5) 從太空到無人載具 同一種失敗在兩個領域重複出現
**每一張都只回答一件事。** 一張圖要講三件事,結果通常是三件都講不清楚。
python grpc_link_diagrams.py
"""
import os
import sys
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt # noqa: E402
from matplotlib.patches import FancyArrowPatch, FancyBboxPatch # noqa: E402
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
sys.path.insert(0, os.path.join(HERE, os.pardir, "tools"))
# 類別色:固定順序,不循環(與本章其他圖一致)
C_BLUE, C_ORANGE, C_AQUA = "#2a78d6", "#eb6834", "#1baf7a"
INK, MUTED, GRID = "#14181d", "#5a6876", "#dde4ec"
WARN = "#b4431c"
PAPER = "#ffffff"
SOFT = "#f2f6fb"
def plain(t):
"""去掉 `**` 標記。
程式碼裡用 `**` 標重點很自然,但 matplotlib 不吃 markdown ——
直接印出來就會在圖上看到一堆星號。
所以在畫之前統一剝掉,而不是在每一處記得不要寫。
"""
return t.replace("**", "")
def _canvas(w, h, title, sub=None):
fig, ax = plt.subplots(figsize=(w, h))
fig.patch.set_facecolor(PAPER)
ax.set_facecolor(PAPER)
ax.set_xlim(0, 100)
ax.set_ylim(0, 100)
ax.axis("off")
ax.text(2, 97, plain(title), fontsize=15, color=INK, va="top", ha="left",
fontweight="bold")
if sub:
ax.text(2, 92.2, plain(sub), fontsize=10, color=MUTED, va="top", ha="left")
return fig, ax
def box(ax, x, y, w, h, text, face=SOFT, edge=C_BLUE, fs=9.5, bold=False,
tcol=INK):
"""畫一個方塊。**座標是左下角**,不是中心 —— 排版時比較好算。"""
ax.add_patch(FancyBboxPatch((x, y), w, h,
boxstyle="round,pad=0.6,rounding_size=1.2",
linewidth=1.4, edgecolor=edge,
facecolor=face, zorder=2))
ax.text(x + w / 2, y + h / 2, plain(text), ha="center", va="center",
fontsize=fs, color=tcol, zorder=3,
fontweight="bold" if bold else "normal", linespacing=1.45)
def arrow(ax, x1, y1, x2, y2, label=None, col=MUTED, fs=8.5, rad=0.0,
lab_dx=0.0, lab_dy=1.6, style="-|>"):
ax.add_patch(FancyArrowPatch((x1, y1), (x2, y2),
arrowstyle=style, mutation_scale=14,
linewidth=1.5, color=col, zorder=1,
connectionstyle="arc3,rad=%.2f" % rad))
if label:
ax.text((x1 + x2) / 2 + lab_dx, (y1 + y2) / 2 + lab_dy, plain(label),
ha="center", va="bottom", fontsize=fs, color=col, zorder=3)
# ------------------------------------------------------------------ (1)
def fig_dataflow(path):
"""整體資料流。**橫向三段,介面標在箭頭上** —— 介面是資料,不是函式呼叫。"""
fig, ax = _canvas(13.6, 6.6, "資料流:三段各做一件事,介面是資料",
"換掉任一段,其他兩段不動 —— MAVLink 模擬器換成 Simulink "
"時,橋接與 STK 側一行都沒改")
box(ax, 3, 58, 24, 23,
"載具模擬\n\nMAVLink 模擬器(多機)\nSimulink(雙向)\n6DOF 動力學",
edge=C_ORANGE, fs=9.5)
box(ax, 38, 58, 24, 23,
"橋接\n\n解封包 · 攢批\n轉絕對時戳\n漏包統計",
edge=C_BLUE, fs=9.5)
box(ax, 73, 58, 24, 23,
"STK\n\nRealtime 星曆\n地形 · RF · GPS\n可見度與鏈路餘裕",
edge=C_AQUA, fs=9.5)
arrow(ax, 27.5, 69.5, 37.5, 69.5, "UDP\n32 bytes / 幀", col=C_ORANGE, lab_dy=1.0)
arrow(ax, 62.5, 69.5, 72.5, 69.5, "gRPC\n:40704", col=C_AQUA, lab_dy=1.0)
# 回饋線走下方,避開上面的方塊
arrow(ax, 85, 57, 85, 45, col=WARN, style="-")
arrow(ax, 85, 45, 50, 45, col=WARN, style="-")
arrow(ax, 50, 45, 15, 45, col=WARN)
arrow(ax, 15, 45, 15, 57, col=WARN)
ax.text(50, 41.5, "回饋:鏈路可用性 / GPS 品質 -> MAVLink 指令(RTL)",
ha="center", va="top", fontsize=9.5, color=WARN)
ax.text(50, 22, plain("**閉迴路的代價**:不能攢批 —— 下一步要等這一步的答案"),
ha="center", va="top", fontsize=10, color=INK)
ax.text(50, 15,
"單向 0.65 ms/點(攢批 50 點一次往返) · "
"閉迴路 1.6-13 ms/步(每步一次查詢)",
ha="center", va="top", fontsize=9.5, color=MUTED)
fig.tight_layout()
fig.savefig(path, dpi=140, facecolor=PAPER)
plt.close(fig)
# ------------------------------------------------------------------ (2)
def fig_ladder(path):
"""六層診斷梯。**每層一列,成因標在右邊** —— 左右分欄就不會擠。"""
fig, ax = _canvas(13.6, 7.4, "六層診斷梯:每一層的失敗恰好對應一個成因",
"輸出不是「連不上」,而是「第 2 層過、第 3 層不過 -> "
"那個埠有人在聽,但聽的不是 gRPC 伺服器」")
rows = [
("第 0 層", "Python 依賴", "grpcio / protobuf 沒裝", C_BLUE),
("第 1 層", "TCP 可達", "**被拒絕** = 伺服器沒開 · "
"**逾時** = 防火牆丟包", C_BLUE),
("第 2 層", "gRPC 通道就緒", "埠有人聽,但聽的不是 gRPC", C_BLUE),
("第 3 層", "STK 交握", "授權或模組不足(網路沒問題)", C_AQUA),
("第 4 層", "物件模型往返", "交握過了,呼叫仍失敗", C_AQUA),
("第 5 層", "Connect 能力面", "前五層全綠,指令仍 NACK", C_ORANGE),
]
y = 78
for tag, name, cause, col in rows:
box(ax, 3, y, 11, 8.5, tag, edge=col, fs=9.5, bold=True)
box(ax, 16, y, 25, 8.5, name, edge=col, fs=10)
ax.text(44, y + 4.25, cause.replace("**", ""), fontsize=9.5,
color=MUTED, va="center", ha="left")
if y > 20:
arrow(ax, 8.5, y - 0.3, 8.5, y - 3.2, col=GRID)
y -= 11.8
ax.text(3, 6.5,
"第 5 層回報的是**能力清單,不是是非題** —— "
"通聯成功不等於功能齊全。",
fontsize=10, color=INK, va="center")
ax.text(3, 2.0,
"四種失敗成因(拒絕 / 逾時 / DNS / 非 gRPC)各自可用注入造出,"
"驗證段裡有對應的檢查。",
fontsize=9, color=MUTED, va="center")
fig.tight_layout()
fig.savefig(path, dpi=140, facecolor=PAPER)
plt.close(fig)
# ------------------------------------------------------------------ (3)
def fig_loop(path):
"""單向 vs 閉迴路。**上下兩條,時間軸對齊** —— 差別一眼看出來。"""
fig, ax = _canvas(13.6, 6.4, "為什麼攢批在單向有效、在閉迴路無效",
"攢批是單向便宜的唯一原因;閉迴路下一步要等這一步的答案")
# 單向
ax.text(2, 76, "單向串流", fontsize=11.5, color=C_BLUE, fontweight="bold")
for i in range(6):
box(ax, 16 + i * 8, 66, 6.5, 7, "點", edge=C_BLUE, fs=8.5)
arrow(ax, 64.5, 69.5, 72, 69.5, col=C_BLUE)
box(ax, 73, 64.5, 22, 10, "一次 gRPC 往返\n(攢批 50 點)",
edge=C_BLUE, fs=9.5)
ax.text(2, 58, "0.65 ms / 點", fontsize=9.5, color=MUTED)
# 閉迴路
ax.text(2, 44, "閉迴路", fontsize=11.5, color=WARN, fontweight="bold")
xs = [16, 38, 60]
for i, x in enumerate(xs):
box(ax, x, 34, 7, 7, "點", edge=WARN, fs=8.5)
arrow(ax, x + 7.2, 37.5, x + 12.8, 37.5, col=WARN)
box(ax, x + 13, 34, 7, 7, "問", edge=WARN, fs=8.5, face="#fdeeea")
if i < 2:
arrow(ax, x + 20.2, 37.5, x + 21.8, 37.5, col=GRID)
ax.text(2, 26, "1.6 - 13 ms / 步", fontsize=9.5, color=MUTED)
ax.text(50, 20,
plain("**每一步都要等答案** —— 攢起來就等於延後了決策,所以攢不了。"),
ha="center", fontsize=10.5, color=INK)
ax.text(50, 13,
plain("多機時更嚴格:每架每幀一次查詢,而且同步呼叫要**依序等**。"),
ha="center", fontsize=9.5, color=MUTED)
ax.text(50, 6.5,
"實測每幀 2.2 / 3.7 / 6.6 ms(1 / 2 / 4 架)—— 與架數線性。",
ha="center", fontsize=9.5, color=MUTED)
fig.tight_layout()
fig.savefig(path, dpi=140, facecolor=PAPER)
plt.close(fig)
# ------------------------------------------------------------------ (4)
def fig_fleet_cost(path):
"""多機的成本結構。**左圖開迴路、右圖閉迴路**,用同一個縱軸比。"""
fig, (a1, a2) = plt.subplots(1, 2, figsize=(13.6, 5.6))
fig.patch.set_facecolor(PAPER)
n = [1, 2, 4, 8]
calls = [13, 26, 48, 96]
a1.bar([str(x) for x in n], calls, color=C_BLUE, width=0.55, zorder=3)
for x, c in zip(range(len(n)), calls):
a1.text(x, c + 2, str(c), ha="center", fontsize=10, color=C_BLUE)
a1.set_title("開迴路:往返數與架數線性", fontsize=12, color=INK, loc="left")
a1.set_xlabel("架數", fontsize=10, color=MUTED)
a1.set_ylabel("gRPC 往返次數", fontsize=10, color=MUTED)
a1.text(0.02, 0.92, "每架 12-13 次 —— 攢批壓得掉同一架的連續點",
transform=a1.transAxes, fontsize=9.5, color=MUTED)
n2 = [1, 2, 4]
per_frame = [2.2, 3.7, 6.6]
a2.plot([str(x) for x in n2], per_frame, "o-", color=WARN, lw=2, ms=9,
zorder=3)
for x, c in zip(range(len(n2)), per_frame):
a2.text(x, c + 0.35, "%.1f" % c, ha="center", fontsize=10, color=WARN)
a2.axhline(100.0 / 10, color=MUTED, ls="--", lw=1.2)
a2.text(2.05, 10.4, "10 Hz 的每幀預算 100 ms(此軸為 ms)",
fontsize=8.5, color=MUTED, ha="right")
a2.set_title("閉迴路:每幀成本與架數線性", fontsize=12, color=INK, loc="left")
a2.set_xlabel("架數", fontsize=10, color=MUTED)
a2.set_ylabel("每幀成本(ms)", fontsize=10, color=MUTED)
a2.set_ylim(0, 12)
a2.text(0.02, 0.92, "不能攢批,且同步呼叫要依序等",
transform=a2.transAxes, fontsize=9.5, color=MUTED)
for ax in (a1, a2):
ax.set_facecolor(PAPER)
for s in ("top", "right"):
ax.spines[s].set_visible(False)
for s in ("left", "bottom"):
ax.spines[s].set_color(GRID)
ax.tick_params(colors=MUTED, labelsize=9)
ax.grid(True, axis="y", color=GRID, lw=0.7, zorder=0)
ax.set_axisbelow(True)
fig.suptitle(plain("多機的上限是**結構**決定的,不是實作差"), fontsize=14,
color=INK, x=0.02, ha="left", y=0.98)
fig.tight_layout(rect=(0, 0, 1, 0.93))
fig.savefig(path, dpi=140, facecolor=PAPER)
plt.close(fig)
# ------------------------------------------------------------------ (5)
def fig_space_to_uav(path):
"""從太空到無人載具。**左右分欄對照**,中間放共用的方法論。"""
fig, ax = _canvas(13.6, 7.6, "同一種失敗,在兩個領域重複出現",
"右欄是無人載具這一章真的踩到的;左欄是先前章節記錄下來的")
ax.text(20, 84, "太空任務", fontsize=12.5, color=C_BLUE,
ha="center", fontweight="bold")
ax.text(80, 84, "無人載具", fontsize=12.5, color=C_ORANGE,
ha="center", fontweight="bold")
rows = [
("ACK 了但沒生效",
"SetHeightAboveGround 有 ACK,\n回讀仍是 False",
"存取約束掛在**兩端**,\n只移除一端沒有用"),
("失效時不報錯,直接停住",
"離開地形檔範圍 ->\nComputeAccess 再也不回來",
"星曆太短 -> access 回空清單,\n被讀成「鏈路斷了」"),
("自洽但都錯",
"兩個量用同一套錯記帳算,\n互相對照一定過",
"能量檢查重算係數、\n漏了俯仰率項"),
("多條路互相驗證",
"解析 / 數值 / Astrogator\n三路一致",
"直接 / Simulink / MAVLink 三路,\n預期差各自算出"),
]
y = 62
for name, left, right in rows:
ax.text(50, y + 9.5, name.replace("**", ""), fontsize=10.5, color=INK,
ha="center", fontweight="bold")
box(ax, 3, y, 40, 8.6, left.replace("**", ""), edge=C_BLUE, fs=9)
box(ax, 57, y, 40, 8.6, right.replace("**", ""), edge=C_ORANGE, fs=9)
y -= 15.5
ax.text(50, 6,
"工具會 ACK、會回讀、會給你一個數字 —— 那都不等於它算對了。",
ha="center", fontsize=10.5, color=INK)
fig.tight_layout()
fig.savefig(path, dpi=140, facecolor=PAPER)
plt.close(fig)
FIGS = [
("arch_dataflow.png", fig_dataflow),
("arch_ladder.png", fig_ladder),
("arch_loop.png", fig_loop),
("arch_fleet_cost.png", fig_fleet_cost),
("arch_space_to_uav.png", fig_space_to_uav),
]
def main():
try:
import cjk
cjk.use_cjk()
except Exception: # noqa: BLE001
pass
matplotlib.rcParams["axes.unicode_minus"] = False
fams = list(matplotlib.rcParams.get("font.sans-serif") or [])
if "DejaVu Sans" not in fams:
matplotlib.rcParams["font.sans-serif"] = fams + ["DejaVu Sans"]
print("=" * 74)
print(" 架構圖(每一張只回答一件事,座標寫死以免重疊)")
print("=" * 74)
for name, fn in FIGS:
p = os.path.join(HERE, name)
fn(p)
print(" %-24s %8.0f KB" % (name, os.path.getsize(p) / 1024))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_fb_ok.m
function ok = grpc_link_fb_ok(t_now)
% GRPC_LINK_FB_OK 回饋的「鏈路可用嗎」,**只回一個 double**(1 或 0)。
%
% ## 為什麼要這一層薄包裝
%
% `grpc_link_udp_recv` 回傳四個值,而 Simulink 的 MATLAB Function 區塊
% 用 `coder.extrinsic` 呼叫多輸出函式時很脆弱:每一個輸出都要先預先賦值
% 把型別定下來,而 extrinsic 回來的是 mxArray,一旦被當成一般數值用
% 就會炸,錯誤訊息還完全看不出跟 extrinsic 有關。
%
% 單一輸出就沒有這個問題。所以模型裡呼叫這一支,
% 需要仰角或回饋年齡時再另外呼叫 `grpc_link_udp_recv`。
%
% **少的那些資訊不是不重要** —— 回饋年齡尤其重要(老掉的回饋和沒有回饋
% 是兩回事)。只是把它擠進同一個 Simulink 區塊會讓這個模型的失敗模式
% 變多,而這個模型要示範的是迴路的形狀。要用年齡的話,
% `grpc_link_udp_recv` 本身就回傳它。
if nargin < 1
t_now = [];
end
[link_ok, ~, ~, ~] = grpc_link_udp_recv(t_now);
ok = double(link_ok);
end
grpc_link/grpc_link_fleetloop.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 多機閉迴路:STK 的判斷經 MAVLink 送回去改變飛行。
## 這一節補完的兩件事
`grpc_link_mavsim.py` 量了**開迴路**的多機成本(只送不收)。但 SWIL 的
價值在閉迴路,而閉迴路有兩個開迴路沒有的限制:
1. **不能攢批** —— 下一步要等這一步的答案。
2. **每架都要各自等** —— 問完第 1 架才能問第 2 架
(STK 的 Python API 是同步的,gRPC 底下雖然是 HTTP/2,
但呼叫端沒有並行入口)。
所以開迴路「8 架很輕鬆」這個結論**不能搬到閉迴路**。這一支把它量出來。
第二件事是:先前的閉迴路都在**本行程內**跑 plant,回饋只是改一個變數。
這一支的回饋是**真的送回去的 MAVLink 指令**,而對面是一個獨立行程的
模擬器 —— 它會照指令改變飛行。整條迴路因此是跨行程、跨協定的。
## 迴路的形狀
```
模擬器(N 架,獨立行程)
| 遙測(MAVLink,推播到 15550+i)
v
本檔:解碼 -> 注入 STK -> 問 STK「這架的鏈路還在嗎」
|
| 不在 -> 送 MAV_CMD_NAV_RETURN_TO_LAUNCH 到 14570+i
v
模擬器:target = home,state = TRANSIT -> 飛機真的掉頭
```
**遙測走推播埠,指令走模擬器自己綁的埠。** 兩條路不同 ——
推播是 `udpout`(模擬器主動送出),指令要送到它 `udpin` 綁的那個埠。
搞混的話指令會送進一個沒人聽的埠,而且**不會報錯**。
## 一個實測到的 STK 陷阱:約束掛在**兩端**
`LineOfSight` 這個存取約束**預設同時掛在載具與地面站上**。
只從載具移除它**沒有用** —— 地面站那一個照樣擋,而且不會有任何訊息。
這一點是被這一節逼出來的:載具在 90 公尺高、離地面站幾百公尺、
仰角二十幾度,access 卻一直是 0 段。查的順序是
「期間對不對 -> 星曆進去了嗎 -> 高度掃描 -> 移除約束」,
而前三步全部正常,直到**兩端都移除**才出現可見窗。
所以處理 access 的時候要記得:**access 是一對物件的事,約束也是。**
## 怎麼證明迴路真的閉了
不看旗標,看**行為**:RTL 之後,載具到起飛點的距離應該開始**單調下降**。
旗標在這裡靠不住 —— 模擬器的心跳把 RTL 和 LOITER 都送成同一個
custom_mode(它只區分 GUIDED 與其他)。所以「模式變了」這件事
在協定上看不出來,但「它開始往家飛」看得出來。
**這比旗標更強**:旗標只證明指令被收下,距離下降證明它被**執行**了。
set GRPC_LINK_MAVSIM=D:\path\to\mav_sim.py
python grpc_link_fleetloop.py # 4 架,STK access 回饋
python grpc_link_fleetloop.py --n 2 --mode local
python grpc_link_fleetloop.py --scaling # 掃 1/2/4 架的閉迴路成本
"""
import io
import json
import math
import os
import sys
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_ardupilot as AP # noqa: E402
import grpc_link_closedloop as CL # noqa: E402
import grpc_link_mavsim as MS # noqa: E402
import grpc_link_realtime as R # noqa: E402
FLEET_OUT = os.path.join(HERE, "grpc_link_fleetloop.json")
# 模擬器的家在 24.99 N / 121.30 E(它的預設值)。地面站放在西南邊,
# 讓載具繞出去之後仰角掉下來 —— 與 grpc_link_closedloop 同樣的手法,
# 但這裡的「掉下來」會真的讓飛機掉頭。
GS_LAT, GS_LON = 24.99, 121.30 # 與模擬器的場地中心同點
# 巡航高度。**不能用模擬器的預設 90 m。**
#
# 實測到一個 STK 的邊界:`LineOfSight` 存取約束在載具高度約 90 公尺以下時
# 判定不可見,即使幾何上仰角超過 20 度、距離不到一公里。門檻落在 90-100 m 之間:
#
# 30 / 90 m -> access 0 段
# 100 / 120 / 150 / 180 / 200 m -> access 1 段
#
# 而模擬器的預設巡航高度正好是 90 m —— **卡在邊界上**,
# 於是整個閉迴路看起來像「鏈路永遠是斷的」。
# 拉到 150 m 既避開邊界,對固定翼無人機也完全合理。
PATROL_ALT_M = 150.0
# 遮蔽角。**這個值是從幾何與時間一起推回來的,不是挑好看的。**
# 高度 150 m、地速 18 m/s(模擬器的設計點),仰角 = atan(150 / 地面距離):
#
# 遮蔽角 觸發距離 從出發飛到那裡要多久
# 25 度 322 m 來不及飛就斷了(回程被待命繞圈的擺盪蓋掉)
# 8 度 1,068 m 約 45 秒 —— 出得去、回得來,兩段都看得見
# 5 度 1,714 m 約 81 秒 —— 出程就吃掉大半個觀測窗
#
# 實測踩過兩次:25 度那一版第一幀就觸發;2 度那一版(當時高度 90 m)
# 要飛 127 秒而只收 45 秒,**RTL 一次都沒觸發**。
# 門檻不是挑「合理的值」就好,還要跟**觀測時間**對得起來。
MIN_ELEV_DEG = 8.0
OUTBOUND_M = 4000.0 # 先把載具派出去多遠(要遠大於觸發距離)
# 收集時間。**用最壞情況估,不是用實際起點估。**
# 載具的起點已經在場地外緣(模擬器預設散開 180 m),所以實際出程約 43 s;
# 但如果從地面站正上方起飛,出程就是 1029/18 = 57 s。
# 檢查用最壞情況,時間也就照最壞情況給 —— 不然這個一致性檢查
# 會因為「剛好夠」而在別人改一個參數時突然變紅。
# access 至少要這麼長的**星曆跨度**才採信(秒)。
#
# **守衛要設在「跨度」,不是「點數」。** 遙測約 5 Hz,所以 10 點只有
# 兩秒的星曆 —— access 照樣算不出東西,於是 RTL 在載具離家
# 一兩百公尺時就觸發,看起來像控制律壞了。
#
# 獨立探測:注入 40 點、跨度 39 秒的軌跡,access 回得出區間。
# 所以門檻訂在 30 秒(比 39 保守一點),而且是**跨度**不是點數 ——
# 取樣率一變,點數的意義就變了,跨度不會。
MIN_SPAN_S_FOR_ACCESS = 30.0
COLLECT_S = 170.0 # 30 s 等星曆 + 45 s 出 + 45 s 回 + 餘裕
SCALING_S = 22.0 # --scaling 只量成本,不等 RTL 跑完(見 main)
def decide_link(span_s, rows, t_sec, min_span=None):
"""由「已注入的星曆跨度」與「access 區間」決定鏈路狀態。
回傳 **True(通)/ False(斷)/ None(還算不出來)** 三態,不是兩態。
抽成純函式有兩個理由:
1. **可以離線驗。** 這個決策不需要 STK —— 需要 STK 的只有「取得區間」。
把兩件事混在一起,就只能開著授權才驗得到最容易出錯的那一段。
2. **三態才寫得下去。** 混在呼叫裡的時候很容易寫成
`ok = len(rows) > 0`。那是錯的:星曆還太短、區間清單是空的時候,
會被讀成「鏈路斷了」,RTL 在載具離家 42 公尺時就觸發。
"""
if min_span is None:
min_span = MIN_SPAN_S_FOR_ACCESS
if span_s < min_span:
return None # 星曆太短 —— **不是**斷線
if not rows:
return False # 點夠多了還是沒有區間 -> 真的不可見
for r in rows:
try:
a, b = float(r[0]), float(r[1])
except (TypeError, ValueError):
continue
if a - 1e-6 <= t_sec <= b + 1e-6:
return True
return False
class Commander:
"""把指令送回模擬器。**送到它綁的埠,不是推播埠。**"""
def __init__(self, n, sim_base=MS.SIM_BASE):
from pymavlink import mavutil
self.mav = mavutil
self.conns = [
mavutil.mavlink_connection("udpout:127.0.0.1:%d" % (sim_base + i),
source_system=255,
dialect="ardupilotmega")
for i in range(n)]
self.sent = [0] * n
def heartbeat(self, i):
"""先送一則心跳。模擬器用它來認得「GCS 已連線」,
而且 `udpout` 要先送過東西,對端才會把我們當成已知的對象。"""
from pymavlink.dialects.v20 import ardupilotmega as ml
self.conns[i].mav.heartbeat_send(ml.MAV_TYPE_GCS,
ml.MAV_AUTOPILOT_INVALID, 0, 0,
ml.MAV_STATE_ACTIVE)
def goto(self, i, lat, lon, alt_m):
"""派到一個點(SET_POSITION_TARGET_GLOBAL_INT)。
先把載具派出去,鏈路才會在飛行中**逐漸**變差 ——
一開始就在門檻外的話,RTL 會在第一幀就觸發,
回程只有幾十公尺,會被待命繞圈的擺盪蓋掉,量不出東西。
"""
from pymavlink.dialects.v20 import ardupilotmega as ml
self.conns[i].mav.set_position_target_global_int_send(
0, i + 1, ml.MAV_COMP_ID_AUTOPILOT1,
ml.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0b0000111111111000,
int(lat * 1e7), int(lon * 1e7), float(alt_m),
0, 0, 0, 0, 0, 0, 0, 0)
def rtl(self, i):
"""送 RETURN_TO_LAUNCH。模擬器會把 target 設成 home 並進 TRANSIT。"""
from pymavlink.dialects.v20 import ardupilotmega as ml
self.conns[i].mav.command_long_send(
i + 1, ml.MAV_COMP_ID_AUTOPILOT1,
ml.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0)
self.sent[i] += 1
def close(self):
for c in self.conns:
try:
c.close()
except Exception: # noqa: BLE001
pass
def dist_m(lat1, lon1, lat2, lon2):
dn = (lat2 - lat1) * AP.M_PER_DEG_LAT
de = (lon2 - lon1) * AP.M_PER_DEG_LAT * math.cos(math.radians(lat1))
return math.hypot(dn, de)
# 待命繞圈的半徑(公尺)。模擬器待命時以 cruise x 0.45 = 8.1 m/s 繞圈、
# 航向每秒轉 8 度,所以半徑約 8.1 / rad(8) = 58 m,離家距離因此本來就
# 會在 ±58 m 之間擺盪。判斷「有沒有飛回家」必須勝過這個幅度。
LOITER_RADIUS_M = 58.0
def came_home(seq, tol=1e-6):
"""RTL 之後有沒有**回到家**。回傳 (是否, 回程段的下降佔比)。
## 判準為什麼不是「單調下降」
「離家距離單調下降」是個很自然但錯的要求,它會在兩個地方失效:
1. **尾巴**。載具抵達後會恢復待命繞圈,那段上下各半,
把一段 463 -> 126 m 的回程稀釋成 55%。**回程在抵達時就結束了**,
所以要先截到最小值再算。
2. **開頭**。RTL 之後載具**必須先轉向**,那幾秒距離還在增加。
截過尾巴之後仍只有 68%,就是這一段造成的。
兩者都不是異常,是飛行本來就會這樣。所以閘門問的是
「**有沒有實質接近**」,下降佔比只當診斷資訊。
現在的判準有兩條,都與待命繞圈的幅度掛鉤,所以擺盪假裝不了回程:
淨下降 > 2 x 待命繞圈半徑
最近距離 < 起點距離的 40%
這是一個一般性的教訓:**一個指標連調三次,通常代表它量的不是
你真正在問的東西。** 這裡真正在問的是「它有沒有回到家」,
而不是「它的距離序列長得好不好看」。
"""
if len(seq) < 5:
return False, 0.0
k = seq.index(min(seq))
leg = seq[:k + 1]
net = seq[0] - seq[k]
frac = 0.0
if len(leg) >= 3:
down = sum(1 for a, b in zip(leg, leg[1:]) if b < a + tol)
frac = down / (len(leg) - 1)
ok = (net > 2.0 * LOITER_RADIUS_M) and (seq[k] < 0.4 * seq[0])
return ok, frac
class FleetLoop:
"""N 架的閉迴路。回饋來自 STK,指令經 MAVLink 送回模擬器。"""
def __init__(self, n, mode="access", chunk=1):
# **chunk 固定為 1。** 閉迴路不能攢批 —— 下一步要等這一步的答案,
# 攢起來就等於延後了決策。這不是參數,是閉迴路的定義。
self.n = n
self.mode = mode
self.fleet = MS.Fleet(n, chunk=1)
self.cmd = Commander(n)
self.home = [None] * n
self.dist = [[] for _ in range(n)]
self.outbound_until = 0.0
self.unknown = [0] * n
self.rtl_at = [None] * n
self.costs = []
self.acs = []
self.routes = []
self.fac = None
self.root = None
def attach(self, root):
from ansys.stk.core.stkobjects import STKObjectType, AccessConstraintType
self.root = root
self.fleet.attach_stk(root)
self.acs = self.fleet.acs
self.routes = self.fleet.routes
self.fac = root.current_scenario.children.new(STKObjectType.FACILITY,
"FleetGS")
self.fac.position.assign_geodetic(GS_LAT, GS_LON, 0.0)
c = self.fac.access_constraints.add_constraint(
AccessConstraintType.ELEVATION_ANGLE)
c.enable_minimum = True
c.minimum = MIN_ELEV_DEG
return self
def link_ok(self, i, t_sec, lat, lon, alt_km):
"""這一架現在的鏈路通不通。回傳 (通不通 或 None, 這一步花的秒數)。
**回 None 代表「還算不出來」,不是「斷了」。**
開場星曆只有幾個點時,`compute_access()` 回傳的是空的區間清單。
把空清單當成「沒有可見窗」=「鏈路斷了」,RTL 就會在載具離家
只有 42-50 公尺時觸發 —— 而那個距離的仰角約 24 度,
遠高於 5 度門檻,根本不該斷。
症狀是「有幾架沒有往家飛」,看起來像控制律的問題;
真正的原因是**把「資料不足」讀成了「結果是否定的」**。
這與 GPS 那一節的「無解 vs 解很差」是同一個錯的兩種長相。
"""
t0 = time.time()
if self.mode == "access":
# 星曆太短就不要下結論。門檻取 10 點:少於這個數,
# access 的結果反映的是取樣不足,不是幾何。
tr = self.fleet.tracks[i]
span = (tr[-1][0] - tr[0][0]) if len(tr) >= 2 else 0.0
if span < MIN_SPAN_S_FOR_ACCESS:
# 連算都不用算 —— 省一次往返,也不會誤判
return None, time.time() - t0
self.routes[i].propagate()
acc = self.acs[i].get_access_to_object(self.fac)
acc.compute_access()
try:
rows = acc.computed_access_interval_times.to_array(0, -1)
except Exception: # noqa: BLE001
rows = []
ok = decide_link(span, rows, t_sec)
else:
ok = CL.elevation_deg(lat, lon, alt_km, GS_LAT, GS_LON, 0.0) \
>= MIN_ELEV_DEG
return ok, time.time() - t0
def run(self, seconds=COLLECT_S):
"""跑閉迴路。回傳統計。"""
for i in range(self.n):
self.cmd.heartbeat(i) # 先敲一下,讓對端認得我們
# **先把整隊派出去。** 各自散開成扇形,免得全部重疊在同一條航線上。
for i in range(self.n):
ang = 2.0 * math.pi * i / max(self.n, 1)
dlat = OUTBOUND_M * math.cos(ang) / AP.M_PER_DEG_LAT
dlon = (OUTBOUND_M * math.sin(ang)
/ (AP.M_PER_DEG_LAT * math.cos(math.radians(GS_LAT))))
self.cmd.goto(i, GS_LAT + dlat, GS_LON + dlon, PATROL_ALT_M)
# 這段期間不判斷斷鏈:要等星曆長到 access 算得動,
# 而那個門檻本身就是 MIN_SPAN_S_FOR_ACCESS。
self.outbound_until = time.time() + MIN_SPAN_S_FOR_ACCESS + 2.0
t_start = time.time()
frames = 0
while time.time() - t_start < seconds:
for i, c in enumerate(self.fleet.conns):
m = c.recv_match(blocking=False)
while m is not None:
typ = m.get_type()
if typ == "HOME_POSITION" and self.home[i] is None:
self.home[i] = (m.latitude * 1e-7, m.longitude * 1e-7)
elif typ == "GLOBAL_POSITION_INT":
t = m.time_boot_ms / 1000.0
if self.fleet.t0 is None:
self.fleet.t0 = t
lat = m.lat * AP.DEG_E7
lon = m.lon * AP.DEG_E7
alt = m.alt * AP.MM / 1000.0
frame = (t - self.fleet.t0, lat, lon, alt)
self.fleet.bridges[i].feed(frame)
self.fleet.tracks[i].append(frame)
ok, cost = self.link_ok(i, frame[0], lat, lon, alt)
self.costs.append(cost)
if ok is None:
# 還算不出來 —— **不要當成斷線**
self.unknown[i] += 1
frames += 1
if self.home[i] is not None:
self.dist[i].append(
dist_m(lat, lon, *self.home[i]))
# 剛派出去的前幾秒不判斷 —— 那時載具還在轉向,
# 而且我們要的是「飛出去之後才斷」,不是一開始就斷。
if (ok is False and self.rtl_at[i] is None
and time.time() > self.outbound_until):
self.cmd.rtl(i)
self.rtl_at[i] = len(self.dist[i]) - 1
m = c.recv_match(blocking=False)
time.sleep(0.002)
return frames
def close(self):
self.fleet.close()
self.cmd.close()
def summarise(loop, frames, wall):
ms = sorted(c * 1000.0 for c in loop.costs) or [0.0]
avg = sum(ms) / len(ms)
per_frame = avg * loop.n
out = {"n": loop.n, "mode": loop.mode, "frames": frames, "wall_s": wall,
"feedback_ms_avg": avg, "feedback_ms_med": ms[len(ms) // 2],
"feedback_ms_max": ms[-1],
"per_frame_ms": per_frame,
"max_hz": (1000.0 / per_frame) if per_frame > 0 else 0.0,
"rtl_sent": list(loop.cmd.sent),
"unknown": list(loop.unknown),
"grpc_calls": loop.fleet.calls()}
proofs = []
for i in range(loop.n):
k = loop.rtl_at[i]
if k is None or len(loop.dist[i]) - k < 5:
proofs.append(None)
continue
after = loop.dist[i][k:]
good, frac = came_home(after)
proofs.append({"from_m": after[0], "to_m": after[-1],
"min_m": min(after), "down_frac": frac,
"ok": bool(good)})
out["rtl_proof"] = proofs
return out
def run_once(path, n, mode, seconds=COLLECT_S):
proc = MS.start_sim(path, n, patrol_alt=PATROL_ALT_M)
time.sleep(4.0)
app = None
loop = FleetLoop(n, mode)
try:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
root = app.new_object_root()
root.new_scenario("FleetLoop%d" % n)
ts = R.timestamps([0.0, seconds * 6.0])
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"'
% (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
root.units_preferences.set_current_unit("DateFormat", "EpSec")
loop.attach(root)
t0 = time.time()
frames = loop.run(seconds)
wall = time.time() - t0
finally:
loop.close()
proc.terminate()
try:
proc.wait(timeout=5)
except Exception: # noqa: BLE001
proc.kill()
res = summarise(loop, frames, wall)
if app is not None:
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return res
def main():
path = MS.sim_path()
mode = "access"
if "--mode" in sys.argv:
mode = sys.argv[sys.argv.index("--mode") + 1]
n = 4
if "--n" in sys.argv:
n = int(sys.argv[sys.argv.index("--n") + 1])
scaling = "--scaling" in sys.argv
print("=" * 78)
print(" 多機閉迴路 — STK 的判斷經 MAVLink 送回去改變飛行")
print("=" * 78)
if not path or not os.path.isfile(path):
print(" 沒有指定 MAVLink 模擬器(GRPC_LINK_MAVSIM 或 --sim)。")
print(" 本教材不附它 —— 見 grpc_link_mavsim.py 的檔頭。")
return 1
print(" 回饋來源 %s 地面站 %.4f N %.4f E 遮蔽角 %.0f deg"
% (mode, GS_LAT, GS_LON, MIN_ELEV_DEG))
print(" 斷鏈時送 MAV_CMD_NAV_RETURN_TO_LAUNCH 回模擬器")
rows = []
for k in ((1, 2, 4) if scaling else (n,)):
print()
print(" --- %d 架 ---" % k)
r = run_once(path, k, mode,
seconds=(SCALING_S if scaling else COLLECT_S))
rows.append(r)
print(" %d 幀,回饋 %.2f ms/架(中位 %.2f,最差 %.2f)"
% (r["frames"], r["feedback_ms_avg"], r["feedback_ms_med"],
r["feedback_ms_max"]))
print(" 每幀 %d 架 x %.2f ms = %.1f ms -> 上限約 %.1f Hz"
% (k, r["feedback_ms_avg"], r["per_frame_ms"], r["max_hz"]))
print(" 送出 RTL:%s 「還算不出來」的幀數:%s"
% (r["rtl_sent"], r["unknown"]))
for i, pf in enumerate(r["rtl_proof"]):
if pf is None:
print(" 載具 %d:沒有觸發 RTL,或 RTL 後樣本不足" % (i + 1))
else:
print(" 載具 %d:RTL 後離家 %.0f -> 最近 %.0f m"
"(淨降 %.0f m,回程下降佔 %.0f%%) %s"
% (i + 1, pf["from_m"], pf["min_m"],
pf["from_m"] - pf["min_m"], pf["down_frac"] * 100,
"**迴路確實閉合**" if pf["ok"] else "**沒有往家飛**"))
if scaling:
print()
print(" **閉迴路的多機成本(對照開迴路)**")
print(" (掃描模式只量成本,收得短,所以 RTL 的行為證據不一定跑得完 ——")
print(" 那個證據在不加 --scaling 的完整一輪裡看。)")
print(" %4s %8s %14s %14s %10s"
% ("架數", "幀數", "回饋 ms/架", "每幀 ms", "上限 Hz"))
for r in rows:
print(" %4d %8d %14.2f %14.1f %10.1f"
% (r["n"], r["frames"], r["feedback_ms_avg"],
r["per_frame_ms"], r["max_hz"]))
print()
print(" 開迴路每架每秒只要 12-13 次往返,8 架仍然很輕鬆;")
print(" 閉迴路則是**每架每幀一次查詢,而且要依序等**。")
print(" 所以上限掉得很快 —— 這不是實作差,是同步呼叫的結構。")
io.open(FLEET_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"rows": rows}, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(FLEET_OUT))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_gps.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 雙向實驗子項:GPS 訊號可用度。
## 為什麼是 GPS
`grpc_link_closedloop.py` 的回饋是「地面站鏈路通不通」。這一支換成
**GPS 可用度**,理由有三個:
1. 它是 STK 真正擅長、而 Simulink 完全算不出來的東西 ——
需要整個 GPS 星座的軌道幾何。
2. 它不是一個二元值。**四顆才有解**,而幾何好壞由 DOP 決定;
所以回饋是連續的、有物理意義的量。
3. GNSS 拒止是無人機的真實情境,而且對應一個真實的飛控決策:
定位品質掉下去就切到航位推算、或返航。
## 星座是**真的**,不是標稱值
STK 內建 GPS almanac,所以這一支用 `PropagatorType.GPS` 建出實際的
PRN 星座(實測 `available_prns` 有 32 個),而不是自己湊一個
24/6/2 的 Walker 近似。差別在於真實星座有備援衛星、有不均勻的相位,
而那正是 DOP 會隨時間起伏的原因。
## 關鍵設計:一次預取,迴圈內本地求解
最直覺的寫法是每一步去問 STK「現在看得到幾顆」。那要對 32 顆各算一次
access —— 以本章量到的 access 成本(每次十幾毫秒)換算,一步就要
好幾百毫秒,迴路直接垮掉。
但 **GPS 星曆是慢變的**:衛星速度約 3.9 km/s,而軌道半徑約 26,560 km。
所以正確的做法是:
開場一次 把 32 顆的 ECEF 星曆整段取回來(32 次 DataProvider 呼叫)
迴圈每一步 用本地的線性內插取位置,算仰角與 DOP(0 次網路往返)
這不是偷懶,是**把慢變的環境與快變的決策分開** ——
而這正是本章從頭到尾的那一條原則:要壓的是往返次數,
不是讓每次往返更快。
內插的代價算得出來:步長 10 s、速度 3.9 km/s,弦高誤差約
(v·dt)²/(8R) ≈ 7 km。在 20,000 km 的距離上那是 0.02 度的方向誤差 ——
對 DOP 完全可以忽略。**這個數字是算出來的,不是假設的。**
## DOP 怎麼算,以及怎麼驗它
對每一顆可見衛星,取接收機到衛星的單位向量在當地 ENU 座標下的分量
(e, n, u),組成幾何矩陣
G 的每一列 = [-e, -n, -u, 1]
然後 Q = (Gᵀ G)⁻¹,各項 DOP 是 Q 對角線的平方根組合:
GDOP = sqrt(tr Q) PDOP = sqrt(Q11+Q22+Q33)
HDOP = sqrt(Q11+Q22) VDOP = sqrt(Q33) TDOP = sqrt(Q44)
**驗證不靠「看起來合理」**,靠兩條數學恆等式:
GDOP² = PDOP² + TDOP²
PDOP² = HDOP² + VDOP²
它們是定義的直接後果,所以對就是對,沒有容差可言。
再加一條外部對照:開闊天空、完整星座下 GDOP 通常落在 1.5–3。
## 控制律:定位品質掉下去就停止轉彎
GPS 可用(>= 4 顆且 HDOP <= 門檻)-> 維持坡度角
GPS 不可用 -> 坡度角歸零(直飛,航位推算)
一樣刻意簡單:要的是「STK 的判斷真的改變了飛行」在軌跡上看得出來。
python grpc_link_gps.py # 建星座、預取、跑閉迴路
python grpc_link_gps.py --mask 10 # 開闊天空(對照組,幾乎全程可用)
python grpc_link_gps.py --dop-only # 只算 DOP 剖面,不跑閉迴路
"""
import io
import json
import math
import os
import sys
import time
import numpy as np
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_closedloop as CL # noqa: E402
import grpc_link_realtime as R # noqa: E402
GPS_OUT = os.path.join(HERE, "grpc_link_gps.json")
# 外部參考值(公開文獻,不是本檔算的)——驗證段拿它們對
GPS_SMA_KM = 26560.0 # 標稱半長軸
GPS_PERIOD_S = 43082.0 # 標稱週期,約 11 小時 58 分(半個恆星日)
GPS_ALT_KM = 20180.0 # 標稱高度
# 接收機的遮蔽角。**預設 30 度不是隨手挑的,是掃出來的**,而它代表的是
# 深谷 / 城市峽谷 —— 也就是台灣山區低空飛行真正會遇到的情況。
# 全程 2 小時、真實 GPS almanac 的實測:
#
# 遮蔽角 10 度(開闊天空) 可見 9-10 顆 HDOP 0.81-1.05 可用 100.0%
# 遮蔽角 20 度 可見 7- 9 顆 HDOP 0.91-1.25 可用 100.0%
# 遮蔽角 25 度 可見 6- 8 顆 HDOP 1.07-1.46 可用 100.0%
# 遮蔽角 30 度 可見 5- 7 顆 HDOP 1.16-4.19 可用 96.4%
#
# **開闊天空下 GPS 幾乎永遠可用 —— 那才是正確答案,不是實驗失敗。**
# 所以有意義的情境不是開闊天空,是遮蔽。這一列本身就是結論:
# 想驗 GNSS 拒止的自動反應,就必須把遮蔽建進來,不然迴路永遠不會觸發。
#
# 遮蔽角再高會看到接近退化的幾何:實測 35 度時 GDOP 最大衝到 358,
# 那是「勉強有四顆但全擠在一起」的樣子,不是 bug。
MASK_DEG = 30.0
HDOP_MAX = 2.5 # 超過就當作定位品質不足
MIN_SVS = 4 # 四顆才有解 —— 這是基本原理,不是參數
EPH_STEP_S = 10.0 # 星曆預取步長(內插誤差見檔頭)
# **任務長度要以小時計,不是分鐘。** 這是實測掃出來的:
# GPS 星座的幾何變化很慢,跑 5 分鐘的話可見顆數從頭到尾固定 10 顆、
# GDOP 只在 2.44-2.45 之間動 —— 閉迴路完全沒有作用的機會。
# 拉到 6 小時、遮蔽角 10 度,可見顆數走 7-10 顆、GDOP 走 1.62-3.66,
# 其中 **16% 的時刻 HDOP 超過門檻**,閉迴路才有東西可反應。
#
# 順帶一提,遮蔽角一拉高就會看到接近退化的幾何:實測 25 度時
# GDOP 最大衝到 320(可見顆數掉到 3-8)。那不是 bug,
# 那是「勉強有四顆但全擠在一起」的樣子。
N_STEPS = 7200 # 2 小時 @ 1 s
DT = CL.DT
# ------------------------------------------------------------------ 幾何
def ecef_from_geodetic(lat_deg, lon_deg, alt_km):
"""球面地球的 ECEF(km)。與 grpc_link_closedloop 同一套近似。
用球面而不是橢球:DOP 只看**方向**,而 20,000 km 外的衛星方向
對接收機位置幾十公尺的差異完全不敏感。這是可以論證的簡化,
不是省事 —— 橢球與球面在台灣緯度差約 20 km,換算成方向是 0.06 度。
"""
re = R.RE_M / 1000.0
la, lo = math.radians(lat_deg), math.radians(lon_deg)
r = re + alt_km
return np.array([r * math.cos(la) * math.cos(lo),
r * math.cos(la) * math.sin(lo),
r * math.sin(la)])
def enu_basis(lat_deg, lon_deg):
"""當地 East / North / Up 的單位向量(ECEF 座標下)。"""
la, lo = math.radians(lat_deg), math.radians(lon_deg)
e = np.array([-math.sin(lo), math.cos(lo), 0.0])
n = np.array([-math.sin(la) * math.cos(lo),
-math.sin(la) * math.sin(lo), math.cos(la)])
u = np.array([math.cos(la) * math.cos(lo),
math.cos(la) * math.sin(lo), math.sin(la)])
return e, n, u
def visible_and_dop(rx_lat, rx_lon, rx_alt_km, sv_ecef, mask_deg=MASK_DEG):
"""回傳 (可見顆數, dop 字典 或 None, 仰角清單)。
`sv_ecef` 是 (N, 3) 的衛星 ECEF 位置(km)。
可見顆數不足 4 顆時 dop 回 None —— **不回一個很大的數字**。
「無解」與「解很差」是兩件事,把前者偽裝成後者會讓下游分不出來。
"""
rx = ecef_from_geodetic(rx_lat, rx_lon, rx_alt_km)
e, n, u = enu_basis(rx_lat, rx_lon)
rows, elevs = [], []
for sv in sv_ecef:
d = sv - rx
nd = np.linalg.norm(d)
if nd == 0.0:
continue
d = d / nd
up = float(np.dot(d, u))
elev = math.degrees(math.asin(max(-1.0, min(1.0, up))))
elevs.append(elev)
if elev < mask_deg:
continue
rows.append([-float(np.dot(d, e)), -float(np.dot(d, n)), -up, 1.0])
if len(rows) < MIN_SVS:
return len(rows), None, elevs
g = np.array(rows)
gtg = g.T @ g
try:
q = np.linalg.inv(gtg)
except np.linalg.LinAlgError:
# 幾何退化(例如所有衛星共面)。同樣回 None,不要編一個數字。
return len(rows), None, elevs
d11, d22, d33, d44 = (float(q[0, 0]), float(q[1, 1]),
float(q[2, 2]), float(q[3, 3]))
# **近退化時 inv() 不會失敗,它會安靜地給出垃圾。**
# 實測:五顆衛星全擠在天頂附近 1 度內,`inv` 正常回傳,
# 但對角線出現**負值** —— 而 DOP 的定義是共變異矩陣的對角線,
# 負的變異數沒有意義,不擋的話下游會是 `math domain error`。
#
# 崩潰其實比「回一個看起來正常的數字」好,但兩者都不對:
# 正確的回答是**無解**。所以這裡連同條件數一起擋掉。
if min(d11, d22, d33, d44) < 0.0:
return len(rows), None, elevs
if np.linalg.cond(gtg) > 1e12:
return len(rows), None, elevs
return len(rows), {
"GDOP": math.sqrt(d11 + d22 + d33 + d44),
"PDOP": math.sqrt(d11 + d22 + d33),
"HDOP": math.sqrt(d11 + d22),
"VDOP": math.sqrt(d33),
"TDOP": math.sqrt(d44),
}, elevs
def dop_identities(dop, tol=1e-9):
"""驗兩條恆等式。回傳 (是否成立, 兩個殘差)。
GDOP² = PDOP² + TDOP²;PDOP² = HDOP² + VDOP²。
**它們是定義的直接後果**,所以殘差應該落在浮點誤差等級。
這比「DOP 看起來合理」強得多 —— 後者抓不到座標軸弄錯之類的錯。
"""
if dop is None:
return True, (0.0, 0.0)
r1 = abs(dop["GDOP"] ** 2 - (dop["PDOP"] ** 2 + dop["TDOP"] ** 2))
r2 = abs(dop["PDOP"] ** 2 - (dop["HDOP"] ** 2 + dop["VDOP"] ** 2))
return (r1 < tol and r2 < tol), (r1, r2)
# ------------------------------------------------------------------ STK 側
class GpsConstellation:
"""在 STK 裡建出 GPS 星座,並把星曆**一次取回來**。"""
def __init__(self, root, scen, n_prn=32, step=EPH_STEP_S):
self.root, self.scen = root, scen
self.n_prn, self.step = n_prn, step
self.prns = []
self.times = None # (T,) 秒
self.eph = None # (T, N, 3) km,ECEF
self.build_s = self.fetch_s = 0.0
def build(self):
"""每個 PRN 建一顆衛星。用 STK 內建的 GPS almanac,不是標稱星座。"""
from ansys.stk.core.stkobjects import STKObjectType, PropagatorType
t0 = time.time()
made = []
for prn in range(1, self.n_prn + 1):
try:
sat = self.scen.children.new(STKObjectType.SATELLITE,
"GPS%02d" % prn)
sat.set_propagator_type(PropagatorType.GPS)
sat.propagator.prn = prn
sat.propagator.propagate()
made.append((prn, sat))
except Exception: # noqa: BLE001
# almanac 裡沒有這個 PRN 很正常(備援槽位)。跳過,不要中斷。
continue
self.prns = made
self.build_s = time.time() - t0
return self
def fetch(self):
"""把所有 PRN 的 ECEF 星曆整段取回來。**這是唯一一次大量往返。**"""
t0 = time.time()
cols = []
for _prn, sat in self.prns:
dp = sat.data_providers.item("Cartesian Position").group.item("Fixed")
res = dp.execute(self.scen.start_time, self.scen.stop_time, self.step)
xs = res.data_sets.get_data_set_by_name("x").get_values()
ys = res.data_sets.get_data_set_by_name("y").get_values()
zs = res.data_sets.get_data_set_by_name("z").get_values()
cols.append(np.column_stack([xs, ys, zs]))
n_t = min(c.shape[0] for c in cols)
self.eph = np.stack([c[:n_t] for c in cols], axis=1) # (T, N, 3)
self.times = np.arange(n_t) * self.step
self.fetch_s = time.time() - t0
return self
def at(self, t_sec):
"""線性內插取 t 時刻的所有衛星位置。**不碰網路。**"""
t = min(max(t_sec, self.times[0]), self.times[-1])
i = int(t // self.step)
i = min(i, len(self.times) - 2)
f = (t - self.times[i]) / self.step
return self.eph[i] * (1.0 - f) + self.eph[i + 1] * f
def orbit_radius_km(self):
"""從取回的星曆量軌道半徑 —— 拿來對公開的標稱值。"""
r = np.linalg.norm(self.eph.reshape(-1, 3), axis=1)
return float(np.mean(r)), float(np.std(r))
# ------------------------------------------------------------------ 閉迴路
def guidance_gps(gps_ok, bank_deg=CL.BANK_DEG):
"""定位品質不足就停止轉彎(直飛 = 航位推算)。
與地面站那一節的「反向轉彎」刻意不同:這裡的失效模式不是
「該回家了」,而是「載具不確定自己在哪」—— 停止機動、維持航向,
是這種情況下比較合理的反應。
"""
return bank_deg if gps_ok else 0.0
def run(const, n=N_STEPS, mask=MASK_DEG, hdop_max=HDOP_MAX):
"""跑閉迴路。回傳逐步的紀錄。"""
plant = CL.Plant(dt=DT)
rec = []
gps_ok, flips, prev = True, 0, True
for _i in range(n):
t, lat, lon, alt = plant.step(guidance_gps(gps_ok))
t0 = time.time()
sv = const.at(t)
nvis, dop, _elevs = visible_and_dop(lat, lon, alt, sv, mask)
cost = time.time() - t0
gps_ok = (dop is not None and nvis >= MIN_SVS
and dop["HDOP"] <= hdop_max)
if gps_ok != prev:
flips += 1
prev = gps_ok
rec.append({"t": t, "lat": lat, "lon": lon, "alt_km": alt,
"n_vis": nvis, "gps_ok": gps_ok, "cost_s": cost,
"dop": dop})
return rec, flips
def main():
mask = MASK_DEG
if "--mask" in sys.argv:
mask = float(sys.argv[sys.argv.index("--mask") + 1])
n = N_STEPS
if "--steps" in sys.argv:
n = int(sys.argv[sys.argv.index("--steps") + 1])
dop_only = "--dop-only" in sys.argv
print("=" * 78)
print(" 雙向實驗子項 — GPS 訊號可用度")
print("=" * 78)
print(" 遮蔽角 %.1f deg HDOP 門檻 %.1f 最少 %d 顆"
% (mask, HDOP_MAX, MIN_SVS))
try:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:110])
print(" 這一節需要 STK —— GPS 星座只有它算得出來。")
return 1
root = app.new_object_root()
root.new_scenario("GpsAvailability")
scen = root.current_scenario
ts = R.timestamps([0.0, max(n * DT, 3600.0)])
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"' % (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
const = GpsConstellation(root, scen).build().fetch()
print()
print(" 星座:%d 個 PRN(STK 內建 almanac),建構 %.2f s"
% (len(const.prns), const.build_s))
print(" 星曆預取:%d 個時間點 x %d 顆,%.2f s(**唯一一次大量往返**)"
% (len(const.times), len(const.prns), const.fetch_s))
r_mean, r_std = const.orbit_radius_km()
print(" 軌道半徑 %.1f +- %.1f km(公開標稱 %.0f km,差 %.2f%%)"
% (r_mean, r_std, GPS_SMA_KM, abs(r_mean - GPS_SMA_KM) / GPS_SMA_KM * 100))
if dop_only:
sv = const.at(0.0)
nvis, dop, elevs = visible_and_dop(R.LAT0, R.LON0, 1.5, sv, mask)
print()
print(" 起點的定位幾何:可見 %d / %d 顆" % (nvis, len(const.prns)))
if dop:
print(" " + " ".join("%s %.3f" % (k, dop[k])
for k in ("GDOP", "PDOP", "HDOP",
"VDOP", "TDOP")))
ok, (r1, r2) = dop_identities(dop)
print(" 恆等式 GDOP²=PDOP²+TDOP²、PDOP²=HDOP²+VDOP²:%s"
% ("成立(殘差 %.2e / %.2e)" % (r1, r2) if ok else "**不成立**"))
app.shutdown()
return 0
rec, flips = run(const, n, mask)
costs = [x["cost_s"] * 1000.0 for x in rec]
nvis = [x["n_vis"] for x in rec]
hd = [x["dop"]["HDOP"] for x in rec if x["dop"]]
gd = [x["dop"]["GDOP"] for x in rec if x["dop"]]
print()
print(" 跑了 %d 步" % len(rec))
print(" 可見顆數 %d - %d(遮蔽角 %.0f deg)" % (min(nvis), max(nvis), mask))
if gd:
print(" GDOP %.2f - %.2f HDOP %.2f - %.2f"
% (min(gd), max(gd), min(hd), max(hd)))
n_ok = sum(1 for x in rec if x["gps_ok"])
print(" GPS 可用狀態翻轉 %d 次;可用時間佔 %.1f%%(%d / %d 步)"
% (flips, 100.0 * n_ok / len(rec), n_ok, len(rec)))
if flips == 0:
print(" **一次都沒翻 —— 閉迴路沒有作用的機會。**")
print(" GPS 幾何以小時為尺度變化,任務太短就看不到;"
"或門檻設得全程通過/全程不過。")
print()
print(" **迴圈內每步只花 %.4f ms**(最差 %.4f)—— 因為它不碰網路。"
% (sum(costs) / len(costs), max(costs)))
print(" 對照:同樣的判斷若每步去問 STK,以本章量到的 access 成本")
print(" (每次約 13 ms)乘上 %d 顆,一步就要約 %.1f 秒。"
% (len(const.prns), len(const.prns) * 0.013))
print(" 預取一次花了 %.2f s,換掉的是 %d 步 x %d 顆的往返。"
% (const.fetch_s, len(rec), len(const.prns)))
bad = [x for x in rec if x["dop"] and not dop_identities(x["dop"])[0]]
print()
print(" DOP 恆等式:%d / %d 步成立"
% (len(rec) - len(bad), len(rec)))
io.open(GPS_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"mask_deg": mask, "hdop_max": HDOP_MAX,
"n_prn": len(const.prns), "build_s": const.build_s,
"fetch_s": const.fetch_s, "eph_points": len(const.times),
"orbit_radius_km": r_mean, "orbit_radius_std_km": r_std,
"steps": len(rec), "flips": flips,
"available_frac": n_ok / len(rec),
"n_vis_min": min(nvis), "n_vis_max": max(nvis),
"gdop_min": min(gd) if gd else None,
"gdop_max": max(gd) if gd else None,
"hdop_min": min(hd) if hd else None,
"hdop_max": max(hd) if hd else None,
"loop_cost_ms_avg": sum(costs) / len(costs),
"track": [{k: x[k] for k in
("t", "lat", "lon", "alt_km", "n_vis", "gps_ok")}
for x in rec]},
ensure_ascii=False, indent=1) + "\n")
print(" 結果已存到 %s" % os.path.basename(GPS_OUT))
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_integration.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — MAVLink 模擬器 + MATLAB/Simulink + PySTK 四方整合測試。
## 這一支與前面幾支的差別
前面每一支各驗一段:探測驗連線、量測驗成本、即時驗注入、橋接驗攢批。
**它們各自通過,不代表接起來是對的** —— 整合的錯誤恰好都落在接縫上:
單位換到一半、時間基準不一致、座標系在某一段被悄悄改掉。
所以這一支把三條來源餵進**同一個 STK 場景**,再把三條軌跡讀回來對帳:
```
同一份 plant(協調轉彎,已對過閉合式 R = V^2/(g tanφ))
|
+---------------------------+---------------------------+
| | |
MAVLink 編碼 Simulink 匯出 直接送(參考)
(pymavlink,真線上格式) (CSV / UDP 封包) (不經任何傳輸)
| | |
+---------------------------+---------------------------+
|
共用的 Bridge(攢批)
|
PySTK / gRPC
|
STK:三架載具,三條 Realtime 星曆
|
對帳
```
## 對帳的判準:每一條路各有**不同**的容許差,而且是事先算得出來的
這是整份測試的重點。用同一個容差套三條路,等於什麼都沒驗:
| 比對 | 預期差 | 為什麼 |
|---|---|---|
| 直接 vs Simulink(CSV) | <= 約 0.16 mm | CSV 用 `%.9f` 寫經緯度,文字格式本身就是量化 |
| 直接 vs Simulink(UDP) | **0** | 二進位 float64,真正無損 |
| 直接 vs MAVLink | **0 < 差 <= 約 1.5 cm** | `GLOBAL_POSITION_INT` 的 lat/lon 是 1e-7 度的整數 |
| 三條 vs 閉合式 | <= 1% | plant 的歐拉離散誤差 |
第二列的**兩邊**都要驗:
- 超過 1.5 cm -> 真的有 bug(欄位錯位、單位、座標系)
- **剛好是 0 -> 一樣可疑**:表示量化沒發生,那條路可能根本沒走 MAVLink 編碼
「差為零」在多數測試裡是好消息,在這裡是紅燈。這是整合測試與
「跑起來沒報錯」最根本的差別。
## Simulink 這一條走 CSV 的理由
撰寫環境的 MATLAB 在多次命令列啟動後停止回應(授權階段),
所以 Simulink 這一條用**它匯出的 CSV 格式**重放,而不是即時 UDP。
兩者在橋接下游完全相同(同一個 `Bridge`),差別只在資料怎麼進來。
有真的 Simulink 匯出檔就用它:`--simulink-csv <路徑>`。
欄位順序與 `grpc_link_plant.m` + `writematrix` 的輸出一致:
`t_sec, lat_deg, lon_deg, alt_km`。
## 為什麼最後要算一次 access
位置差 1.5 cm 聽起來小,但「小」要相對於**用途**才有意義。
所以最後對三架載具各算一次對地面站的可見窗:如果三條的可見窗
逐段相同,就證明 MAVLink 的量化**對這個應用不重要** ——
這是個結論,不是猜測。反過來,如果哪天換成需要公分級的應用,
同一個測試會立刻變紅。
python grpc_link_integration.py # 三條路 + 對帳
python grpc_link_integration.py --access # 再加可見窗比對
python grpc_link_integration.py --simulink-csv out.csv
python grpc_link_integration.py --dry-run # 不連 STK,只對帳來源
"""
import io
import json
import math
import os
import sys
import threading
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_ardupilot as AP # noqa: E402
import grpc_link_realtime as R # noqa: E402
import grpc_link_simulink as S # noqa: E402
INTEG_OUT = os.path.join(HERE, "grpc_link_integration.json")
# 三條路在 STK 裡各用一架載具。名字就是來源,讀回來才不會弄混。
LEGS = ["Direct", "Simulink", "MAVLink"]
GS_LAT, GS_LON, GS_ALT_KM = 24.60, 121.30, 0.0 # 對帳用的地面站
N_POINTS = 60
# Simulink 的 CSV 匯出用 `%.9f` 寫經緯度,所以**文字格式本身就是一個量化步驟**:
# 1e-9 度。把這條路當成「無損、差應為 0」是錯的,實測給了 67 微米 ——
# 不是 bug,是那九位小數。既然算得出來,就該把它算出來當判準,
# 而不是塞一個籠統的容差把它蓋掉。
#
# 走二進位 UDP(32-byte float64)那條路才是真的無損,差會是 0。
CSV_DECIMALS = 9
def csv_bound_m(lat_deg=R.LAT0):
"""CSV 文字格式(%.9f)造成的最大位置誤差(公尺)。與 MAVLink 那個界同樣是事先算的。"""
q = 10.0 ** (-CSV_DECIMALS)
dlat = q * AP.M_PER_DEG_LAT
dlon = q * AP.M_PER_DEG_LAT * math.cos(math.radians(lat_deg))
return math.hypot(dlat, dlon)
# STK 自己的星曆儲存與回讀精度留的餘量(公尺)。與上面的來源層界是兩回事。
STK_ROUNDTRIP_TOL_M = 0.002
def gather_direct(n=N_POINTS):
"""參考路徑:plant 直接出來,不經任何傳輸。"""
t, lat, lon, alt = R.plant(n=n)
return list(zip(t, lat, lon, alt))
def gather_simulink(n=N_POINTS, csv_path=None):
"""Simulink 路徑:走它匯出的 CSV 格式。
沒給檔案就現產一份 —— 格式與 `grpc_link_plant.m` 配 `writematrix`
的輸出一致,所以這條路的下游與拿到真檔案時完全相同。
"""
if csv_path is None:
csv_path = S.write_csv(n=n)
return list(S.source_csv(csv_path)), csv_path
def gather_mavlink(n=N_POINTS):
"""MAVLink 路徑:pymavlink 編出真正的線上封包,再解回來。
用高送出頻率(200 Hz)是因為這裡要的是**編解碼保真度**,不是節奏 ——
節奏在 `grpc_link_simulink.py` 與 `grpc_link_ardupilot.py` 量過了。
"""
th = threading.Thread(target=AP.fake_sitl,
kwargs={"n": n, "hz": 200.0}, daemon=True)
th.start()
return list(AP.source_mavlink(expect=n, first_timeout=20.0,
idle_timeout=2.0))
def worst_offset_m(a, b):
"""兩條軌跡逐點的最大水平距離(公尺)。長度不同就比到短的那條為止。"""
worst = 0.0
for (_t1, la1, lo1, _a1), (_t2, la2, lo2, _a2) in zip(a, b):
dlat = (la1 - la2) * AP.M_PER_DEG_LAT
dlon = (lo1 - lo2) * AP.M_PER_DEG_LAT * math.cos(math.radians(la1))
worst = max(worst, math.hypot(dlat, dlon))
return worst
def inject_leg(root, name, frames, chunk=20):
"""把一條軌跡注入成一架 STK 載具。回傳 (載具, route, 橋接統計)。"""
ac, route, pts = R.make_realtime_aircraft(root, name + "UAV")
bridge = S.Bridge(pts, chunk)
for f in frames:
bridge.feed(f)
bridge.flush()
route.propagate()
return ac, route, bridge.report()
def access_windows(root, ac, gs_path="*/Facility/IntegGS"):
"""算某架載具對地面站的可見窗。回傳 [(起, 迄), ...]。
**要在 route.propagate() 之後算。** 沒有重新傳播的話,access 看到的是
舊星曆 —— 而它不會報錯,只會給一組對不上的可見窗。
"""
fac = root.get_object_from_path(gs_path)
acc = ac.get_access_to_object(fac)
acc.compute_access()
iv = acc.computed_access_interval_times
try:
rows = iv.to_array(0, -1)
except Exception: # noqa: BLE001
return []
return [(str(r[0]), str(r[1])) for r in rows]
def main():
dry = "--dry-run" in sys.argv
want_access = "--access" in sys.argv
csv_path = None
if "--simulink-csv" in sys.argv:
csv_path = sys.argv[sys.argv.index("--simulink-csv") + 1]
print("=" * 78)
print(" 四方整合測試 — MAVLink 模擬器 + Simulink + PySTK + STK")
print("=" * 78)
# ---------- 1. 三條來源 ----------
direct = gather_direct()
sim, csv_used = gather_simulink(csv_path=csv_path)
mav = gather_mavlink()
print()
print(" 來源 直接 %d 點 / Simulink %d 點(%s)/ MAVLink %d 點"
% (len(direct), len(sim), os.path.basename(csv_used), len(mav)))
# ---------- 2. 來源層對帳(還沒進 STK)----------
d_sim = worst_offset_m(direct, sim)
d_mav = worst_offset_m(direct, mav)
bound = AP.quantization_bound_m()
print()
print(" 來源層對帳(每一條路各有不同的預期差):")
print(" %-22s %12s %14s %s" % ("比對", "實際最大差", "預期", "判定"))
cbound = csv_bound_m()
ok_sim = d_sim <= cbound * 1.05
print(" %-22s %10.6f m %12s %s"
% ("直接 vs Simulink", d_sim, "<= %.6f" % cbound,
"通過" if ok_sim else "**不通過**"))
print(" -> 這條路的差來自 CSV 的 %%.%df 文字格式(1e-%d 度),不是 bug。"
% (CSV_DECIMALS, CSV_DECIMALS))
print(" 走二進位 UDP 封包那條路才是真的無損,差會是 0。")
ok_mav = 0.0 < d_mav <= bound * 1.05
print(" %-22s %10.6f m %12s %s"
% ("直接 vs MAVLink", d_mav, "0 < d <= %.4f" % bound,
"通過" if ok_mav else "**不通過**"))
print(" -> MAVLink 那一條**必須**有差:差為 0 表示量化沒發生,")
print(" 也就是那條路可能根本沒走 MAVLink 編碼。")
result = {"n": len(direct), "direct_vs_simulink_m": d_sim,
"direct_vs_mavlink_m": d_mav, "quant_bound_m": bound,
"csv_bound_m": cbound,
"source_pass": bool(ok_sim and ok_mav)}
if dry:
io.open(INTEG_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(result, ensure_ascii=False, indent=1) + "\n")
print()
print(" (--dry-run:不連 STK。結果已存到 %s)"
% os.path.basename(INTEG_OUT))
return 0 if result["source_pass"] else 1
# ---------- 3. 三條都注入同一個 STK 場景 ----------
try:
from ansys.stk.core.stkruntime import STKRuntime
from ansys.stk.core.stkobjects import STKObjectType
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
except Exception as e: # noqa: BLE001
print()
print(" 接不上 STK:%s" % str(e)[:110])
print(" 先跑 grpc_link_probe.py;或用 --dry-run 只做來源層對帳。")
return 1
root = app.new_object_root()
root.new_scenario("Integration")
ts = R.timestamps([f[0] for f in direct])
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"' % (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
legs = {}
for name, frames in (("Direct", direct), ("Simulink", sim),
("MAVLink", mav)):
ac, route, rep = inject_leg(root, name, frames)
legs[name] = {"ac": ac, "route": route, "report": rep}
print(" 注入 %-9s %3d 點 -> %d 次 gRPC 往返,%.3f ms/點"
% (name, rep["points"], rep["grpc_calls"], rep["ms_per_point"]))
# ---------- 4. 從 STK 讀回來再對一次 ----------
scen = root.current_scenario
back = {}
for name in LEGS:
la, lo = R.readback(legs[name]["ac"], scen)
back[name] = list(zip([0.0] * len(la), list(la), list(lo),
[0.0] * len(la)))
r_ref = R.turn_radius()
print()
print(" STK 讀回之後(這一輪才真的走完整條鏈):")
print(" %-10s %6s %14s %12s" % ("來源", "列數", "擬圓半徑(m)", "對閉合式"))
radii = {}
for name in LEGS:
la = [b[1] for b in back[name]]
lo = [b[2] for b in back[name]]
rr = R.fitted_radius(la, lo)
radii[name] = rr
print(" %-10s %6d %14.1f %11.3f%%"
% (name, len(la), rr, abs(rr - r_ref) / r_ref * 100))
print(" %-10s %6s %14.1f %11s" % ("閉合式", "-", r_ref, "(基準)"))
b_sim = worst_offset_m(back["Direct"], back["Simulink"])
b_mav = worst_offset_m(back["Direct"], back["MAVLink"])
print()
print(" STK 內的逐點差:直接 vs Simulink %.6f m,直接 vs MAVLink %.6f m"
% (b_sim, b_mav))
result.update({"stk_direct_vs_simulink_m": b_sim,
"stk_direct_vs_mavlink_m": b_mav,
"radii_m": radii, "r_closed_m": r_ref,
"reports": {k: legs[k]["report"] for k in LEGS}})
# ---------- 5. 可見窗:那 1.5 cm 到底重不重要 ----------
if want_access:
fac = scen.children.new(STKObjectType.FACILITY, "IntegGS")
fac.position.assign_geodetic(GS_LAT, GS_LON, GS_ALT_KM)
wins = {}
for name in LEGS:
wins[name] = access_windows(root, legs[name]["ac"])
print()
print(" 對地面站的可見窗(檢驗那 %.1f cm 的量化在這個應用裡重不重要):"
% (bound * 100))
for name in LEGS:
print(" %-10s %d 段" % (name, len(wins[name])))
same = (wins["Direct"] == wins["Simulink"] == wins["MAVLink"])
print(" 三條的可見窗%s" % ("**逐段相同**" if same else "**不相同**"))
if same:
print(" -> 結論:MAVLink 的整數量化對這個應用不重要。")
print(" 這是量出來的結論,不是假設 —— 換成需要公分級的應用,")
print(" 同一個測試會立刻變紅。")
result["access_same"] = bool(same)
result["access_counts"] = {k: len(v) for k, v in wins.items()}
passed = (result["source_pass"]
and b_sim <= cbound + STK_ROUNDTRIP_TOL_M
and 0.0 < b_mav <= bound * 1.05
and all(abs(radii[n] - r_ref) / r_ref < 0.01 for n in LEGS))
result["pass"] = bool(passed)
io.open(INTEG_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(result, ensure_ascii=False, indent=1) + "\n")
print()
print(" 整合判定:%s" % ("通過" if passed else "**不通過**"))
print(" 結果已存到 %s" % os.path.basename(INTEG_OUT))
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0 if passed else 1
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_mavsim.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 用外部 MAVLink 模擬器驅動多架載具。
## 為什麼不是 ArduPilot SITL
原本的缺口清單把「真實 ArduPilot SITL」列為待補。改用一個**外部的
MAVLink 模擬器**之後,補到的東西與沒補到的東西都要講清楚:
**補到了**
- 訊息面從一種變成七種:HEARTBEAT、GLOBAL_POSITION_INT、GPS_RAW_INT、
ATTITUDE、VFR_HUD、SYS_STATUS、HOME_POSITION。
先前的合成來源只送位置,所以「解碼路徑」驗到的只有一個訊息;
現在包含心跳(飛行模式與解鎖狀態)、姿態、電量。
- **多機。** 每架一個 UDP 埠、各自的 sysid。這讓「多機的成本」
從外插變成可量 —— 那正是先前列為未驗的一項。
**沒補到**
- 仍然**不是真的自駕韌體**:沒有 EKF、沒有控制迴路調校、沒有失效保護行為。
所以「飛控行為」這一欄仍然是未驗,不可宣稱等同 SITL。
(這個界線是模擬器作者自己在檔頭寫下的,本檔照搬,不稀釋。)
## 模擬器不在版本庫裡
與地形檔同一個原則:本教材**不附**模擬器,路徑由環境變數
`GRPC_LINK_MAVSIM` 或 `--sim <路徑>` 指定,本檔只啟動它、讀它,
不複製也不修改。它有自己的來源與授權。
## 架構沒有改動 —— 這一節同時是個檢查
`grpc_link_ardupilot.py` 的 `source_mavlink()` 收 MAVLink、
`grpc_link_simulink.py` 的 `Bridge` 攢批送進 STK。這一支**兩個都直接沿用**,
只是把資料來源換掉。
那正是這一章一直在主張的事:**介面是資料,不是函式呼叫**。
如果當初把 STK 的呼叫混進 UDP 解析裡,這一節就得重寫一遍。
所以這不只是省工,它是那個主張的驗證。
## 連線方式:用推播,不用綁定
模擬器每架**綁定**一個埠(`udpin`),等地面站連過去。那適合 GCS,
不適合這裡:我們要同時收 N 架,而且不想先送東西去「敲醒」對方。
所以用它的推播模式:`--gcs 127.0.0.1:<base>`,第 i 架就主動送到
`base + i - 1`。本檔一架綁一個埠收。
set GRPC_LINK_MAVSIM=D:\path\to\mav_sim.py
python grpc_link_mavsim.py # 預設 4 架
python grpc_link_mavsim.py --n 8 # 8 架
python grpc_link_mavsim.py --scaling # 掃 1/2/4/8 架,量擴展成本
python grpc_link_mavsim.py --n 3 --no-stk # 只驗解碼與多機收訊
"""
import io
import json
import math
import os
import subprocess
import sys
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_ardupilot as AP # noqa: E402
import grpc_link_realtime as R # noqa: E402
import grpc_link_simulink as S # noqa: E402
MAVSIM_OUT = os.path.join(HERE, "grpc_link_mavsim.json")
GCS_BASE = 15550 # 本檔收訊的起始埠(模擬器推播到這裡)
SIM_BASE = 14570 # 模擬器自己綁的埠(我們不用,但要避開別的服務)
DEFAULT_N = 4
COLLECT_S = 12.0 # 每輪收多久
SIM_RATE = 25.0 # 模擬器的步進頻率
CHUNK = 10 # 攢批大小
# 模擬器宣告的巡航速度(m/s)。**這是它的設計參數,不是本檔算的** ——
# 拿它去對從位置序列算出來的速度,就是一個獨立對照。
SIM_CRUISE_MS = 18.0
def sim_path(argv=None):
"""模擬器路徑。--sim 優先,其次環境變數。沒有預設值。"""
argv = sys.argv if argv is None else argv
if "--sim" in argv:
return argv[argv.index("--sim") + 1]
return os.environ.get("GRPC_LINK_MAVSIM")
def start_sim(path, n, gcs_base=GCS_BASE, sim_base=SIM_BASE, rate=SIM_RATE,
lat=24.99, lon=121.30, patrol_alt=90.0, spread=180.0):
"""啟動模擬器(子行程)。回傳 Popen。
用推播模式 —— 理由見檔頭。`--rate` 給高一點,不然位置更新太稀疏,
從位置序列算出來的速度會被離散化淹掉。
"""
return subprocess.Popen(
[sys.executable, path,
"--n", str(n), "--port-base", str(sim_base),
"--gcs", "127.0.0.1:%d" % gcs_base,
"--lat", "%.6f" % lat, "--lon", "%.6f" % lon,
"--patrol-alt", "%.1f" % patrol_alt,
"--spread", "%.1f" % spread,
"--rate", "%.1f" % rate],
stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
text=True, encoding="utf-8", errors="replace")
class Fleet:
"""收 N 架的 MAVLink,並把位置注入 STK。
每架一條連線、一個 `Bridge`、一架 STK 載具。**不共用 Bridge** ——
攢批是「同一架的一串點壓成一次呼叫」,不同架的點混在一起壓不了,
因為 STK 的 `add_position_batch` 是掛在某一架的 point builder 上的。
這一點決定了多機的成本下界:**每架每幀至少一次往返。**
"""
def __init__(self, n, gcs_base=GCS_BASE, chunk=CHUNK):
from pymavlink import mavutil
self.n = n
self.conns = [mavutil.mavlink_connection("udpin:0.0.0.0:%d"
% (gcs_base + i))
for i in range(n)]
self.chunk = chunk
self.bridges = [None] * n
self.tracks = [[] for _ in range(n)]
self.msg_types = [set() for _ in range(n)]
self.hb = [0] * n
self.speeds = [[] for _ in range(n)] # VFR_HUD 報的地速
self.t0 = None
def attach_stk(self, root):
"""在 STK 裡建 N 架 Realtime 載具,各配一個 Bridge。"""
self.acs, self.routes = [], []
for i in range(self.n):
ac, route, pts = R.make_realtime_aircraft(root, "MavUAV%02d" % (i + 1))
self.acs.append(ac)
self.routes.append(route)
self.bridges[i] = S.Bridge(pts, self.chunk)
return self
def attach_dummy(self):
"""不連 STK 時也要能跑(只驗解碼與收訊)。"""
self.acs, self.routes = [], []
for i in range(self.n):
self.bridges[i] = S.Bridge(None, self.chunk)
return self
def collect(self, seconds=COLLECT_S):
"""收一段時間。回傳 (總點數, 注入總耗時秒)。"""
t_start = time.time()
inject_s = 0.0
pts_total = 0
while time.time() - t_start < seconds:
for i, c in enumerate(self.conns):
m = c.recv_match(blocking=False)
while m is not None:
typ = m.get_type()
self.msg_types[i].add(typ)
if typ == "HEARTBEAT":
self.hb[i] += 1
elif typ == "VFR_HUD":
self.speeds[i].append(float(m.groundspeed))
elif typ == "GLOBAL_POSITION_INT":
t = m.time_boot_ms / 1000.0
if self.t0 is None:
self.t0 = t
frame = (t - self.t0,
m.lat * AP.DEG_E7, m.lon * AP.DEG_E7,
m.alt * AP.MM / 1000.0)
self.tracks[i].append(frame)
t_i = time.time()
self.bridges[i].feed(frame)
inject_s += time.time() - t_i
pts_total += 1
m = c.recv_match(blocking=False)
time.sleep(0.005)
for b in self.bridges:
t_i = time.time()
b.flush()
inject_s += time.time() - t_i
return pts_total, inject_s
def calls(self):
return sum(b.report()["grpc_calls"] for b in self.bridges)
def close(self):
for c in self.conns:
try:
c.close()
except Exception: # noqa: BLE001
pass
def track_speed(track):
"""從位置序列算平均地速(m/s)。**與 VFR_HUD 無關,是獨立的一條路。**"""
if len(track) < 3:
return None
tot_d = tot_t = 0.0
for a, b in zip(track, track[1:]):
dt = b[0] - a[0]
if dt <= 0 or dt > 2.0:
continue
dlat = (b[1] - a[1]) * AP.M_PER_DEG_LAT
dlon = (b[2] - a[2]) * AP.M_PER_DEG_LAT * math.cos(math.radians(a[1]))
tot_d += math.hypot(dlat, dlon)
tot_t += dt
return tot_d / tot_t if tot_t > 0 else None
def run_once(path, n, use_stk, seconds=COLLECT_S):
"""跑一輪。回傳結果字典。"""
proc = start_sim(path, n)
time.sleep(4.0) # 等它把 N 架建起來並開始推播
app = root = None
fleet = Fleet(n)
try:
if use_stk:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
root = app.new_object_root()
root.new_scenario("MavSimFleet%d" % n)
ts = R.timestamps([0.0, seconds * 4.0])
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"'
% (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
fleet.attach_stk(root)
else:
fleet.attach_dummy()
pts, inject_s = fleet.collect(seconds)
finally:
fleet.close()
proc.terminate()
try:
proc.wait(timeout=5)
except Exception: # noqa: BLE001
proc.kill()
res = {
"n": n, "points": pts, "inject_s": inject_s,
"grpc_calls": fleet.calls(),
"ms_per_point": (inject_s / pts * 1000.0) if pts else 0.0,
"heartbeats": sum(fleet.hb),
"msg_types": sorted(set().union(*fleet.msg_types)) if pts else [],
"per_vehicle_points": [len(t) for t in fleet.tracks],
"track_speed": [track_speed(t) for t in fleet.tracks],
"vfr_speed": [(sum(s) / len(s) if s else None) for s in fleet.speeds],
}
if use_stk and app is not None:
try:
for r in fleet.routes:
r.propagate()
except Exception: # noqa: BLE001
pass
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return res
def main():
path = sim_path()
use_stk = "--no-stk" not in sys.argv
n = DEFAULT_N
if "--n" in sys.argv:
n = int(sys.argv[sys.argv.index("--n") + 1])
scaling = "--scaling" in sys.argv
print("=" * 78)
print(" 外部 MAVLink 模擬器驅動多架載具")
print("=" * 78)
if not path:
print(" 沒有指定模擬器。本教材**不附**它(有自己的來源與授權)。")
print(" set GRPC_LINK_MAVSIM=D:\\path\\to\\mav_sim.py")
print(" python grpc_link_mavsim.py --sim D:\\path\\to\\mav_sim.py")
return 1
if not os.path.isfile(path):
print(" 找不到模擬器:%s" % path)
return 1
print(" 模擬器 %s" % path)
print(" **協定層真實,飛控行為簡化** —— 沒有 EKF、沒有控制迴路調校、")
print(" 沒有失效保護。不可宣稱等同 ArduPilot SITL。")
if scaling:
rows = []
for k in (1, 2, 4, 8):
print()
print(" --- %d 架 ---" % k)
r = run_once(path, k, use_stk, seconds=8.0)
rows.append(r)
print(" 收 %d 點,gRPC 往返 %d 次,注入 %.3f ms/點"
% (r["points"], r["grpc_calls"], r["ms_per_point"]))
print()
print(" **多機的成本:從外插變成量出來的**")
print(" %4s %8s %10s %12s %14s"
% ("架數", "點數", "往返次數", "ms/點", "每架每秒往返"))
for r in rows:
per = r["grpc_calls"] / r["n"] / 8.0
print(" %4d %8d %10d %12.3f %14.2f"
% (r["n"], r["points"], r["grpc_calls"],
r["ms_per_point"], per))
# **往返次數與架數的關係才是重點,不是每點成本。**
# 只印「單點成本從 X 到 Y」並用 max() 取 Y 會誤導:max 會挑到
# 第一列(單機時固定開銷分攤得最少,每點最貴),印出「1.00 倍」,
# 把「成本其實下降了」講成「沒變」。
calls_per_veh = [r["grpc_calls"] / r["n"] for r in rows]
lin = max(calls_per_veh) / min(calls_per_veh)
ms = [r["ms_per_point"] for r in rows]
print()
print(" **往返次數與架數嚴格線性**:每架 %.1f - %.1f 次"
% (min(calls_per_veh), max(calls_per_veh)))
print(" (最大/最小 = %.2f,%d 架與 1 架的每架往返數一樣)"
% (lin, rows[-1]["n"]))
print(" 這證實了下界:**攢批壓得掉同一架的連續點,壓不掉跨載具的**,")
print(" 因為 add_position_batch 掛在某一架的 point builder 上。")
print()
print(" 而每點成本**隨架數下降**(%.3f -> %.3f ms):"
% (ms[0], ms[-1]))
print(" 固定開銷被更多點分攤掉了。所以多機的限制**不在每點成本**,")
print(" 在每幀要送幾次往返。")
print()
rtt_ms = 1.7 # 本章量到的 gRPC 單次往返(見 grpc_link_transport.py)
for hz in (10.0, 50.0):
budget = 1000.0 / hz
cap = int(budget / rtt_ms)
print(" %2.0f Hz(每幀 %.0f ms)、單次往返 %.1f ms -> 上限約 %d 架"
% (hz, budget, rtt_ms, cap))
print(" 這個上限是**結構決定的**,不是實作差 —— 除非 STK 提供")
print(" 跨載具的批次入口,否則再怎麼優化都繞不過每架一次往返。")
out = {"scaling": rows, "calls_per_vehicle": calls_per_veh}
else:
r = run_once(path, n, use_stk)
out = {"single": r}
print()
print(" 收到 %d 點 / %d 架,心跳 %d 則"
% (r["points"], n, r["heartbeats"]))
print(" 訊息種類(%d):%s" % (len(r["msg_types"]),
", ".join(r["msg_types"])))
print(" 每架收到的點數:%s" % r["per_vehicle_points"])
if use_stk:
print(" gRPC 往返 %d 次,注入 %.3f ms/點"
% (r["grpc_calls"], r["ms_per_point"]))
print()
print(" **獨立對照:位置序列算出的地速 vs VFR_HUD 報的地速**")
print(" 兩者來自**不同的 MAVLink 欄位**,對得上才表示解碼沒錯位。")
for i, (ts, vs) in enumerate(zip(r["track_speed"], r["vfr_speed"])):
if ts is None or vs is None:
print(" 載具 %d 資料不足" % (i + 1))
continue
print(" 載具 %d 位置序列 %6.2f m/s VFR_HUD %6.2f m/s 差 %+.2f"
% (i + 1, ts, vs, ts - vs))
io.open(MAVSIM_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(out, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(MAVSIM_OUT))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_plant.m
function [t, lat, lon, alt_km] = grpc_link_plant(n, dt, V, bank_deg, gamma_deg, lat0, lon0, alt0_m)
% GRPC_LINK_PLANT 3 自由度協調轉彎運動學 —— 與 grpc_link_realtime.plant() 逐字對應。
%
% 這一支存在的唯一理由是**對照**:Simulink 這一側與 Python 那一側必須
% 算出同一條軌跡,否則「把 Simulink 接進 STK」這件事無法驗證 ——
% 軌跡對不上的時候,你不知道是接錯了、還是兩邊的模型本來就不同。
%
% 所以這裡的每一個選擇都以「與 Python 那側逐字相同」為優先:
%
% * 積分器用**顯式歐拉**,不是 ode45。歐拉精度差,但兩側的離散誤差
% 一模一樣,所以兩條軌跡會逐點吻合到浮點誤差。換成 ode45 精度更高,
% 兩側卻對不上,對照就失去意義了。Simulink 模型也要設成
% FixedStepDiscrete / 固定步長,理由相同。
% * 平板地球近似:緯度增量 = 北向位移 / 地球半徑,經度再除以 cos(lat)。
% * 常數也要一致:g = 9.80665、Re = 6378137(WGS-84 赤道半徑)。
%
% 獨立對照(不是拿模型對自己):固定坡度角與空速下,地面軌跡是半徑
%
% R = V^2 / (g * tan(phi))
%
% 的圓。Python 那側用代數擬圓去對這個閉合式,實測相對差約 0.017%。
%
% 用法:
% [t,lat,lon,alt] = grpc_link_plant(); % 用預設設計點
% [t,lat,lon,alt] = grpc_link_plant(120, 1.0); % 120 步、1 秒
%
% 回傳 alt 的單位是 **km**,因為 STK 場景的距離單位設成 km。
if nargin < 1 || isempty(n), n = 120; end
if nargin < 2 || isempty(dt), dt = 1.0; end
if nargin < 3 || isempty(V), V = 120.0; end % m/s
if nargin < 4 || isempty(bank_deg), bank_deg = 15.0; end
if nargin < 5 || isempty(gamma_deg), gamma_deg = 2.0; end
if nargin < 6 || isempty(lat0), lat0 = 24.80; end
if nargin < 7 || isempty(lon0), lon0 = 121.00; end
if nargin < 8 || isempty(alt0_m), alt0_m = 1500.0; end
t = zeros(n, 1);
lat = zeros(n, 1);
lon = zeros(n, 1);
alt_km = zeros(n, 1);
% **方程只有一份,在 grpc_link_step.m 裡。**
% 這一支與閉迴路的 Simulink 模型都呼叫它,所以「兩邊一致」
% 是結構上保證的,不是每次都要重新量的巧合。
s = [0.0; lat0; lon0; alt0_m]; % [psi_rad; lat; lon; h_m]
for i = 1:n
t(i) = (i - 1) * dt;
lat(i) = s(2);
lon(i) = s(3);
alt_km(i) = s(4) / 1000.0;
s = grpc_link_step(s, bank_deg, dt, V, gamma_deg);
end
end
grpc_link/grpc_link_plot.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 四張圖。
## 每一張圖在回答什麼(先定工作,再挑形式)
(a) gRPC 比 COM 貴多少 兩類別 x 兩操作的量值 -> 分組橫條,對數軸
(b) 即時迴路塞得下幾架 連續關係 + 門檻 -> 折線 + 預算參考線
(c) 一次能讀回多少 連續關係 + 硬上限 -> 折線 + 上限參考線
(d) 拒絕與逾時怎麼分 兩序列 + 誤判區 -> 折線 + 陰影區
## 配色
四張圖比的都是**類別**(COM vs gRPC、逐點 vs 成批、被拒 vs 逾時),
沒有序數關係,所以一律用類別色、固定順序取:blue -> orange,不循環。
只有 (c) 的「過/不過」是狀態,用灰階加標記區分,不另外配色 ——
狀態色留給真正的良/警/危,不拿來當第三個數列。
## 不用雙 y 軸
(b) 想同時表達「每幀成本」與「架數」,(c) 想同時表達「列數」與「大小」。
兩個都刻意只留一個軸:(b) 的 x 軸就是架數,(c) 的 x 軸就是列數,
另一個量用**直接標籤**掛上去。雙軸讓讀者無從判斷兩條線的交叉有沒有意義。
## 數字從哪裡來
優先讀各模組跑完留下的 JSON(`grpc_link_transport.json` 等),
讀不到才退回本檔記錄的實測值 —— 而且會在圖上標明是哪一種。
**手抄的數字會過期**,這個教材庫在別的章節為此吃過兩次虧,
所以這裡寧可多一段讀檔的程式碼。
python grpc_link_plot.py
"""
import io
import json
import os
import sys
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt # noqa: E402
import numpy as np # noqa: E402
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
sys.path.insert(0, os.path.join(HERE, os.pardir, "tools"))
OUT = os.path.join(HERE, "grpc_link.png")
# 類別色:固定順序,不循環
C_BLUE, C_ORANGE = "#2a78d6", "#eb6834"
INK, MUTED, GRID = "#14181d", "#5a6876", "#dde4ec"
WARN = "#b4431c"
# 撰寫時的實測值。**只在讀不到 JSON 時才用**,而且圖上會標「內建實測值」。
FALLBACK = {
"transport": {"com_property_ms": 0.0469, "grpc_property_ms": 1.7005,
"com_command_ms": 16.3047, "grpc_command_ms": 14.0071},
# 逐點 / 成批的每點注入成本(ms)。成批是 60 點約 3 ms。
"inject": {"one_by_one_ms": 1.06, "batch_ms": 3.0 / 60.0},
# DataProvider 取回:(列數, 位元組, 預設上限下是否過)
"payload": [(60481, 60481 * 8, True), (302401, 302401 * 8, True),
(604801, 604801 * 8, False), (1209601, 1209601 * 8, False),
(3024001, 3024001 * 8, False), (6048002, 6048002 * 8, False)],
# 第 1 層的耗時特徵:(timeout 秒, 被拒 ms, 逾時 ms)
"tcp": [(1.0, 1011.1, 1006.0), (3.0, 2051.7, 3003.3),
(6.0, 2040.0, 6012.5), (10.0, 2040.2, 10000.0),
(20.0, 2039.6, 20000.0)],
}
GRPC_LIMIT_MB = 4 * 1024 * 1024 / 1e6 # gRPC 預設上限換成 MB
REFUSE_MS = 2040.0 # OS 回報「被拒」所需時間(實測)
def load(name):
"""讀某個模組留下的 JSON。讀不到回 None —— 不要猜,交給呼叫端決定。"""
p = os.path.join(HERE, "grpc_link_%s.json" % name)
if not os.path.isfile(p):
return None
try:
return json.load(io.open(p, encoding="utf-8"))
except (ValueError, OSError):
return None
def plain_log(ax, which="both"):
"""把對數軸的刻度標籤換成純十進位(0.05 / 1 / 100),不要 10^n。
兩個理由,而且第二個是硬的:
1. 這四張圖的讀者要的是「1.7 毫秒」,不是「10^0.23」。
2. matplotlib 的對數刻度預設走 **mathtext**,負指數用的是 U+2212
(真正的減號)。中文字型沒有這個字,於是 10^-1 會變成缺字方框 ——
`axes.unicode_minus = False` 管不到 mathtext,實測換 fontset 也沒用。
換成自己格式化的純文字就完全繞過這條路。
"""
from matplotlib.ticker import FuncFormatter
def fmt(v, _pos):
if v <= 0:
return ""
if v >= 1000:
return format(int(round(v)), ",")
if v >= 1:
return "%g" % v
return ("%.3f" % v).rstrip("0").rstrip(".")
for axis in ((ax.xaxis,) if which == "x" else
(ax.yaxis,) if which == "y" else (ax.xaxis, ax.yaxis)):
axis.set_major_formatter(FuncFormatter(fmt))
axis.set_minor_formatter(FuncFormatter(lambda v, p: ""))
def _frame(ax):
ax.set_facecolor("white")
for s in ("top", "right"):
ax.spines[s].set_visible(False)
for s in ("left", "bottom"):
ax.spines[s].set_color(GRID)
ax.tick_params(colors=MUTED, labelsize=8.5)
ax.grid(True, color=GRID, lw=0.7, zorder=0)
ax.set_axisbelow(True)
# ------------------------------------------------------------------ (a)
def panel_transport(ax):
"""gRPC 比 COM 貴多少 —— 而且貴在哪一種操作上。
對數軸是必要的:0.047 ms 與 16.3 ms 跨了近三個數量級,
線性軸會把讀屬性那一組壓成看不見的兩條線,而那正是重點所在。
"""
d = load("transport") or FALLBACK["transport"]
src = "實測 JSON" if load("transport") else "內建實測值"
ops = ["讀一個屬性", "送一條 Connect 指令"]
com = [d["com_property_ms"], d["com_command_ms"]]
grp = [d["grpc_property_ms"], d["grpc_command_ms"]]
y = np.arange(len(ops))
# **對數軸上不能用長條。** 長條的長度要從 0 起算才有意義,而對數軸沒有 0 ——
# 畫出來的長度取決於座標軸左緣設在哪裡,是個可以隨手調整的假比例。
# 這裡改用點狀圖:兩點標位置、中間一段連線表示差距,比例交給刻度去讀。
for yy, c, g in zip(y, com, grp):
ax.plot([c, g], [yy, yy], "-", color=GRID, lw=3, zorder=2,
solid_capstyle="round")
ax.plot(com, y, "o", color=C_BLUE, ms=11, zorder=4, label="行程內 COM")
ax.plot(grp, y, "o", color=C_ORANGE, ms=11, zorder=4, label="跨行程 gRPC")
for yy, val in zip(y, com):
ax.text(val, yy + 0.16, "%.3f" % val, ha="center", va="bottom",
fontsize=8.5, color=C_BLUE)
for yy, val in zip(y, grp):
ax.text(val, yy - 0.17, "%.2f" % val, ha="center", va="top",
fontsize=8.5, color=C_ORANGE)
# 比值直接標出來 —— 這張圖的結論就是這兩個數字
for i, (c, g) in enumerate(zip(com, grp)):
ax.text(0.98, i + 0.30, "gRPC / COM = %.1fx" % (g / c),
transform=ax.get_yaxis_transform(), ha="right", fontsize=9,
color=WARN if g / c > 2 else MUTED)
ax.set_xscale("log")
ax.set_yticks(y)
ax.set_yticklabels(ops, fontsize=9)
ax.set_xlabel("每次呼叫耗時(ms,對數軸)", fontsize=9, color=MUTED)
ax.set_xlim(0.02, 120)
ax.set_ylim(-0.55, 1.62)
ax.legend(frameon=False, fontsize=8.5, loc="upper center", ncol=2,
bbox_to_anchor=(0.5, -0.13))
ax.set_title("(a) 傳輸成本只在細粒度操作上顯現(%s)" % src,
fontsize=10.5, color=INK, loc="left")
_frame(ax)
plain_log(ax, "x")
# ------------------------------------------------------------------ (b)
def panel_budget(ax):
"""10 Hz 迴路每幀 100 ms,逐點與成批各塞得下幾架載具。
門檻(100 ms)是這張圖的主角,所以它是一條標了字的參考線,
不是圖例裡的第三個數列。
"""
inj = FALLBACK["inject"]
rt = load("realtime")
if rt and rt.get("ms_per_point"):
inj = {"one_by_one_ms": rt["ms_per_point"],
"batch_ms": rt["batch_s"] / rt["n"] * 1000.0}
src = "實測 JSON"
else:
src = "內建實測值"
n = np.arange(1, 201)
ax.plot(n, n * inj["one_by_one_ms"], color=C_ORANGE, lw=1.9, zorder=3,
label="逐點 add_position")
# 成批是「一幀一次往返」:成本與架數無關,所以它就是一條水平線。
# 這不是近似,是這個做法的定義。
ax.axhline(inj["batch_ms"], color=C_BLUE, lw=1.9, zorder=3,
label="成批 add_position_batch(1 次往返)")
ax.axhline(100.0, color=WARN, lw=1.4, ls="--", zorder=2)
ax.text(198, 108, "10 Hz 的每幀預算 100 ms", ha="right", fontsize=8.5,
color=WARN)
cap = int(100.0 / inj["one_by_one_ms"])
ax.plot([cap], [100.0], "o", color=C_ORANGE, ms=8, zorder=4)
ax.annotate("逐點在約 %d 架就吃完預算" % cap, xy=(cap, 100.0),
xytext=(cap + 12, 260), fontsize=8.5, color=C_ORANGE,
arrowprops=dict(arrowstyle="-", color=C_ORANGE, lw=0.9))
ax.set_yscale("log")
ax.set_xlabel("同時注入的載具數", fontsize=9, color=MUTED)
ax.set_ylabel("每幀注入成本(ms,對數軸)", fontsize=9, color=MUTED)
ax.set_xlim(1, 200)
ax.set_ylim(0.01, 1000)
ax.legend(frameon=False, fontsize=8.5, loc="lower right")
ax.set_title("(b) 成批不是微調,是能不能做即時的分界(%s)" % src,
fontsize=10.5, color=INK, loc="left")
_frame(ax)
plain_log(ax, "y")
# ------------------------------------------------------------------ (c)
def panel_payload(ax):
"""一次 DataProvider 取回多少列會撞上 gRPC 的訊息上限。
上限是硬的,所以它是參考線;過與不過用標記形狀區分,不另外配色。
"""
rows = FALLBACK["payload"]
b = load("bench")
if b and b.get("payload"):
got = [(r["rows"], r["bytes"], r["err"] == "")
for r in b["payload"] if r.get("rows")]
if len(got) >= 2:
rows = got
src = "實測 JSON" if (b and b.get("payload")) else "內建實測值"
xs = [r[0] for r in rows]
mb = [r[1] / 1e6 for r in rows]
ok = [r[2] for r in rows]
ax.plot(xs, mb, color=C_BLUE, lw=1.7, zorder=3)
for x, y, good in zip(xs, mb, ok):
ax.plot([x], [y], "o" if good else "X", ms=9,
color=C_BLUE if good else WARN, zorder=4)
ax.axhline(GRPC_LIMIT_MB, color=WARN, lw=1.4, ls="--", zorder=2)
ax.text(max(xs), GRPC_LIMIT_MB * 1.12, "gRPC 預設上限 4 MiB",
ha="right", fontsize=8.5, color=WARN)
# 只標關鍵的兩點:最後一個過的、第一個不過的
last_ok = max([x for x, g in zip(xs, ok) if g], default=None)
first_bad = min([x for x, g in zip(xs, ok) if not g], default=None)
for x, lab, col in ((last_ok, "最後一個過的", C_BLUE),
(first_bad, "預設上限下開始失敗", WARN)):
if x is None:
continue
y = mb[xs.index(x)]
ax.annotate("%s\n%s 列 / %.1f MB" % (lab, format(x, ","), y),
xy=(x, y), xytext=(x * 0.62, y * (2.6 if col == WARN else 0.30)),
fontsize=8, color=col,
arrowprops=dict(arrowstyle="-", color=col, lw=0.9))
ax.set_xscale("log")
ax.set_yscale("log")
ax.set_xlabel("一次取回的列數(對數軸)", fontsize=9, color=MUTED)
ax.set_ylabel("酬載大小(MB,對數軸)", fontsize=9, color=MUTED)
ax.set_title("(c) 分段取,不要把上限開大(%s)" % src,
fontsize=10.5, color=INK, loc="left")
_frame(ax)
plain_log(ax)
# ------------------------------------------------------------------ (d)
def panel_tcp(ax):
"""第 1 層:被拒與逾時的耗時特徵,以及會誤判的那一段。
這張圖是本章一個 bug 的來源:原本假設「被拒是瞬間回來」,
所以用耗時當判準。實測顯示被拒要約 2040 ms 且與 timeout 無關,
於是 timeout 低於它的時候,關閉的埠會被誤診成防火牆丟包。
陰影區就是那個誤判區。
"""
data = FALLBACK["tcp"]
tos = [d[0] for d in data]
ref = [d[1] for d in data]
fil = [d[2] for d in data]
# 誤判區:timeout 小於 OS 回報被拒所需的時間
ax.axvspan(0.8, REFUSE_MS / 1000.0, color="#fdeeea", zorder=1)
ax.text(1.55, 15000, "誤判區\ntimeout < %.1f s 時\n關閉的埠會被當成\n防火牆丟包"
% (REFUSE_MS / 1000.0), fontsize=8, color=WARN, ha="center",
va="top")
ax.plot(tos, ref, "o-", color=C_ORANGE, lw=1.9, ms=7, zorder=4,
label="關閉的埠(被拒)")
ax.plot(tos, fil, "s-", color=C_BLUE, lw=1.9, ms=6, zorder=3,
label="不可路由位址(逾時)")
ax.axhline(REFUSE_MS, color=C_ORANGE, lw=1.1, ls=":", zorder=2)
ax.text(20, REFUSE_MS * 1.14, "OS 回報『被拒』約 %.0f ms,與 timeout 無關"
% REFUSE_MS, ha="right", fontsize=8.5, color=C_ORANGE)
ax.set_xscale("log")
ax.set_yscale("log")
ax.set_xticks(tos)
ax.set_xticklabels(["%g" % t for t in tos])
ax.set_xlabel("設定的 timeout(秒,對數軸)", fontsize=9, color=MUTED)
ax.set_ylabel("實際回來的時間(ms,對數軸)", fontsize=9, color=MUTED)
ax.set_xlim(0.8, 26)
ax.legend(frameon=False, fontsize=8.5, loc="upper left")
ax.set_title("(d) 判準是例外型別,不是耗時(內建實測值)",
fontsize=10.5, color=INK, loc="left")
_frame(ax)
plain_log(ax, "y")
def main():
try:
import cjk # noqa: F401
cjk.use_cjk()
except Exception: # noqa: BLE001
# 找不到中文字型就照樣畫 —— 圖仍然有用,只是標籤會缺字
pass
# cjk.use_cjk() 已經關掉 axes.unicode_minus,但**對數軸的指數標籤走 mathtext**,
# 那條路仍然會用 U+2212。補一個西文字型當後備,缺字就從它取。
matplotlib.rcParams["mathtext.fontset"] = "dejavusans"
fams = list(matplotlib.rcParams.get("font.sans-serif") or [])
if "DejaVu Sans" not in fams:
matplotlib.rcParams["font.sans-serif"] = fams + ["DejaVu Sans"]
fig, ax = plt.subplots(2, 2, figsize=(13.8, 10.2))
panel_transport(ax[0][0])
panel_budget(ax[0][1])
panel_payload(ax[1][0])
panel_tcp(ax[1][1])
fig.suptitle("STK 與 gRPC 通聯測試 — 四個量出來的結論",
fontsize=13, color=INK, y=0.985)
fig.tight_layout(rect=(0, 0, 1, 0.965))
fig.savefig(OUT, dpi=130)
print("圖已存到 %s" % os.path.basename(OUT))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_probe.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 分層探測。
## 這一支要解決什麼
「連不上 STK」是這門課最常收到的一句回報,而它**至少有七個互不相同的成因**,
從 Python 這一側看起來卻幾乎一樣 —— 一個例外、一句英文。學生(和助教)
於是只能亂試:重開 STK、重裝 grpcio、關防火牆、換埠,碰對了就過,
沒碰對就卡住,而且下一次再遇到還是一樣亂試。
這一支把「通不通」拆成**六層**,每一層只檢查一件事,而且每一層的失敗
**恰好對應一個成因**。所以它的輸出不是「連不上」,而是
「第 2 層過、第 3 層不過 → 那個埠有人在聽,但聽的不是 gRPC 伺服器」。
第 0 層 Python 依賴 grpcio / protobuf / agi.stk12 在不在
第 1 層 TCP 可達 socket 連得上那個 host:port 嗎
第 2 層 gRPC 通道就緒 HTTP/2 握手成功嗎
第 3 層 STK 交握 拿得到 application 物件嗎
第 4 層 物件模型往返 建場景、讀回名字
第 5 層 Connect 面 這個 host 支援哪些 Connect 指令
## 關鍵:第 1 層要分「拒絕」與「逾時」
這是整支程式最有用的一行判斷,而它常被寫成同一個 `except OSError`:
- **連線被拒絕**(WSAECONNREFUSED / ECONNREFUSED,收到 RST):
封包到得了那台機器,但那個埠沒人在聽 —— **伺服器沒開**。
- **連線逾時**(等到設定的秒數才放棄,**沒有** RST):
封包**沒有**被回應。中間有東西把它丟掉了 —— 防火牆、安全群組,
或那個 host 根本不存在。
兩者都是 `OSError`,但成因與處置完全相反:前者去開伺服器,
後者去開防火牆。**把它們合併成一句「連不上」,就是把最有價值的
一位資訊丟掉。**
### 而「被拒絕是瞬間回來的」這個直覺是錯的
「回得快就是被拒、等滿就是逾時」這個判準禁不起實測。
在本章的撰寫環境(Windows 11,回文迴路)上量到:
timeout 1.0 s -> 1011 ms **誤判成 filtered**
timeout 3.0 s -> 2052 ms refused
timeout 10.0 s -> 2040 ms refused
timeout 20.0 s -> 2039 ms refused
**回報「連線被拒」要花約 2040 ms,而且與 timeout 無關** —— 那是作業系統
重送 SYN 的時間,不是網路延遲。對照組(不可路由位址)則一律剛好等滿
timeout:1.0 s 給 1006 ms、3.0 s 給 3003 ms、6.0 s 給 6013 ms。
所以正確的判準**不是耗時,是例外型別**:`ConnectionRefusedError` 對
`socket.timeout`。耗時只是輔助證據,而且它有一個致命的陷阱:
> **timeout 只要低於作業系統回報「被拒」所需的時間,
> 「伺服器沒開」就會被誤診成「防火牆丟包」。**
這是真的會發生的誤診,而且它把使用者推向完全錯誤的處置(去查防火牆,
而其實只要把伺服器開起來)。所以本檔的預設 timeout 訂在 `DEFAULT_TCP_TIMEOUT`
= 10 秒 —— 比量到的 2040 ms 有將近五倍餘裕,遠端主機也還撐得住。
而當 timeout 被調到餘裕不足、又真的逾時了,本檔**不會**硬說是 filtered,
而是回報 `timeout_too_short`(見 CAUSES)。寧可說「測不出來」,
也不要給一個聽起來很確定的錯答案。
## 七個成因,以及分別是哪一層會攔下來
| 成因 | 攔在哪一層 | 特徵 |
|---|---|---|
| grpcio / protobuf 沒裝 | 0 | STKInitializationError 明講缺哪個模組 |
| gRPC 伺服器沒開 | 1 | TCP **拒絕**,瞬間 |
| 防火牆丟包 / host 不存在 | 1 | TCP **逾時**,等滿 |
| 埠有人聽但不是 gRPC | 2 | TCP 通、通道**不就緒** |
| 伺服器只綁 127.0.0.1 | 1(從遠端跑才看得到) | 本機通、遠端拒絕 |
| 授權或模組不足 | 3 / 4 | 交握過了,物件模型呼叫才炸 |
| host 不支援該 Connect 指令 | 5 | 前五層全綠,指令仍 NACK |
倒數第二列與最後一列是**這一支最想教的東西**:
**通聯成功不等於功能齊全。** 前四層全綠、腳本照樣跑不動,
是完全正常的狀況 —— 第 5 層就是為它存在的。
## 沒有 STK 也要跑得動
第 0-2 層完全不碰 STK,只用 `socket` 與 `grpc`。所以這支程式在
沒裝 STK、或授權不在的機器上依然能回答「網路這一側通不通」,
而這正好是遠端佈署最常卡住的地方。第 3 層之後拿不到 STK 就標成
**略過**,不是失敗 —— 把「沒測」講成「測過了沒問題」是這類工具最糟的謊。
python grpc_link_probe.py # 探測 localhost:40704
python grpc_link_probe.py 192.168.1.20 40704 # 探測遠端
python grpc_link_probe.py --no-stk # 只做第 0-2 層
離開碼 0 = 到得了第 4 層,1 = 中途斷掉
"""
import io
import json
import os
import socket
import sys
import time
# notebook 的 kernel 沒有 __file__,而且啟動 STK 會改變 process 的工作目錄,
# 所以不能事後才靠 os.getcwd() —— notebook 開頭的 bootstrap cell 會先把
# 章節目錄釘在 WEEK_DIR。庫裡每一支都用同一段。
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
# STK 的 gRPC 伺服器預設埠。這個數字不是猜的 —— agi.stk12.stkdesktop 與
# agi.stk12.stkruntime 兩邊的 grpc_port 預設值都是它。
DEFAULT_PORT = 40704
PROBE_OUT = os.path.join(HERE, "grpc_link_probe.json")
# 作業系統回報「連線被拒」所需的時間(秒)。**量出來的,不是猜的**:
# Windows 11 回文迴路上約 2.04 s,而且與 timeout 設定無關(SYN 重送)。
# Linux 的回文迴路通常是毫秒等級,所以這個值是各平台裡偏保守的一端。
REFUSE_REPORT_S = 2.05
# TCP 連線的預設等待時間。必須**明顯大於** REFUSE_REPORT_S,
# 否則關閉的埠會逾時、被誤判成「防火牆丟包」(見檔頭)。取約 5 倍餘裕。
DEFAULT_TCP_TIMEOUT = 10.0
# 餘裕不足到這個倍數以下就不敢下「filtered」的結論
REFUSE_MARGIN = 1.5
# 每一層的名字,順序就是探測順序
LAYERS = ["依賴", "TCP", "通道", "交握", "物件模型", "Connect 面"]
# 成因說明。**這張表是這支程式的產品** —— 程式碼只是把觀察對進表裡。
CAUSES = {
"deps_missing":
"grpcio 或 protobuf 沒裝。STK 的 Python API 把 gRPC 當選用相依,"
"少了它 gRPC 這條路整條不存在,但本機 COM 那條還能走。"
"處置:pip install grpcio protobuf。",
"refused":
"TCP 被**拒絕**(瞬間回 RST):封包到得了那台機器,但那個埠沒人在聽。"
"處置:去開 gRPC 伺服器 —— STK Desktop 要帶 /grpcServer On,"
"或改用 STKRuntime。順便確認埠號兩邊一致。",
"filtered":
"TCP **逾時**(等滿都沒有回應):封包被丟掉了,不是被拒絕。"
"處置:防火牆 / 安全群組 / 路由。也可能是 host 名稱打錯或那台機器沒開。",
"timeout_too_short":
"TCP 逾時了,但等待時間太短,**分不出是被拒還是被丟包**。"
"作業系統回報「連線被拒」本身就要約 2 秒(SYN 重送),"
"等待時間低於它的時候,關閉的埠也會表現成逾時。"
"處置:把 timeout 調大(預設 10 秒)再測一次 —— 不要據此去查防火牆。",
"dns":
"host 名稱解不出 IP。處置:改用 IP,或修 DNS / hosts。",
"not_grpc":
"TCP 通了但 gRPC 通道**不就緒**:那個埠有人在聽,聽的不是 gRPC 伺服器。"
"處置:確認埠號沒撞到別的服務(這是把 STK 換到非預設埠時最常見的錯)。",
"handshake":
"gRPC 通了但 STK 不肯交握。處置:看伺服器那一側的主控台輸出 —— "
"成因通常在 STK 自己(啟動未完成、模組或授權不足),不在網路。",
"om_failed":
"交握過了,物件模型呼叫失敗。**網路沒問題,STK 那一側有問題。**"
"處置:同上,看伺服器主控台。",
"no_stk_api":
"agi.stk12 沒裝(或不在這個 Python 環境裡)。第 3 層之後略過。",
}
def _record(layer, ok, detail="", cause=None, ms=None):
return {"layer": layer, "ok": ok, "detail": detail,
"cause": cause, "ms": ms, "skipped": ok is None}
# ---------------------------------------------------------------- 第 0 層
def layer_deps():
"""grpcio / protobuf / agi.stk12 在不在。不碰網路,不碰 STK。"""
have = {}
for mod in ("grpc", "google.protobuf", "agi.stk12"):
try:
__import__(mod)
have[mod] = True
except ImportError:
have[mod] = False
detail = " ".join(("%s%s" % ("+" if v else "-", k)) for k, v in have.items())
if not (have["grpc"] and have["google.protobuf"]):
return _record("依賴", False, detail, "deps_missing")
if not have["agi.stk12"]:
# gRPC 有、STK API 沒有:網路層測得動,第 3 層之後測不動。
return _record("依賴", True, detail + "(STK API 缺,後段會略過)")
return _record("依賴", True, detail)
# ---------------------------------------------------------------- 第 1 層
def layer_tcp(host, port, timeout=None):
"""TCP 連得上嗎 —— 而且**拒絕**與**逾時**要分開。
判準是**例外型別**,不是耗時:`ConnectionRefusedError` 對
`socket.timeout`。耗時只記錄下來當輔助證據 —— 理由見檔頭那張量測表,
「被拒絕是瞬間回來的」這個直覺在 Windows 上是錯的。
這裡故意不用 `socket.create_connection` 的預設(永久等待):
等多久是本層的量測對象,不能讓它無限等。
"""
if timeout is None:
timeout = DEFAULT_TCP_TIMEOUT
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.settimeout(timeout)
t0 = time.time()
try:
s.connect((host, port))
ms = (time.time() - t0) * 1000.0
return _record("TCP", True, "%s:%d 連上" % (host, port), None, ms)
except socket.gaierror as e:
ms = (time.time() - t0) * 1000.0
return _record("TCP", False, "解不出 %s(%s)" % (host, e.args[-1]),
"dns", ms)
except (socket.timeout, TimeoutError):
ms = (time.time() - t0) * 1000.0
return _record("TCP", False, *_timeout_cause(timeout, ms), ms)
except ConnectionRefusedError:
ms = (time.time() - t0) * 1000.0
# 收到 RST —— 那台機器在,那個埠沒人聽。
# **不要**因為它花了兩秒就改判成逾時:那兩秒是 OS 在重送 SYN。
return _record("TCP", False, "被拒絕(%.0f ms,含 OS 的 SYN 重送)" % ms,
"refused", ms)
except OSError as e:
ms = (time.time() - t0) * 1000.0
if getattr(e, "winerror", None) == 10060: # WSAETIMEDOUT
return _record("TCP", False, *_timeout_cause(timeout, ms), ms)
if getattr(e, "winerror", None) == 10061: # WSAECONNREFUSED
return _record("TCP", False, "被拒絕(%.0f ms)" % ms, "refused", ms)
# 其他 OSError 不猜 —— 原樣回報,別編一個成因出來
return _record("TCP", False, "%s" % e.args[-1], "filtered", ms)
finally:
s.close()
def _timeout_cause(timeout, ms):
"""逾時了,但這個 timeout 下不下得了「被丟包」的結論?
回傳 (說明, 成因)。timeout 沒有明顯大於 OS 回報「被拒」所需的時間時,
關閉的埠也會表現成逾時 —— 這時候**寧可說測不出來**。
"""
if timeout < REFUSE_REPORT_S * REFUSE_MARGIN:
return ("逾時 %.0f ms,但 timeout %.1fs 不足以與『被拒』區分"
% (ms, timeout)), "timeout_too_short"
return "逾時,等了 %.0f ms 沒有回應" % ms, "filtered"
# ---------------------------------------------------------------- 第 2 層
def layer_channel(host, port, timeout=5.0):
"""gRPC 通道就緒嗎。
`grpc.channel_ready_future(...).result(timeout)` 就是 STK 自己
在 `GrpcClient.new_client` 裡用的那一招 —— 本層刻意與它一致,
所以本層過了,STK 那一步就不會因為通道而失敗。
**通道用的是 `insecure_channel`:沒有 TLS、沒有驗證。**
這不是本檔的選擇,是 STK Python API 的作法。含意寫在導論裡,
一句話:能連到這個埠的人,就能完全操控那個 STK。
"""
try:
import grpc
except ImportError:
return _record("通道", None, "grpc 沒裝")
t0 = time.time()
ch = grpc.insecure_channel("%s:%d" % (host, port))
try:
grpc.channel_ready_future(ch).result(timeout=timeout)
ms = (time.time() - t0) * 1000.0
return _record("通道", True, "HTTP/2 就緒", None, ms)
except grpc.FutureTimeoutError:
ms = (time.time() - t0) * 1000.0
return _record("通道", False, "通道逾時(%.0f ms)" % ms, "not_grpc", ms)
finally:
ch.close()
# ---------------------------------------------------------------- 第 3 層
def layer_handshake(host, port, timeout=20.0):
"""STK 交握。回傳 (紀錄, app) —— app 給後面兩層用,失敗就是 None。
這裡用 `STKRuntime.AttachToApplication`:它**只**會接上已經在跑的
伺服器,不會順手幫你啟一個。探測工具絕對不能自己啟伺服器 ——
那會讓「連不上」變成「連上了」,把要診斷的問題蓋掉。
"""
try:
from agi.stk12.stkruntime import STKRuntime
except ImportError:
return _record("交握", None, "agi.stk12 沒裝", "no_stk_api"), None
t0 = time.time()
try:
app = STKRuntime.AttachToApplication(grpc_host=host, grpc_port=port,
grpc_timeout_sec=int(timeout))
except Exception as e: # noqa: BLE001
ms = (time.time() - t0) * 1000.0
return _record("交握", False, str(e)[:110], "handshake", ms), None
ms = (time.time() - t0) * 1000.0
if app is None:
return _record("交握", False, "AttachToApplication 回 None",
"handshake", ms), None
return _record("交握", True, type(app).__name__, None, ms), app
# ---------------------------------------------------------------- 第 4 層
def layer_object_model(app):
"""建一個場景再讀回名字 —— 一次 set 加一次 get,雙向都走過。
只讀不寫是不夠的:讀得到不代表寫得進去。而只寫不讀更糟 ——
設定成功與否根本沒有人確認(這個教材庫的驗證段就是為這件事存在的)。
"""
if app is None:
return _record("物件模型", None, "沒有 app")
t0 = time.time()
name = "GrpcLinkProbe"
try:
root = app.NewObjectRoot()
root.NewScenario(name)
got = root.CurrentScenario.InstanceName
except Exception as e: # noqa: BLE001
ms = (time.time() - t0) * 1000.0
return _record("物件模型", False, str(e)[:110], "om_failed", ms)
ms = (time.time() - t0) * 1000.0
if got != name:
return _record("物件模型", False, "寫入 %s 讀回 %s" % (name, got),
"om_failed", ms)
return _record("物件模型", True, "建場景並讀回 %s" % got, None, ms)
# ---------------------------------------------------------------- 第 5 層
# 這批指令在 STK Desktop 上全部 ACK。拿它們去問一個 host
# 「你支援到哪裡」—— 差異本身就是答案。
SURFACE = [
("建物件", "New / */Satellite ProbeSat"),
("設單位", "SetUnits / km"),
("設分析期間",
'SetAnalysisTimePeriod * "1 Jan 2026 00:00:00.000" "2 Jan 2026 00:00:00.000"'),
("設軌道", "SetState */Satellite/ProbeSat Classical TwoBody "
'UseScenarioInterval 60 ICRF "1 Jan 2026 00:00:00.000" '
"7000 0 28.5 0 0 0"),
("傳播", "Propagate */Satellite/ProbeSat"),
("查單位", "Units_Get * Distance"),
("建報告", 'ReportCreate */Satellite/ProbeSat Type Export Style '
'"Classical Orbit Elements" File "%s"'
% os.path.join(HERE, "grpc_link_surface_report.txt")),
]
def layer_surface(app):
"""這個 host 支援哪些 Connect 指令。**這一層不判定通聯成敗。**
實測(STKRuntime 12,無介面):設定類指令全部 ACK,
`Units_Get` 與 `ReportCreate` 兩條 NACK。前五層全綠、指令照樣不動 ——
那不是通聯壞了,是**這個 host 的功能面比 Desktop 小**。
所以本層回報的是一張能力清單,而不是一個是非題。把它算進通聯成敗,
會讓「Runtime 本來就沒有這條指令」被誤診成「網路有問題」。
"""
if app is None:
return _record("Connect 面", None, "沒有 app"), []
try:
root = app.NewObjectRoot()
root.NewScenario("GrpcLinkSurface")
except Exception as e: # noqa: BLE001
return _record("Connect 面", None, str(e)[:80]), []
rows = []
for label, cmd in SURFACE:
try:
root.ExecuteCommand(cmd)
rows.append((label, cmd, True, ""))
except Exception as e: # noqa: BLE001
rows.append((label, cmd, False, str(e)[:60]))
ok = sum(1 for _l, _c, good, _m in rows if good)
return _record("Connect 面", None,
"%d/%d 條 ACK(能力清單,不計成敗)" % (ok, len(rows))), rows
# ---------------------------------------------------------------- 彙總
def probe(host="localhost", port=DEFAULT_PORT, use_stk=True,
tcp_timeout=None, channel_timeout=5.0):
"""跑整條梯子。**任何一層失敗就停** —— 後面的層失敗只是它的下游,
再測下去只會多幾行紅字、沒有多一位資訊。回傳 (紀錄清單, 指令清單)。"""
out = []
r = layer_deps()
out.append(r)
if not r["ok"]:
return out, []
r = layer_tcp(host, port, tcp_timeout)
out.append(r)
if not r["ok"]:
return out, []
r = layer_channel(host, port, channel_timeout)
out.append(r)
if not r["ok"]:
return out, []
if not use_stk:
return out, []
r, app = layer_handshake(host, port)
out.append(r)
if r["ok"] is not True:
return out, []
r = layer_object_model(app)
out.append(r)
if not r["ok"]:
return out, []
r, rows = layer_surface(app)
out.append(r)
try:
app.ShutDown()
except Exception: # noqa: BLE001
pass
return out, rows
def verdict(records):
"""整體結論:到得了第 4 層(物件模型)才算通。
門檻刻意訂在第 4 層而不是第 5 層 —— 第 5 層是能力清單,
不是是非題;也不訂在第 3 層,因為交握成功但物件模型不動的
情況是真的會發生的(見 CAUSES["om_failed"])。
"""
for r in records:
if r["ok"] is False:
return False, r
got = {r["layer"] for r in records if r["ok"] is True}
return ("物件模型" in got), None
def main():
args = [a for a in sys.argv[1:] if not a.startswith("--")]
use_stk = "--no-stk" not in sys.argv
host = args[0] if args else "localhost"
port = int(args[1]) if len(args) > 1 else DEFAULT_PORT
print("=" * 76)
print(" STK gRPC 通聯測試 — %s:%d%s"
% (host, port, "" if use_stk else "(只做第 0-2 層)"))
print("=" * 76)
records, rows = probe(host, port, use_stk)
for i, r in enumerate(records):
mark = "略過" if r["ok"] is None else ("通" if r["ok"] else "**不通**")
ms = "" if r["ms"] is None else "%8.1f ms" % r["ms"]
print(" 第 %d 層 %-10s %-8s %10s %s"
% (i, r["layer"], mark, ms, r["detail"]))
ok, bad = verdict(records)
print()
if bad is not None:
print(" 斷在第 %d 層(%s)。成因:" % (records.index(bad), bad["layer"]))
for line in CAUSES.get(bad["cause"], "沒有對應的成因說明").split("。"):
if line.strip():
print(" %s。" % line.strip())
elif ok:
print(" 通聯正常:到得了物件模型。")
else:
print(" 沒有測到物件模型(STK 那段略過了)—— **這不等於通**。")
if rows:
print()
print(" Connect 能力面(通聯成功不等於功能齊全):")
for label, _cmd, good, msg in rows:
print(" %-10s %s %s" % (label, "ACK " if good else "NACK", msg))
io.open(PROBE_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"host": host, "port": port, "layers": records,
"surface": [{"label": a, "cmd": b, "ack": c, "msg": d}
for a, b, c, d in rows]},
ensure_ascii=False, indent=1) + "\n")
print()
print(" 紀錄已存到 %s" % os.path.basename(PROBE_OUT))
return 0 if ok else 1
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_realtime.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 外部模擬驅動 STK(即時軌跡注入)。
## 這一節在做什麼
前面兩支證明「連得上」與「有多快」。這一支回答真正的用途:
**讓外部的模擬去驅動 STK。** 飛控軟體、硬體迴路、Simulink 模型算出
載具狀態,逐步推進 STK 裡的載具,再由 STK 去算它算不了的東西 ——
可見度、鏈路、覆蓋、干擾。
STK 這一側的機制是 **Realtime 傳播器**:載具的星曆不由 STK 自己算,
而是由外部一點一點餵進去。
## 三個實測到的坑
### 坑 1:飛機沒有 propagator,飛機有 route
衛星用 `satellite.propagator`;飛機、船、車用 `aircraft.route`。
把衛星那一套抄到飛機上,拿到的是 `AttributeError: 'Aircraft' object has
no attribute 'propagator'`,而錯誤訊息不會告訴你該改成 `route`。
切換的方法也跟著不同:`set_route_type(PropagatorType.REAL_TIME)`。
### 坑 2:`propagate()` 要**先**呼叫,不是最後
`PropagatorRealtime.propagate()` 的語意不是「把已經加好的點跑一遍」,
它的作用是**選定即時傳播器**。所以順序是反直覺的:
rt = aircraft.route
rt.propagate() # 先:選定即時傳播器
pts.add_position(...) # 後:才能加點
順序顛倒的話,第一次 `add_position` 就會收到
`Realtime propagator must be selected first.` —— 這句訊息其實講得很清楚,
但前提是你知道 `propagate()` 就是那個「select」。
### 坑 3:point builder 的屬性名在庫裡**拼錯了**
point_builder.ephemeris_in_latitude_longituide_altitude
^^^^^^^^^^ longituide
不是本檔打錯字,是裝好的套件裡就是這樣拼的。照正確拼法寫會拿到
`AttributeError`,而且你會以為是自己記錯了 API。
這種東西**只有內省裝好的套件才找得到**,任何文件或範例都不會提。
## 為什麼要成批注入
逐點 `add_position` 一次一趟網路往返;`add_position_batch` 一次送完。
實測 60 點:逐點約 1.1-1.6 ms/點,成批約 3 ms 總共 —— **快 20-30 倍**。
這個差距決定即時迴路跑不跑得動。一個 10 Hz 的迴路每幀有 100 ms 預算:
1 架載具、逐點 1 次往返/幀 約 1.5 ms 綽綽有餘
60 架載具、逐點 60 次往返/幀 約 90 ms **貼著預算,會掉幀**
60 架載具、成批 1 次往返/幀 約 3 ms 仍然綽綽有餘
所以「成批」不是微調,而是**能不能做即時**的分界。
## plant 為什麼選協調轉彎
外部模擬這一側用的是 3 自由度協調轉彎運動學,因為它有**解析解可對**:
固定坡度角 φ、固定空速 V,地面軌跡是一個半徑
R = V^2 / (g tan φ)
的圓。所以注入進 STK 再讀回來的軌跡,可以拿這個半徑去驗 ——
**不是拿 STK 對自己,而是拿外部的閉合式對 STK。**
同一組方程也寫在 `grpc_link_plant.m`,讓 Simulink 那一側算出同一條軌跡,
兩邊再互相對照(見 `GrpcLink_Simulink.md`)。
python grpc_link_realtime.py # 自己啟 Runtime,注入並讀回
python grpc_link_realtime.py --plant # 只算 plant,不連 STK
"""
import io
import json
import math
import os
import sys
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
RT_PORT = 40704
RT_OUT = os.path.join(HERE, "grpc_link_realtime.json")
G0 = 9.80665 # m/s^2
RE_M = 6378137.0 # WGS-84 赤道半徑(m)
EPOCH = "27 Sep 2026 09:00:00.000"
# plant 的設計點
V_TAS = 120.0 # 空速 m/s
BANK_DEG = 15.0 # 坡度角
GAMMA_DEG = 2.0 # 爬升角
LAT0, LON0, ALT0 = 24.80, 121.00, 1500.0 # 起點(台灣北部),高度 m
DT = 1.0 # plant 步長 s
N_STEPS = 120 # 兩分鐘
def turn_radius(v=V_TAS, bank_deg=BANK_DEG):
"""協調轉彎的地面軌跡半徑(m):R = V^2 / (g tan phi)。
這是**閉合式**,不是從 plant 積出來的 —— 所以它能當 plant 的獨立對照。
"""
return v * v / (G0 * math.tan(math.radians(bank_deg)))
def plant(n=N_STEPS, dt=DT, v=V_TAS, bank_deg=BANK_DEG,
gamma_deg=GAMMA_DEG, lat0=LAT0, lon0=LON0, alt0=ALT0):
"""3 自由度協調轉彎運動學。回傳 (t, lat, lon, alt_km) 四個等長清單。
psi_dot = (g / V) tan(phi) 航向角速率
x_dot = V cos(gamma) cos(psi) 北向
y_dot = V cos(gamma) sin(psi) 東向
h_dot = V sin(gamma) 爬升
用**平板地球**把北東位移換成經緯度:緯度直接除以地球半徑,
經度再除以 cos(lat)。兩分鐘、十幾公里的尺度上這個近似夠了,
而且它讓 MATLAB 那一側能寫出逐字相同的方程 —— 這比多幾位精度重要,
因為兩側要能互相對照。
積分用顯式歐拉,而且**刻意**用歐拉:Simulink 的 plant 也設成
固定步長歐拉,兩側才會逐點吻合。換成 RK4 精度更好,但兩側就對不上了,
那會讓「對照」失去意義。
"""
psi_dot = (G0 / v) * math.tan(math.radians(bank_deg))
cg, sg = math.cos(math.radians(gamma_deg)), math.sin(math.radians(gamma_deg))
t, lat, lon, alt = [], [], [], []
psi, la, lo, h = 0.0, lat0, lon0, alt0
for i in range(n):
t.append(i * dt)
lat.append(la)
lon.append(lo)
alt.append(h / 1000.0) # STK 的距離單位設成 km
dn = v * cg * math.cos(psi) * dt
de = v * cg * math.sin(psi) * dt
la += math.degrees(dn / RE_M)
lo += math.degrees(de / (RE_M * math.cos(math.radians(la))))
h += v * sg * dt
psi += psi_dot * dt
return t, lat, lon, alt
def fitted_radius(lat, lon):
"""把地面軌跡擬一個圓,回傳半徑(m)。用來對 turn_radius()。
代數擬圓(Kasa 法):圓 x^2+y^2+Dx+Ey+F=0 對 (D,E,F) 是線性的,
所以一次最小平方就解完,不需要疊代、也不需要初始猜值。
"""
xs = [math.radians(lo - lon[0]) * RE_M * math.cos(math.radians(lat[0]))
for lo in lon]
ys = [math.radians(la - lat[0]) * RE_M for la in lat]
n = len(xs)
# 正規方程 A^T A c = A^T b,A 的列是 (x, y, 1),b = -(x^2+y^2)
sxx = sum(x * x for x in xs)
syy = sum(y * y for y in ys)
sxy = sum(x * y for x, y in zip(xs, ys))
sx, sy = sum(xs), sum(ys)
r2 = [x * x + y * y for x, y in zip(xs, ys)]
bx = -sum(x * r for x, r in zip(xs, r2))
by = -sum(y * r for y, r in zip(ys, r2))
bf = -sum(r2)
# 3x3 解(Cramer)—— 規模小,直接寫開比拉進 numpy 清楚
m = [[sxx, sxy, sx], [sxy, syy, sy], [sx, sy, float(n)]]
b = [bx, by, bf]
def det3(a):
return (a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
- a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
+ a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]))
d = det3(m)
if abs(d) < 1e-9:
return float("nan")
sol = []
for j in range(3):
mj = [row[:] for row in m]
for i in range(3):
mj[i][j] = b[i]
sol.append(det3(mj) / d)
D, E, F = sol
return math.sqrt(max(D * D / 4 + E * E / 4 - F, 0.0))
def timestamps(t, epoch=EPOCH):
"""把秒數變成 STK 吃的絕對時間字串。
**一定要給絕對時間,不要靠迴圈的節奏。** 外部迴路的牆鐘時間會抖,
而 STK 該用的是模擬時間 —— 兩者混在一起,算出來的可見度窗
會跟著你的 CPU 負載變動,而那種誤差事後查不出來。
"""
base = epoch.rsplit(" ", 1)
hh, mm, ss = base[1].split(":")
start = int(hh) * 3600 + int(mm) * 60 + float(ss)
out = []
for sec in t:
v = start + sec
out.append("%s %02d:%02d:%06.3f"
% (base[0], int(v // 3600), int((v % 3600) // 60), v % 60))
return out
# ------------------------------------------------------------------ STK 側
def make_realtime_aircraft(root, name="ControlledAircraft"):
"""建一架用 Realtime route 的飛機,回傳 point builder。
坑 1 與坑 2 都在這裡:`route` 不是 `propagator`,
而且 `propagate()` 要先呼叫。
"""
from ansys.stk.core.stkobjects import STKObjectType, PropagatorType
ac = root.current_scenario.children.new(STKObjectType.AIRCRAFT, name)
ac.set_route_type(PropagatorType.REAL_TIME) # 坑 1:route 不是 propagator
route = ac.route
route.propagate() # 坑 2:先選定,才能加點
# 坑 3:套件裡就是拼成 longituide,不是本檔打錯
return ac, route, route.point_builder.ephemeris_in_latitude_longituide_altitude
def inject(pts, times, lat, lon, alt, batch=True):
"""把軌跡注入 STK。回傳耗時(秒)。"""
import time as _t
t0 = _t.time()
if batch:
pts.add_position_batch(list(times), list(lat), list(lon), list(alt))
else:
for i in range(len(times)):
pts.add_position(times[i], lat[i], lon[i], alt[i])
return _t.time() - t0
def readback(ac, scen, step=DT, start=None, stop=None):
"""從 STK 讀回注入的軌跡。回傳 (lat, lon)。
`start` / `stop` 沒給就用整個場景區間。**注入的點只覆蓋場景的一部分時
一定要給** —— 不然 DataProvider 會把沒有星曆的那一大段也一起取回來,
而它**不會報錯**:實測在 24 小時的場景裡注入 60 秒的軌跡,
讀回來是 1860 列,擬圓半徑從 5,480 m 變成 2,951,519 m。
**這一步不能省。** 注入沒有拋例外,只代表指令送到了;
STK 有沒有真的收下那條軌跡,要讀回來才知道。
這個教材庫每一章的驗證段都建立在同一件事上。
"""
dp = ac.data_providers.item("LLA State").group.item("Fixed")
res = dp.execute(scen.start_time if start is None else start,
scen.stop_time if stop is None else stop, step)
return (res.data_sets.get_data_set_by_name("Lat").get_values(),
res.data_sets.get_data_set_by_name("Lon").get_values())
def frame_budget(ms_per_point, hz=10.0):
"""在量到的每點成本下,一幀塞得下幾架載具。回傳 (每幀預算 ms, 架數)。"""
budget = 1000.0 / hz
return budget, int(budget / ms_per_point) if ms_per_point > 0 else 0
def main():
t, lat, lon, alt = plant()
r_closed = turn_radius()
r_fit = fitted_radius(lat, lon)
rel = abs(r_fit - r_closed) / r_closed
print("=" * 78)
print(" 外部模擬驅動 STK — 協調轉彎軌跡注入")
print("=" * 78)
print(" plant:V=%.0f m/s 坡度 %.0f deg 爬升 %.0f deg %d 點 @ %.1f s"
% (V_TAS, BANK_DEG, GAMMA_DEG, len(t), DT))
print(" 轉彎半徑 閉合式 %.1f m 擬圓 %.1f m 相對差 %.3f%%"
% (r_closed, r_fit, rel * 100))
print(" -> 閉合式是獨立對照,不是拿 plant 對自己。")
if "--plant" in sys.argv:
io.open(RT_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"r_closed_m": r_closed, "r_fit_m": r_fit,
"rel": rel, "lat": lat, "lon": lon, "alt_km": alt},
ensure_ascii=False, indent=1) + "\n")
print(" (--plant:不連 STK。已存 %s)" % os.path.basename(RT_OUT))
return 0
try:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:110])
print(" 先跑 grpc_link_probe.py 找出斷在哪一層;或用 --plant 只算 plant。")
return 1
root = app.new_object_root()
root.new_scenario("RealtimeInject")
scen = root.current_scenario
times = timestamps(t)
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"' % (times[0], times[-1]))
root.execute_command("SetUnits / km")
ac, route, pts = make_realtime_aircraft(root)
print()
dt_one = inject(pts, times, lat, lon, alt, batch=False)
ms_pt = dt_one / len(times) * 1000.0
print(" 逐點 add_position %d 點 %.3f s %.2f ms/點"
% (len(times), dt_one, ms_pt))
route.point_builder.remove_all_points()
dt_batch = inject(pts, times, lat, lon, alt, batch=True)
print(" 成批 add_position_batch %d 點 %.3f s 快 %.1f 倍"
% (len(times), dt_batch, dt_one / dt_batch if dt_batch else float("nan")))
route.propagate()
rb_lat, rb_lon = readback(ac, scen)
r_stk = fitted_radius(rb_lat, rb_lon)
print()
print(" 讀回 %d 列。STK 裡的軌跡擬圓半徑 %.1f m(閉合式 %.1f m,差 %.3f%%)"
% (len(rb_lat), r_stk, r_closed, abs(r_stk - r_closed) / r_closed * 100))
print()
print(" 即時預算(10 Hz,每幀 100 ms):")
for label, cost in (("逐點", ms_pt),
("成批", dt_batch / len(times) * 1000.0)):
budget, n = frame_budget(cost)
print(" %s %.3f ms/點 -> 一幀塞得下約 %d 點" % (label, cost, n))
io.open(RT_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"r_closed_m": r_closed, "r_fit_m": r_fit, "rel": rel,
"r_stk_m": r_stk, "n": len(times),
"one_by_one_s": dt_one, "batch_s": dt_batch,
"ms_per_point": ms_pt,
"lat": lat, "lon": lon, "alt_km": alt,
"readback_lat": list(rb_lat), "readback_lon": list(rb_lon)},
ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(RT_OUT))
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_simulink.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — Simulink 到 STK 的橋接。
## 架構,以及為什麼不走「Simulink 直接呼叫 Python」
最短的接法是讓 Simulink 的 MATLAB Function 區塊直接用 MATLAB 內建的
Python 介面(`pyenv` / `py.模組.函式`)去呼叫 PySTK。少一層、少一個埠。
**但它把兩個版本綁在一起**:MATLAB 的行內 Python 介面只支援它那一版
明確列出的 Python 版本區間,而 PySTK 要新的 Python。兩邊的版本窗一旦
不重疊,這條路就整條不存在,而且症狀是 `pyenv` 設定失敗這種
完全看不出跟 STK 有關的錯誤。
所以本章走**行程分離**:Simulink 只管把狀態丟出來,Python 這一側
收下來再進 STK。代價是多一個埠,換到三件事:
1. **版本解耦。** MATLAB 用它的 Python(或完全不用),Python 這側用自己的。
2. **可單獨測試。** 沒有 MATLAB 也能測整條鏈 —— 本檔的 `--fake` 就是
一個合成發送端,拿它驗橋接,不必開 Simulink。
3. **可換掉任一端。** 之後要換成 ROS 2、UDP 遙測、硬體迴路,
改的是發送端,橋接與 STK 這側不動。
## 兩條路,擇一
--udp 即時串流。Simulink 每一步送一個封包過來。
需要 Simulink 那側有 UDP 傳送區塊(視工具箱授權而定)。
--csv 離線重放。Simulink 跑完把結果存成 CSV,本檔讀進來重放。
**不需要任何額外工具箱**,而且結果可重現 —— 教學建議從這條開始。
兩條路進 STK 的程式碼是同一段,所以先用 `--csv` 把物理與介接都弄對,
再換 `--udp` 只是把資料來源換掉。
## 封包格式(刻意選得很笨)
4 個 float64,little-endian,共 32 bytes:t(秒), lat(deg), lon(deg), alt(km)
沒有標頭、沒有版本欄、沒有校驗和 —— 因為這是教學用的固定格式,
加上那些只會讓 Simulink 那側的區塊設定變複雜。
**代價要講清楚**:UDP 不保證送到也不保證順序,所以
本檔用 `t` 當唯一的時間依據,**不靠封包到達的順序**,
## 節奏:這才是 gRPC 在這裡的重點
Simulink 一步、往 STK 送一次,就是一步一次網路往返。實測 gRPC 上
逐點注入約 1-1.6 ms,所以 10 Hz、單載具很輕鬆;但載具一多,
或步長縮到 100 Hz,往返就吃掉整個預算。
本檔因此**攢批**:收滿 `--chunk` 個點才送一次 `add_position_batch`。
攢批會帶來延遲(最多 chunk x 步長),所以它是個取捨,
`--chunk 1` 就退回逐點。橋接結束時會印出實際的往返數與每點成本,
讓這個取捨變成一個可以量的東西,而不是憑感覺調。
python grpc_link_simulink.py --fake # 合成發送端,端到端自測
python grpc_link_simulink.py --csv out.csv # 重放 Simulink 匯出的 CSV
python grpc_link_simulink.py --udp # 等 Simulink 即時送
python grpc_link_simulink.py --udp --wait 120 # 首包多等一會(MATLAB 冷啟動)
python grpc_link_simulink.py --fake --no-stk # 只測橋接,不連 STK
"""
import io
import json
import os
import socket
import struct
import sys
import threading
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_realtime as R # noqa: E402
UDP_HOST = "127.0.0.1"
UDP_PORT = 41000
FRAME = "<4d" # t, lat, lon, alt —— little-endian float64 x4
FRAME_SIZE = struct.calcsize(FRAME)
BRIDGE_OUT = os.path.join(HERE, "grpc_link_simulink.json")
CSV_OUT = os.path.join(HERE, "grpc_link_simulink_frames.csv")
assert FRAME_SIZE == 32, "封包長度必須是 32 bytes,Simulink 那側也照這個設"
# ------------------------------------------------------------ 發送端(測試用)
def fake_simulink(n=None, hz=10.0, host=UDP_HOST, port=UDP_PORT, drop=0):
"""合成發送端:照 plant 的軌跡用 UDP 送出去,模擬 Simulink。
`drop` 是刻意丟掉的封包數(前 drop 個每隔一個丟一次),用來驗
橋接的漏包統計真的算得對 —— 不注入失敗,就不知道統計是不是假的。
"""
t, lat, lon, alt = R.plant(n=n or R.N_STEPS)
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sent = 0
for i in range(len(t)):
if drop and i < drop * 2 and i % 2 == 1:
continue # 刻意漏掉
s.sendto(struct.pack(FRAME, t[i], lat[i], lon[i], alt[i]), (host, port))
sent += 1
time.sleep(1.0 / hz)
s.close()
return sent
# ------------------------------------------------------------ 資料來源
def source_udp(port=UDP_PORT, expect=None, idle_timeout=3.0, first_timeout=None):
"""從 UDP 收封包,yield (t, lat, lon, alt)。
收到 `expect` 個,或連續 `idle_timeout` 秒沒東西,就結束。
**沒有 idle 超時的接收端會永遠掛著** —— Simulink 停了不會通知你。
**首包要另計超時。** 這是實測踩到的:MATLAB 從冷啟動到開始送封包
要二、三十秒,而封包之間只隔幾十毫秒。用同一個 3 秒超時,
橋接會在 MATLAB 還在開機時就放棄,然後回報「收到 0 點」——
看起來像介接壞了,其實只是等太短。所以首包預設給 60 秒,
接上之後才縮到 `idle_timeout`。
"""
if first_timeout is None:
first_timeout = 60.0
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(("0.0.0.0", port))
s.settimeout(first_timeout)
got = 0
try:
while expect is None or got < expect:
try:
data, _addr = s.recvfrom(4096)
except socket.timeout:
break
if len(data) != FRAME_SIZE:
continue # 長度不對就丟掉,別猜
if got == 0:
s.settimeout(idle_timeout) # 首包到了,改用短超時
got += 1
yield struct.unpack(FRAME, data)
finally:
s.close()
def source_csv(path):
"""從 CSV 讀,yield (t, lat, lon, alt)。第一行當標頭跳過。"""
with io.open(path, encoding="utf-8") as f:
for i, line in enumerate(f):
line = line.strip()
if not line or (i == 0 and not line[0].isdigit()
and not line[0] == "-"):
continue
parts = [p for p in line.replace(",", " ").split() if p]
if len(parts) < 4:
continue
yield tuple(float(p) for p in parts[:4])
def write_csv(path=CSV_OUT, n=None):
"""把 plant 匯成 CSV,格式與 Simulink 的 `writematrix` 輸出一致。
這樣 `--csv` 這條路在沒有 MATLAB 的機器上也測得動。"""
t, lat, lon, alt = R.plant(n=n or R.N_STEPS)
with io.open(path, "w", encoding="utf-8", newline="\n") as f:
f.write("t_sec,lat_deg,lon_deg,alt_km\n")
for row in zip(t, lat, lon, alt):
f.write("%.6f,%.9f,%.9f,%.6f\n" % row)
return path
# ------------------------------------------------------------ 橋接
class Bridge:
"""把資料來源的點攢成批,再送進 STK。
`pts` 給 None 就只做統計、不連 STK(`--no-stk`)—— 橋接本身
因此可以在沒有 STK 授權的機器上驗。
"""
def __init__(self, pts=None, chunk=10, epoch=R.EPOCH):
self.pts = pts
self.chunk = max(1, chunk)
self.epoch = epoch
self.buf = []
self.n_points = 0
self.n_calls = 0
self.inject_s = 0.0
self.t_seen = []
def feed(self, frame):
self.buf.append(frame)
self.t_seen.append(frame[0])
if len(self.buf) >= self.chunk:
self.flush()
def flush(self):
if not self.buf:
return
ts = R.timestamps([f[0] for f in self.buf], self.epoch)
lat = [f[1] for f in self.buf]
lon = [f[2] for f in self.buf]
alt = [f[3] for f in self.buf]
if self.pts is not None:
t0 = time.time()
# 一次往返送掉整批 —— chunk=1 就退回逐點
self.pts.add_position_batch(ts, lat, lon, alt)
self.inject_s += time.time() - t0
self.n_points += len(self.buf)
self.n_calls += 1
self.buf = []
def gaps(self, dt=R.DT):
"""用 t 序列找漏包 —— **不靠到達順序**,UDP 不保證順序。"""
if len(self.t_seen) < 2:
return 0
ts = sorted(self.t_seen)
expect = int(round((ts[-1] - ts[0]) / dt)) + 1
return max(0, expect - len(ts))
def report(self):
per_pt = (self.inject_s / self.n_points * 1000.0) if self.n_points else 0.0
return {"points": self.n_points, "grpc_calls": self.n_calls,
"chunk": self.chunk, "inject_s": self.inject_s,
"ms_per_point": per_pt, "gaps": self.gaps()}
def main():
use_stk = "--no-stk" not in sys.argv
chunk = 10
if "--chunk" in sys.argv:
chunk = int(sys.argv[sys.argv.index("--chunk") + 1])
drop = 0
if "--drop" in sys.argv:
drop = int(sys.argv[sys.argv.index("--drop") + 1])
wait = 60.0
if "--wait" in sys.argv:
wait = float(sys.argv[sys.argv.index("--wait") + 1])
mode = "fake" if "--fake" in sys.argv else (
"csv" if "--csv" in sys.argv else "udp")
csv_path = None
if mode == "csv":
i = sys.argv.index("--csv")
csv_path = (sys.argv[i + 1] if len(sys.argv) > i + 1
and not sys.argv[i + 1].startswith("--") else write_csv())
print("=" * 78)
print(" Simulink -> STK 橋接(模式 %s,chunk %d%s)"
% (mode, chunk, "" if use_stk else ",不連 STK"))
print("=" * 78)
app = pts = route = ac = root = None
if use_stk:
try:
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
root = app.new_object_root()
root.new_scenario("SimulinkBridge")
t_all, _, _, _ = R.plant()
ts = R.timestamps(t_all)
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"'
% (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
ac, route, pts = R.make_realtime_aircraft(root, "SimulinkAircraft")
print(" STK 已就緒:%s(Realtime route)" % ac.instance_name)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:100])
print(" 改用 --no-stk 只驗橋接,或先跑 grpc_link_probe.py。")
return 1
bridge = Bridge(pts, chunk)
t0 = time.time()
if mode == "csv":
print(" 來源:CSV %s" % os.path.basename(csv_path))
for frame in source_csv(csv_path):
bridge.feed(frame)
else:
n_expect = R.N_STEPS - drop
if mode == "fake":
sender = threading.Thread(target=fake_simulink,
kwargs={"drop": drop}, daemon=True)
sender.start()
print(" 來源:合成發送端(UDP :%d,刻意漏 %d 包)" % (UDP_PORT, drop))
else:
print(" 來源:等 Simulink 送到 UDP :%d" % UDP_PORT)
print(" 首包最多等 %.0f 秒(MATLAB 冷啟動要二三十秒),"
"之後閒置 3 秒就結束" % wait)
n_expect = None
for frame in source_udp(expect=n_expect, first_timeout=wait):
bridge.feed(frame)
bridge.flush()
wall = time.time() - t0
rep = bridge.report()
rep["wall_s"] = wall
print()
print(" 收到 %d 點,漏 %d 包" % (rep["points"], rep["gaps"]))
print(" gRPC 往返 %d 次(攢批把 %d 點壓成 %d 次)"
% (rep["grpc_calls"], rep["points"], rep["grpc_calls"]))
if use_stk:
print(" 注入耗時 %.3f s,%.3f ms/點" % (rep["inject_s"], rep["ms_per_point"]))
budget, cap = R.frame_budget(rep["ms_per_point"] or 1e-9)
print(" -> 10 Hz 每幀 %.0f ms 預算下,塞得下約 %d 點/幀" % (budget, cap))
route.propagate()
rb_lat, rb_lon = R.readback(ac, root.current_scenario)
r_stk = R.fitted_radius(rb_lat, rb_lon)
r_ref = R.turn_radius()
rep["r_stk_m"], rep["r_closed_m"] = r_stk, r_ref
print()
print(" 端到端驗證:STK 裡讀回 %d 列,擬圓半徑 %.1f m" % (len(rb_lat), r_stk))
print(" 閉合式 V^2/(g tan phi) = %.1f m,差 %.3f%%"
% (r_ref, abs(r_stk - r_ref) / r_ref * 100))
print(" -> 走完 Simulink 側方程 -> UDP -> gRPC -> STK,")
print(" 再與**外部閉合式**對照,而不是拿 STK 對自己。")
io.open(BRIDGE_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(rep, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(BRIDGE_OUT))
if app is not None:
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_simulink_bidir.m
function mdl = grpc_link_simulink_bidir(mdl, dt, stop_s, force_ok)
% GRPC_LINK_SIMULINK_BIDIR 雙向(閉迴路)的 Simulink 模型,用腳本建出來。
%
% ## 與單向模型的差別
%
% 單向模型(`grpc_link_simulink_build.m`)用 From Workspace 餵**預算好的**
% 軌跡,所以 STK 算出來的東西影響不了飛行 —— STK 在那條鏈裡只是終點。
%
% 這一支把 plant 做成**模型裡的狀態**,並把 Python 橋接送回來的判斷
% 接進控制律。於是迴路真的閉起來了:
%
% STK 說鏈路斷了 -> 坡度角翻向 -> 飛機折返 -> 下一步的位置不一樣
%
% ## 模型長什麼樣(六個區塊)
%
% [RecvFB] --ok----------+
% ^ v
% [Clock] ----+ [StateDelay] --s--> [PlantStep] --s(k+1)--+
% | | | |
% +--> [SendState] <------+ +--> [Logged] |
% |
% +---------------(走最下方繞回)-------------------------- +
%
% **六個區塊,七條線。** 要讓腳本建出來的圖能看,有兩件事決定成敗,
% 而它們都不是排版:
%
% 常數不要做成區塊 dt / V / bank / gamma / force 如果各做一個 Constant
% 區塊接進 PlantStep,就是 11 個區塊、12 條線。
% 它們是**常數不是訊號**,烘進函式的程式碼裡就好。
% 同一個來源的消費端擺同一欄
% Clock 要餵 RecvFB 與 SendState。這兩個若一個在左上、
% 一個在右上,Clock 的線**必然**橫跨中間那一帶,
% 於是穿過其他方塊 —— 這種情況調座標救不回來。
% 把它們排成同一欄,長距水平線就不存在。
%
% 線穿過方塊多半不是繞線器笨,是**圖的拓樸本來就逼它穿過去**。
%
% 另外有個看起來很合理的簡化要避開:「狀態的第一個元素就是時間,
% 可以省掉 Clock」。不對 —— `grpc_link_step` 的狀態是 `[psi; lat; lon; alt]`,
% 第一個元素是**航向角**。真照那樣改,SendState 會把航向角當成時戳送出去,
% 而且兩端都不會報錯。省區塊之前先去看方程。
%
% `StateDelay`(Unit Delay)是**故意**放在迴路裡的:它把代數迴路打斷,
% 也讓「這一步用的是上一步的狀態」這件事在圖上看得見。
%
% ## 方程只有一份
%
% `PlantStep` 呼叫 `grpc_link_step.m`,而 `grpc_link_plant.m` 也呼叫它。
% 所以這個模型與那支批次版是**同一組方程**,不是第三份實作 ——
% 「兩邊一致」因此是結構上保證的,不必每次重新量。
%
% ## 怎麼驗它(退化情形)
%
% 把 `force_ok` 設成 true,回饋就被強制成「永遠可用」,控制律永遠回
% 同一個坡度角 —— 這時模型的軌跡**必須與 `grpc_link_plant()` 逐點相同**。
% 那是個乾淨的退化情形檢查,不需要另外找一個解析基準。
%
% 用法:
% % 先在另一個終端機啟動 Python 側:
% % python grpc_link_closedloop.py --udp --mode access
% grpc_link_simulink_bidir(); % 閉迴路
% grpc_link_simulink_bidir('chk', 1.0, 400, true); % 退化情形(不需 Python)
%
% 回傳模型名稱。跑完記得 `grpc_link_udp_send('close')` 與
% `grpc_link_udp_recv('close')`。
if nargin < 1 || isempty(mdl), mdl = 'grpc_link_sim_bidir'; end
if nargin < 2 || isempty(dt), dt = 1.0; end
if nargin < 3 || isempty(stop_s), stop_s = 400.0; end
if nargin < 4 || isempty(force_ok), force_ok = false; end
V = 120.0;
BANK_DEG = 15.0;
GAMMA_DEG = 2.0;
LAT0 = 24.80;
LON0 = 121.00;
ALT0_M = 1500.0;
% 常數烘進 MATLAB Function 的程式碼裡,不經 base workspace ——
% 少了五個 Constant 區塊,也少了「base 裡的變數被別人改掉」這個失敗模式。
% ---- 乾淨重建 ----
if bdIsLoaded(mdl)
close_system(mdl, 0);
end
new_system(mdl);
if usejava('desktop')
open_system(mdl);
else
load_system(mdl);
end
set_param(mdl, 'SolverType', 'Fixed-step', ...
'Solver', 'FixedStepDiscrete', ...
'FixedStep', num2str(dt), ...
'StartTime', '0', ...
'StopTime', num2str(stop_s), ...
'EnablePacing', 'on', ...
'PacingRate', '1');
% ---- 1. Clock ----
add_block('simulink/Sources/Clock', [mdl '/SimTime']);
% ---- 2. 狀態的單位延遲(打斷代數迴路)----
add_block('simulink/Discrete/Unit Delay', [mdl '/StateDelay']);
set_param([mdl '/StateDelay'], ...
'InitialCondition', sprintf('[0; %.10g; %.10g; %.10g]', ...
LAT0, LON0, ALT0_M), ...
'SampleTime', num2str(dt));
% ---- 3. 收回饋(不阻塞)----
% **常數烘進函式,不用 Constant 區塊接線。**
% dt / V / bank / gamma / force 若各做一個 Constant 區塊接進 PlantStep,
% PlantStep 就有六個輸入埠、五條長線橫跨整張圖,自動繞線只好讓線
% 穿過其他方塊 —— 排版再怎麼調都救不回來。
%
% 它們是**常數,不是訊號**;而且這個模型本來就由腳本建,
% 改值就重建。烘進去之後區塊從 11 個降到 6 個,線也少了一半。
add_block('simulink/User-Defined Functions/MATLAB Function', ...
[mdl '/RecvFB']);
set_matlab_function_body([mdl '/RecvFB'], sprintf([ ...
'function ok = RecvFB(t)\n' ...
'%%#codegen\n' ...
'coder.extrinsic(''grpc_link_fb_ok'');\n' ...
'ok = 1.0;\n' ...
'if %d < 1\n' ... % force_ok 為真時不碰網路
' ok = grpc_link_fb_ok(t);\n' ...
'end\n' ...
'end\n'], double(force_ok)));
% ---- 4. plant 單步 + 控制律 ----
add_block('simulink/User-Defined Functions/MATLAB Function', ...
[mdl '/PlantStep']);
set_matlab_function_body([mdl '/PlantStep'], sprintf([ ...
'function s2 = PlantStep(s, ok)\n' ...
'%%#codegen\n' ...
'coder.extrinsic(''grpc_link_step'');\n' ...
'%% 控制律:鏈路可用維持坡度角,中斷就翻向(朝回地面站)\n' ...
'b = %.10g;\n' ...
'if ok < 0.5\n' ...
' b = -%.10g;\n' ...
'end\n' ...
's2 = zeros(4, 1);\n' ...
's2 = grpc_link_step(s, b, %.10g, %.10g, %.10g);\n' ...
'end\n'], BANK_DEG, BANK_DEG, dt, V, GAMMA_DEG));
% ---- 5. 送狀態出去 ----
add_block('simulink/User-Defined Functions/MATLAB Function', ...
[mdl '/SendState']);
set_matlab_function_body([mdl '/SendState'], sprintf([ ...
'function nbytes = SendState(t, s)\n' ...
'%%#codegen\n' ...
'coder.extrinsic(''grpc_link_udp_send'');\n' ...
'nbytes = 0;\n' ...
'if %d < 1\n' ...
' nbytes = grpc_link_udp_send(t, s(2), s(3), s(4) / 1000.0);\n' ...
'end\n' ...
'end\n'], double(force_ok)));
% ---- 6. 記錄 ----
add_block('simulink/Sinks/To Workspace', [mdl '/Logged']);
set_param([mdl '/Logged'], 'VariableName', 'gl_bidir_out', ...
'SaveFormat', 'Array', 'SampleTime', num2str(dt));
% ---- 版面:**必須在接線之前** ----
% `add_line(..., 'autorouting', 'on')` 是在**加線的當下**算路徑的,
% 不是在存檔或畫圖的時候。先接線再搬方塊,繞線器看到的是預設位置,
% 搬動只是把既有路徑拉長 —— 於是線穿過方塊、出現對角線。
%
% 徵兆很明確:調了座標卻**一點差別都沒有**。那通常不代表調得不夠,
% 而代表改的東西根本沒被讀到。
% 兩條規則:
% (1) Clock 的兩個消費端排成同一欄(RecvFB 上、SendState 下),
% 它只要兩條短線,不必橫跨整張圖。
% (2) 唯一往回走的線(PlantStep -> StateDelay)走**最下方**,
% 所以最低的方塊底邊要留在那條走道之上。
pos = struct( ...
'SimTime', [ 40, 150, 80, 190], ...
'RecvFB', [170, 30, 285, 100], ...
'StateDelay', [170, 240, 235, 295], ...
'PlantStep', [430, 120, 565, 230], ...
'SendState', [430, 340, 545, 410], ...
'Logged', [700, 240, 800, 290]);
names = fieldnames(pos);
for k = 1:numel(names)
set_param([mdl '/' names{k}], 'Position', pos.(names{k}));
end
% ---- 接線(七條)----
add_line(mdl, 'SimTime/1', 'RecvFB/1', 'autorouting', 'on');
add_line(mdl, 'SimTime/1', 'SendState/1', 'autorouting', 'on');
add_line(mdl, 'StateDelay/1', 'PlantStep/1', 'autorouting', 'on');
add_line(mdl, 'RecvFB/1', 'PlantStep/2', 'autorouting', 'on');
add_line(mdl, 'StateDelay/1', 'SendState/2', 'autorouting', 'on');
add_line(mdl, 'StateDelay/1', 'Logged/1', 'autorouting', 'on');
add_line(mdl, 'PlantStep/1', 'StateDelay/1', 'autorouting', 'on');
fprintf('模型 %s 已建好(dt=%.3f s,停在 %.0f s,force_ok=%d)\n', ...
mdl, dt, stop_s, force_ok);
if ~force_ok
fprintf('先啟動 Python 側再按 Run:\n');
fprintf(' python grpc_link_closedloop.py --udp --mode access\n');
end
end
function set_matlab_function_body(blk, body)
% 把程式碼寫進 MATLAB Function 區塊。
% 沒有簡單的 set_param 可用 —— 要透過 Stateflow 的根物件找到對應的 EML chart。
rt = sfroot();
h = rt.find('-isa', 'Stateflow.EMChart', '-and', 'Path', blk);
if isempty(h)
error('找不到 %s 的 MATLAB Function chart', blk);
end
h.Script = body;
end
grpc_link/grpc_link_simulink_build.m
function mdl = grpc_link_simulink_build(mdl, dt, do_save, n)
% GRPC_LINK_SIMULINK_BUILD 用程式建出串流到 STK 的 Simulink 模型。
%
% ## 為什麼用腳本建,而不是附一個 .slx
%
% `.slx` 是**二進位**。放進教材庫會有三個問題,而且每一個都真的會發生:
%
% 1. **看不出改了什麼。** git diff 只會說「二進位檔不同」。
% 學生拿到一個壞掉的模型,沒有辦法知道是哪一步被改壞的。
% 2. **綁 MATLAB 版本。** 新版存的 .slx 舊版開不起來,
% 而課堂上的機器版本不會一致。
% 3. **沒辦法審。** 助教無法在不開 MATLAB 的情況下檢查模型對不對。
%
% 腳本是純文字:可 diff、可審、可跨版本重建。代價是要多跑一行指令,
% 換到的是「模型的定義本身進了版控」。
%
% ## 模型長什麼樣(四個區塊)
%
% [Clock] ------------------\
% >--> [MATLAB Function: 送 UDP] --> [To Workspace]
% [From Workspace] ---------/ grpc_link_udp_send grpc_link_out
% grpc_link_ws (lat,lon,alt)
%
% **時間走 Clock,不跟資料混在一起。** 把 [t lat lon alt] 整個丟給
% From Workspace,以為區塊會輸出四個元素 —— 但 From Workspace 是把
% **第一欄當時間**、其餘當資料,所以輸出只有三個元素,函式裡的 u(4) 越界。
% 固定尺寸訊號的越界在編譯期就會被抓,症狀是一句看不出原因的
% 「Errors occurred during parsing of MATLAB function」。
%
% 改成 Clock 之後還多一個好處:時間來源變成**明確的模擬時間**,
% 而這正是本章一再強調的事 —— 外部迴路的牆鐘會抖,STK 要的是模擬時間。
%
% ## 為什麼 plant 不做在 Simulink 的積分器裡
%
% 這是刻意的。plant 已經有一份權威實作(`grpc_link_plant.m`),
% 而它**已經逐點驗過與 Python 側吻合到 5e-14 度**。如果再用 Simulink 的
% 積分器做第三份,那三份都要互相交叉驗證,而它們的離散化只要有一點不同
% 就對不上 —— 於是「Simulink 接進 STK 對不對」這個問題,
% 會被「三份 plant 為什麼不一樣」淹掉。
%
% 所以這個模型專注在本章真正要教的東西:**串流路徑與節奏**。
% 要換成真正的 6 自由度模型,就把 From Workspace 那一格換掉,
% 其餘完全不動 —— 這也正是行程分離架構的好處。
%
% ## 節奏
%
% 解算器設成 FixedStepDiscrete、步長 = dt,並開啟模擬節奏
% (`EnablePacing`),讓模擬時間貼著牆鐘跑。**不開 pacing 的話
% Simulink 會用最快速度跑完**,幾秒鐘噴出全部封包 —— 那是在測吞吐量,
% 不是在測即時串流,而兩者的結論完全不同。
%
% 用法:
% grpc_link_simulink_build(); % 建模型並開起來
% grpc_link_simulink_build('mysim', 0.1, false, 600); % 自訂步長與點數
%
% 跑之前先在另一個終端機啟動 Python 橋接:
% python grpc_link_simulink.py --udp --wait 120
%
% ## 驗證狀態(請先讀這一段)
%
% **本支尚未在撰寫環境中實際執行過。** 撰寫時該機器的 MATLAB 在多次
% 命令列啟動之後停止回應(授權階段),因此本支與 `grpc_link_udp_send.m`
% 只做過人工審閱,沒有跑過。相對地,以下是**真的跑過並對照過**的:
%
% * `grpc_link_plant.m` 在 MATLAB R2021b 實跑,與 Python 側的
% `grpc_link_realtime.plant()` 逐點吻合到 5e-14 度(浮點捨入量級)。
% * Python 這一側的整條鏈(合成發送端 -> UDP -> 攢批 -> 統計)
% 用 `grpc_link_simulink.py --fake` 跑過,含刻意注入漏包來驗漏包統計。
%
% 所以「模型的三個區塊接起來會不會跑」這一件事,請你在自己的機器上
% 用下面的檢查清單確認一次,不要假設它是驗過的:
%
% 1. `grpc_link_simulink_build('grpc_link_sim', 0.1, false)` 要不報錯,
% 且 `find_system('grpc_link_sim','Type','Block')` 要看到三個區塊。
% 2. 先單獨測發送端:`grpc_link_udp_send(0, 24.8, 121.0, 1.5)` 應回 32。
% 同時讓 Python 那側跑 `--udp`,它應該收到 1 點。
% 3. 才按 Run。
%
% 這一段寫在這裡而不是寫在實驗單裡,是因為**它是這支程式的狀態,
% 不是課程的步驟** —— 拿到檔案的人應該先看到它。
if nargin < 1 || isempty(mdl), mdl = 'grpc_link_sim'; end
if nargin < 2 || isempty(dt), dt = 1.0; end
if nargin < 3 || isempty(do_save), do_save = false; end
% 預設跑滿一圈多一點:R = V^2/(g tanphi) 的週期是 2*pi*V/(g tanphi),
% 約 287 秒。跑不滿一圈的話,地面軌跡看起來只是一小段弧,
% 「協調轉彎會畫出一個圓」這件事在圖上完全看不出來。
if nargin < 4 || isempty(n)
n = ceil(1.1 * 2 * pi * 120.0 / (9.80665 * tand(15.0)) / dt);
end
% ---- plant:唯一權威來源 ----
% From Workspace 吃 [時間, 資料...],所以這裡的第一欄是時間、
% 後三欄才是輸出的訊號。時間另外由 Clock 給函式用。
[t, lat, lon, alt_km] = grpc_link_plant(n, dt);
ws = [t, lat, lon, alt_km];
assignin('base', 'grpc_link_ws', ws);
fprintf('plant:%d 點,步長 %.3f s,共 %.1f s\n', numel(t), dt, t(end));
% ---- 乾淨重建 ----
if bdIsLoaded(mdl)
close_system(mdl, 0);
end
new_system(mdl);
% **只有在真的有桌面時才開視窗。** 少了這個判斷,這支腳本就不能
% 在 `matlab -nodesktop` 或 CI 裡跑 —— 而「模型能不能無介面重建」
% 正是把建構寫成腳本的主要理由之一。
if usejava('desktop')
open_system(mdl);
else
load_system(mdl);
end
% ---- 解算器與節奏 ----
set_param(mdl, 'SolverType', 'Fixed-step', ...
'Solver', 'FixedStepDiscrete', ...
'FixedStep', num2str(dt), ...
'StartTime', '0', ...
'StopTime', num2str(t(end)), ...
'EnablePacing', 'on', ...
'PacingRate', '1');
% ---- 區塊 1:From Workspace(餵 plant 軌跡)----
src = [mdl '/Plant'];
add_block('simulink/Sources/From Workspace', src);
set_param(src, 'VariableName', 'grpc_link_ws', ...
'SampleTime', num2str(dt), ...
'Interpolate', 'off', ...
'OutputAfterFinalValue', 'Holding final value');
% ---- 區塊 2:Clock(明確的模擬時間)----
clk = [mdl '/SimTime'];
% Clock 的取樣由解算器的固定步長決定,不必另外設參數
%(它的 'Decimation' 只影響顯示,而且名字不是 'DecimationFactor' ——
% 寫錯會在 add_block 之後才炸,訊息只說「沒有這個參數」)。
add_block('simulink/Sources/Clock', clk);
% ---- 區塊 3:MATLAB Function(送 UDP)----
% 用 coder.extrinsic 呼叫 grpc_link_udp_send —— udpport 不能生成程式碼,
% 但一般模擬模式下 extrinsic 呼叫沒問題。**這也是這個模型不能
% 用來生成程式碼的原因**,而它本來就不是為此而生。
%
% `nbytes = 0;` 那一行不是多餘的:extrinsic 呼叫回來的是 mxArray,
% 要先把變數的型別與尺寸定下來,它才知道怎麼轉回來。
fcn = [mdl '/SendUDP'];
add_block('simulink/User-Defined Functions/MATLAB Function', fcn);
body = sprintf([ ...
'function nbytes = SendUDP(t, u)\n' ...
'%%#codegen\n' ...
'coder.extrinsic(''grpc_link_udp_send'');\n' ...
'nbytes = 0;\n' ...
'nbytes = grpc_link_udp_send(t, u(1), u(2), u(3));\n' ...
'end\n']);
set_matlab_function_body(fcn, body);
% ---- 區塊 4:To Workspace(留一份在 MATLAB 這側好對照)----
% **取法**:`sim()` 回傳的是 Simulink.SimulationOutput,這個變數掛在
% 它身上,不會自動丟進 base workspace:
%
% simout = sim('grpc_link_sim');
% logged = simout.grpc_link_out; % <- 不是 evalin('base', ...)
%
% 寫成 evalin('base', ...) 會拿到
% `Unrecognized function or variable 'grpc_link_out'`,
% 而那時候模型其實已經跑完、封包也已經送出去了 ——
% 看起來像模擬失敗,其實只是取值的地方錯了。
dst = [mdl '/Logged'];
add_block('simulink/Sinks/To Workspace', dst);
set_param(dst, 'VariableName', 'grpc_link_out', ...
'SaveFormat', 'Array', ...
'SampleTime', num2str(dt));
% SendUDP 的第 1 個輸入是時間(Clock),第 2 個是狀態(From Workspace)
add_line(mdl, 'SimTime/1', 'SendUDP/1', 'autorouting', 'on');
add_line(mdl, 'Plant/1', 'SendUDP/2', 'autorouting', 'on');
add_line(mdl, 'SendUDP/1', 'Logged/1', 'autorouting', 'on');
Simulink.BlockDiagram.arrangeSystem(mdl);
if do_save
save_system(mdl, [mdl '.slx']);
fprintf('已存 %s.slx(注意:二進位,建議仍以本腳本為準)\n', mdl);
end
fprintf('模型 %s 已建好。先啟動 Python 橋接再按 Run:\n', mdl);
fprintf(' python grpc_link_simulink.py --udp\n');
end
function set_matlab_function_body(blk, body)
% 把程式碼寫進 MATLAB Function 區塊。
% 這段 API 很少見,而且**沒有簡單的 set_param 可用** ——
% 要透過 Stateflow 的根物件去找到那個區塊對應的 EML chart。
rt = sfroot();
h = rt.find('-isa', 'Stateflow.EMChart', '-and', 'Path', blk);
if isempty(h)
error('找不到 %s 的 MATLAB Function chart', blk);
end
h.Script = body;
end
grpc_link/grpc_link_sixdof.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 六自由度飛行動力學,以及怎麼驗它。
## 這一節補的缺口
前面所有的 plant 都是**三自由度協調轉彎運動學**:給坡度角就轉,
沒有氣動力、沒有質量、沒有操縱面、沒有失速。那對「介接對不對」
是夠的(而且刻意如此),但對「這架飛機飛不飛得起來」完全沒有話語權。
這一支補上六自由度:三個平移 + 三個轉動,含氣動力與力矩、
四元數姿態、以及操縱面。
## 係數是**代表值**,不是某一架真實飛機的量測
這一點必須先講清楚:下面那組氣動係數是小型固定翼無人機的**代表值**,
用來讓動力學的形狀正確,**不是任何一架真機的風洞或飛測結果**。
所以本檔驗的是:
- **動力學自洽**(配平殘差、四元數範數、能量)
- **模態結構正確**(短週期 / 長週期的存在、量級與阻尼)
而**不是**「這架飛機的操穩品質如何」—— 那需要真實係數,本檔沒有。
把代表值的結果講成真機性能,是這一類模型最常見的誤用。
## 怎麼驗一個 6DOF:三層,由硬到軟
**第一層:數學恆等式(對就是對)**
- 四元數範數在積分過程中必須維持 1。掉了就是積分或正規化壞了。
- 無動力滑翔時,總能量的變化率必須等於阻力作功:`dE/dt = -D·V`。
這是能量守恆的直接後果,沒有容差可言(除了積分誤差)。
**第二層:配平(可以解到殘差為零)**
給定空速與高度,解出讓合力與合力矩都為零的
(攻角、升降舵、推力)。**殘差不是「小」,是解到機器精度。**
解不出來就表示那個飛行狀態不存在 —— 那本身就是有用的資訊。
**第三層:模態(對外部的解析近似)**
在配平點附近做**數值線性化**,取特徵值。縱向應該出現兩組共軛複根:
短週期 快、阻尼重 ω_sp ≈ sqrt(-M_α)
長週期 慢、阻尼輕 ω_ph ≈ g·√2 / V (Lanchester 近似)
這兩個近似很粗,所以判準是**量級與結構**,不是小數點:
兩組複根要存在、短週期要比長週期快一個數量級以上、
長週期的阻尼比要明顯小於短週期。
**這一層才是真正的外部對照** —— 前兩層只證明程式自洽,
第三層才把結果對上飛行力學的已知結論。
## 為什麼不用 MATLAB 的 Aerospace Blockset
這台機器上它的授權是有的,而且它做這件事很合適。不用它的理由只有一個:
**本章的驗證段刻意不依賴任何授權。** 六自由度的數學不長,
比省下兩百行程式碼重要。
要換成 Aerospace Blockset 的話,上面三層驗證一條都不用改 ——
它們驗的是結果,不是實作。
python grpc_link_sixdof.py 配平 + 模態 + 能量檢查
python grpc_link_sixdof.py --sweep 掃不同空速的配平與模態
"""
import io
import json
import math
import os
import sys
import numpy as np
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
SIXDOF_OUT = os.path.join(HERE, "grpc_link_sixdof.json")
G0 = 9.80665
RHO = 1.225 # 海平面標準大氣密度 kg/m^3
# ---------------------------------------------------------------------------
# 小型固定翼無人機的**代表性**氣動與質量參數。
# **不是任何一架真機的量測值。** 它們的作用是讓動力學的形狀正確
#(靜穩定為負的 Cm_alpha、正常的阻尼導數),好讓模態出現在該出現的地方。
# 換成真實係數時,底下的驗證一條都不用改。
# ---------------------------------------------------------------------------
MASS = 5.0 # kg
S_REF = 0.40 # 機翼面積 m^2
B_REF = 2.00 # 翼展 m
C_REF = 0.22 # 平均氣動弦長 m
IXX, IYY, IZZ = 0.28, 0.36, 0.58 # kg m^2
CL0, CLA, CLQ, CLDE = 0.25, 5.0, 7.0, 0.35
CD0, K_IND = 0.030, 0.060
CM0, CMA, CMQ, CMDE = 0.030, -0.90, -12.0, -1.20
CYB = -0.30
CLB, CLP, CLR, CLDA = -0.080, -0.450, 0.090, 0.180 # 滾轉
CNB, CNP, CNR, CNDR = 0.060, -0.020, -0.060, -0.070 # 偏航
T_MAX = 25.0 # 最大推力 N
def quat_to_dcm(q):
"""四元數(w, x, y, z)轉方向餘弦矩陣(機體 <- 地理)。"""
w, x, y, z = q
return np.array([
[1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)],
[2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)],
[2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)]])
def euler_to_quat(phi, theta, psi):
cp, sp = math.cos(phi / 2), math.sin(phi / 2)
ct, st = math.cos(theta / 2), math.sin(theta / 2)
cs, ss = math.cos(psi / 2), math.sin(psi / 2)
return np.array([cp * ct * cs + sp * st * ss,
sp * ct * cs - cp * st * ss,
cp * st * cs + sp * ct * ss,
cp * ct * ss - sp * st * cs])
def aero_angles(u, v, w):
"""回傳 (空速, 攻角 alpha, 側滑角 beta)。"""
va = math.sqrt(u * u + v * v + w * w)
if va < 1e-6:
return 0.0, 0.0, 0.0
return va, math.atan2(w, u), math.asin(max(-1.0, min(1.0, v / va)))
def aero_force_body(state, ctrl):
"""**只有氣動力**的機體軸分量(N),不含重力與推力。
抽出來是為了讓能量檢查能用**功-能定理**寫:
`dE/dt = (氣動力 + 推力) · 速度`。
能量檢查若自己重算升力與阻力係數,很容易漏掉 `CLQ` 的俯仰率項 ——
滑翔中長週期振盪讓 q 不為零,於是檢查與模型算的阻力差 0.35%。
那種差**不隨步長收斂**,會被步長細化抓出來:
它不是離散誤差,是檢查本身寫錯了。
"""
u, v, w, p, q, r = state[0:6]
de, da, dr, _thr = ctrl
va, alpha, beta = aero_angles(u, v, w)
if va < 1e-6:
return np.zeros(3)
qbar = 0.5 * RHO * va * va
p_h = p * B_REF / (2 * va)
q_h = q * C_REF / (2 * va)
r_h = r * B_REF / (2 * va)
cl = CL0 + CLA * alpha + CLQ * q_h + CLDE * de
cd = CD0 + K_IND * cl * cl
cy = CYB * beta
ca, sa = math.cos(alpha), math.sin(alpha)
return np.array([qbar * S_REF * (cl * sa - cd * ca),
qbar * S_REF * cy,
qbar * S_REF * (-cl * ca - cd * sa)])
def forces_moments(state, ctrl):
"""回傳 (機體軸合力 N, 機體軸合力矩 N·m)。
`state` = [u, v, w, p, q, r, qw, qx, qy, qz, xn, xe, xd]
`ctrl` = [升降舵, 副翼, 方向舵, 油門(0-1)](角度為弧度)
"""
u, v, w, p, q, r = state[0:6]
quat = state[6:10]
de, da, dr, thr = ctrl
va, alpha, beta = aero_angles(u, v, w)
qbar = 0.5 * RHO * va * va
if va < 1e-6:
qbar = 0.0
p_h = p * B_REF / (2 * va) if va > 1e-6 else 0.0
q_h = q * C_REF / (2 * va) if va > 1e-6 else 0.0
r_h = r * B_REF / (2 * va) if va > 1e-6 else 0.0
# **氣動力只算一次,在 aero_force_body 裡。** 在兩個地方各算一次,
# 就會出現兩邊漏掉不同項的情況,而且兩邊各自都看起來合理。
f_aero = aero_force_body(state, ctrl)
dcm = quat_to_dcm(quat)
g_body = dcm @ np.array([0.0, 0.0, G0 * MASS])
f = f_aero + g_body + np.array([T_MAX * thr, 0.0, 0.0])
l = qbar * S_REF * B_REF * (CLB * beta + CLP * p_h + CLR * r_h + CLDA * da)
m = qbar * S_REF * C_REF * (CM0 + CMA * alpha + CMQ * q_h + CMDE * de)
n = qbar * S_REF * B_REF * (CNB * beta + CNP * p_h + CNR * r_h + CNDR * dr)
return f, np.array([l, m, n])
def derivs(state, ctrl):
"""狀態導數。剛體六自由度的標準式。"""
u, v, w, p, q, r = state[0:6]
quat = state[6:10]
f, mom = forces_moments(state, ctrl)
du = f[0] / MASS - q * w + r * v
dv = f[1] / MASS - r * u + p * w
dw = f[2] / MASS - p * v + q * u
dp = (mom[0] + (IYY - IZZ) * q * r) / IXX
dq = (mom[1] + (IZZ - IXX) * r * p) / IYY
dr = (mom[2] + (IXX - IYY) * p * q) / IZZ
qw, qx, qy, qz = quat
dq_ = 0.5 * np.array([-qx * p - qy * q - qz * r,
qw * p + qy * r - qz * q,
qw * q + qz * p - qx * r,
qw * r + qx * q - qy * p])
dcm = quat_to_dcm(quat)
vel_ned = dcm.T @ np.array([u, v, w])
return np.concatenate(([du, dv, dw, dp, dq, dr], dq_, vel_ned))
def trim(v_target, h_m=150.0):
"""解水平等速直線飛行的配平。回傳 (state, ctrl, 殘差)。
**殘差要解到機器精度,不是「夠小」。** 解不出來就表示那個飛行狀態
不存在(例如空速低於失速、或推力不足)—— 那本身就是有用的資訊,
不該用一個大容差蓋過去。
"""
from scipy.optimize import fsolve
def residual(x):
alpha, de, thr = x
u = v_target * math.cos(alpha)
w = v_target * math.sin(alpha)
# 水平飛行:俯仰角 = 攻角(航跡角為零)
quat = euler_to_quat(0.0, alpha, 0.0)
st = np.concatenate(([u, 0.0, w, 0.0, 0.0, 0.0], quat,
[0.0, 0.0, -h_m]))
d = derivs(st, [de, 0.0, 0.0, thr])
return [d[0], d[2], d[4]] # du, dw, dq 都要為零
sol, info, ier, msg = fsolve(residual, [0.05, -0.02, 0.4],
full_output=True)
alpha, de, thr = sol
res = residual(sol)
u = v_target * math.cos(alpha)
w = v_target * math.sin(alpha)
quat = euler_to_quat(0.0, alpha, 0.0)
st = np.concatenate(([u, 0.0, w, 0.0, 0.0, 0.0], quat, [0.0, 0.0, -h_m]))
return st, [de, 0.0, 0.0, thr], {"alpha": alpha, "elevator": de,
"throttle": thr,
"residual": [float(x) for x in res],
"max_abs_residual": max(abs(x) for x in res),
"converged": bool(ier == 1), "msg": msg}
def linearise(state, ctrl, eps=1e-6):
"""在配平點做**數值**線性化。回傳縱向 4x4 的 A 矩陣。
縱向狀態取 [u, w, q, theta]。用數值 Jacobian 而不是手推導數:
手推容易在某一項漏掉,而漏掉的那一項通常剛好是決定模態的那一項。
數值的代價是精度,但用中央差分已經足夠分辨兩組模態。
"""
idx = [0, 2, 4] # u, w, q
n = 4
a = np.zeros((n, n))
base = derivs(state, ctrl)
theta0 = 2.0 * math.asin(max(-1.0, min(1.0, state[8])))
def f_of(x):
st = state.copy()
st[0], st[2], st[4] = x[0], x[1], x[2]
st[6:10] = euler_to_quat(0.0, x[3], 0.0)
d = derivs(st, ctrl)
# dtheta = q(小擾動、水平飛行時)
return np.array([d[0], d[2], d[4], st[4]])
x0 = np.array([state[0], state[2], state[4], theta0])
f0 = f_of(x0)
for j in range(n):
xp = x0.copy()
xm = x0.copy()
h = eps * max(1.0, abs(x0[j]))
xp[j] += h
xm[j] -= h
a[:, j] = (f_of(xp) - f_of(xm)) / (2 * h)
return a, base, f0
def modes(a):
"""由 A 矩陣取模態。回傳按頻率排序的 [(ω_n, ζ), ...]。"""
ev = np.linalg.eigvals(a)
out = []
seen = set()
for lam in ev:
key = (round(lam.real, 6), round(abs(lam.imag), 6))
if key in seen:
continue
seen.add(key)
wn = abs(lam)
zeta = -lam.real / wn if wn > 1e-12 else 0.0
out.append((float(wn), float(zeta), complex(lam)))
out.sort(key=lambda t: t[0])
return out
def integrate(state, ctrl, dt, n, renorm=True):
"""RK4 積分。回傳 (軌跡, 四元數範數的最大偏差)。
`renorm=False` 時**不**正規化四元數 —— 那是為了驗「範數本來就守恆」,
而不是「我們每一步都把它扳回來」。兩者的差別很重要:
每步正規化會把積分誤差藏起來,於是範數檢查變成檢查那行正規化程式碼。
"""
tr = [state.copy()]
worst = 0.0
st = state.copy()
for _ in range(n):
k1 = derivs(st, ctrl)
k2 = derivs(st + 0.5 * dt * k1, ctrl)
k3 = derivs(st + 0.5 * dt * k2, ctrl)
k4 = derivs(st + dt * k3, ctrl)
st = st + (dt / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)
nq = np.linalg.norm(st[6:10])
worst = max(worst, abs(nq - 1.0))
if renorm:
st[6:10] /= nq
tr.append(st.copy())
return tr, worst
def energy(state):
"""總能量(J):動能 + 位能。位置的第三個分量是 NED 的下,所以高度是 -xd。"""
v2 = state[0] ** 2 + state[1] ** 2 + state[2] ** 2
h = -state[12]
return 0.5 * MASS * v2 + MASS * G0 * h
def glide_energy_check(v0=18.0, n=400, dt=0.02):
"""無動力滑翔:`dE/dt` 必須等於阻力作功 `-D·V`。
這是能量守恆的直接後果 —— 對就是對,沒有容差可言(除了積分誤差)。
它抓得到的是**力的符號或座標轉換寫反**這一類錯:
那種錯不會讓飛機飛得很怪,只會讓能量悄悄多出來或少掉。
"""
st, ctrl, _info = trim(v0)
ctrl = [ctrl[0], 0.0, 0.0, 0.0] # 關油門
tr, _ = integrate(st, ctrl, dt, n)
worst = 0.0
for k in range(1, len(tr) - 1):
de_dt = (energy(tr[k + 1]) - energy(tr[k - 1])) / (2 * dt)
st_k = tr[k]
# **功-能定理**:非保守力的功率 = 氣動力與推力對速度的內積。
# 不要重算係數 —— 重算就會出現檢查與模型漏掉不同項的情況。
vb = st_k[0:3]
power = float(np.dot(aero_force_body(st_k, ctrl), vb)
+ T_MAX * ctrl[3] * vb[0])
worst = max(worst, abs(de_dt - power) / max(1.0, abs(power)))
return worst
def glide_energy_order(v0=18.0, dts=(0.04, 0.02, 0.01), span_s=8.0):
"""步長細化:能量殘差必須**隨步長收斂**。
單看「殘差是 3.5e-3,很小」不能區分兩件事:
離散誤差,還是模型真的有一點點不守恆。兩者都會給一個小數字。
但它們對步長的反應完全不同:**離散誤差會隨步長縮小而下降,
模型錯誤不會。** 所以這裡掃三個步長,回報收斂比。
中央差分的導數估計是二階,所以步長減半時殘差應該約掉到四分之一。
"""
out = []
for dt in dts:
n = int(span_s / dt)
out.append((dt, glide_energy_check(v0=v0, n=n, dt=dt)))
ratios = [out[i][1] / out[i + 1][1] for i in range(len(out) - 1)
if out[i + 1][1] > 0]
return out, ratios
def main():
sweep = "--sweep" in sys.argv
print("=" * 78)
print(" 六自由度飛行動力學 — 以及怎麼驗它")
print("=" * 78)
print(" **係數是小型固定翼的代表值,不是任何一架真機的量測。**")
print(" 所以驗的是動力學自洽與模態結構,不是這架飛機的操穩品質。")
speeds = [14.0, 18.0, 22.0, 26.0] if sweep else [18.0]
rows = []
for v0 in speeds:
st, ctrl, info = trim(v0)
a, _b, _f = linearise(st, ctrl)
md = modes(a)
osc = [(wn, z) for wn, z, lam in md if abs(lam.imag) > 1e-6]
rows.append({"v": v0, "trim": info,
"modes": [(w, z) for w, z, _l in md],
"oscillatory": osc})
print()
print(" 空速 %.1f m/s" % v0)
print(" 配平:攻角 %.3f deg,升降舵 %.3f deg,油門 %.1f%%"
% (math.degrees(info["alpha"]), math.degrees(info["elevator"]),
info["throttle"] * 100))
print(" 殘差最大 %.3e(收斂 %s)—— **要解到機器精度,不是「夠小」**"
% (info["max_abs_residual"], info["converged"]))
if len(osc) >= 2:
(w1, z1), (w2, z2) = osc[0], osc[-1]
ph_approx = G0 * math.sqrt(2.0) / v0
sp_approx = math.sqrt(abs(0.5 * RHO * v0 * v0 * S_REF * C_REF
* CMA / IYY))
print(" 長週期 ω %.4f rad/s(T %.1f s)ζ %.4f"
" Lanchester 近似 %.4f" % (w1, 2 * math.pi / w1, z1,
ph_approx))
print(" 短週期 ω %.4f rad/s(T %.1f s)ζ %.4f"
" 近似 sqrt(-Mα) %.4f" % (w2, 2 * math.pi / w2, z2,
sp_approx))
print(" 兩者頻率比 %.1f 倍;阻尼比 %.4f vs %.4f"
% (w2 / w1, z1, z2))
else:
print(" **找不到兩組共軛複根** —— 縱向模態的結構不對")
print()
print(" 無動力滑翔的能量檢查:dE/dt vs -D·V")
print(" 這是能量守恆的直接後果。它抓的是**力的符號或座標轉換寫反**")
print(" 那一類錯 —— 那種錯不會讓飛機飛得很怪,只會讓能量悄悄跑掉。")
conv, ratios = glide_energy_order()
for dt, e in conv:
print(" 步長 %.3f s -> 最大相對差 %.3e" % (dt, e))
if ratios:
print(" 收斂比 %s(中央差分是二階,步長減半應約掉到 1/4)"
% ", ".join("%.2f" % r for r in ratios))
print(" **會收斂才表示那是離散誤差,不是模型不守恆** ——")
print(" 兩者都只給一個小數字,但對步長的反應完全不同。")
worst = conv[-1][1]
st, ctrl, _ = trim(18.0)
_tr, qerr = integrate(st, ctrl, 0.01, 2000, renorm=False)
print()
print(" 四元數範數偏差(2000 步 RK4,**不做每步正規化**):%.3e" % qerr)
print(" 不正規化是刻意的:每步扳回來會把積分誤差藏起來,")
print(" 於是這項檢查就變成在檢查那一行正規化程式碼。")
io.open(SIXDOF_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"cases": rows, "glide_energy_rel": worst,
"glide_convergence": conv, "glide_ratios": ratios,
"quat_norm_err": qerr}, ensure_ascii=False,
indent=1, default=str) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(SIXDOF_OUT))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_step.m
function s2 = grpc_link_step(s1, bank_deg, dt, V, gamma_deg)
% GRPC_LINK_STEP 協調轉彎運動學的**單步**推進。
%
% 狀態向量 s = [psi_rad ; lat_deg ; lon_deg ; h_m]。
%
% ## 這支為什麼要存在
%
% 雙向(閉迴路)的 Simulink 模型需要把 plant 做成模型裡的狀態,
% 而不是像單向模型那樣用 From Workspace 餵預算好的軌跡 ——
% 因為閉迴路的重點就是「回饋會改變接下來要飛去哪」。
%
% 直覺的做法是在 Simulink 裡再寫一份方程。**那會很糟**:
% 教材裡就會有三份 plant(Python 一份、grpc_link_plant.m 一份、
% Simulink 一份),它們的離散化只要有一點不同就對不上,
% 於是「Simulink 接進 STK 對不對」這個問題,會被
% 「三份 plant 為什麼不一樣」淹掉。
%
% 所以把**單步**抽出來:`grpc_link_plant.m` 迴圈呼叫它,
% Simulink 的 PlantStep 區塊也呼叫它。方程只有一份,
% 「兩邊一致」因此是**結構上保證的**,不是每次都要重新量的巧合。
%
% 顯式歐拉、平板地球,與 Python 側的 `grpc_link_realtime.plant()` 逐字對應。
% 更新順序也一樣:先用**舊的** psi 算位移,再更新 psi;
% 經度除的是**更新後**的緯度。這兩點都不是隨便寫的 ——
% 換順序在兩分鐘的尺度上只差幾公分,但既然要逐點對照,
% 就沒有理由留下一個已知的不一致。
%
% 用法:
% s = [0; 24.80; 121.00; 1500];
% s = grpc_link_step(s, 15.0, 1.0, 120.0, 2.0);
G0 = 9.80665; % m/s^2 —— 與 Python 側同值
RE_M = 6378137.0; % m —— WGS-84 赤道半徑
psi = s1(1);
la = s1(2);
lo = s1(3);
h = s1(4);
cg = cos(deg2rad(gamma_deg));
sg = sin(deg2rad(gamma_deg));
dn = V * cg * cos(psi) * dt; % 北向位移 m(用**舊的** psi)
de = V * cg * sin(psi) * dt; % 東向位移 m
la = la + rad2deg(dn / RE_M);
% 注意:除的是**更新後**的 la,與 Python 側同順序
lo = lo + rad2deg(de / (RE_M * cos(deg2rad(la))));
h = h + V * sg * dt;
psi = psi + (G0 / V) * tan(deg2rad(bank_deg)) * dt;
s2 = [psi; la; lo; h];
end
grpc_link/grpc_link_stk_verify.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — **需要 STK** 的驗證段。
## 為什麼要分成兩支
`grpc_link_verify.py` 刻意完全不碰 STK:協定常數、API 名稱、診斷邏輯、
封包格式、DOP 的數學恆等式 —— 這些都不在 STK 裡,綁上授權只會讓
最該常跑的檢查變成最少跑的。
但那一支也因此**驗不到 STK 自己**。這一支補的正是那一塊:
STK 的 GPS almanac 給的星座對不對?
Realtime 注入的點,讀回來還是同一條軌跡嗎?
設下去的存取約束,STK 真的照做了嗎?
閉迴路的回饋,真的改變了飛行嗎?
最後一項是這一支的重點。前面三項是「STK 照做了嗎」,
最後一項是「**這整件事有沒有意義**」—— 如果開迴路與閉迴路的軌跡
一模一樣,那前面全部通過也沒有用。
## 判準一律對外部來源,不對 STK 自己
GPS 星座的檢查全部對**公開的標稱值**(半長軸 26,560 km、傾角 55 度、
週期 11 小時 58 分),不是拿 STK 算出來的東西去釘 STK 算出來的東西。
週期更是**從星曆量出來的**(看半徑的極值間隔),不是從半長軸反推 ——
從半長軸反推只會驗到克卜勒定律,驗不到 STK 的星曆。
執行:
python grpc_link_stk_verify.py
離開碼 0 = 全部通過,1 = 有項目對不上
這一支會自己啟一個 STK Runtime(無介面),跑完關掉。
"""
import math
import os
import sys
import numpy as np
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
sys.path.insert(0, os.path.join(HERE, os.pardir, "tools"))
from stk_verify import Verifier # noqa: E402
import grpc_link_closedloop as CL # noqa: E402
import grpc_link_comm as CM # noqa: E402
import grpc_link_gps as GPS # noqa: E402
import grpc_link_terrain as TR # noqa: E402
import grpc_link_realtime as R # noqa: E402
# 公開標稱值(外部來源,不是本教材算的)
GPS_INC_DEG = 55.0
N_PRN_EXPECTED = 24 # 標稱星座是 24 槽;實際 almanac 通常更多
def measure_period(times, radii):
"""從半徑的極小值間隔量軌道週期(秒)。
**不從半長軸反推** —— 那只會驗到克卜勒定律,驗不到 STK 的星曆。
GPS 軌道近圓,半徑起伏很小,但仍然有近地點;取連續兩個極小值的間隔。
"""
idx = [i for i in range(1, len(radii) - 1)
if radii[i] < radii[i - 1] and radii[i] <= radii[i + 1]]
if len(idx) < 2:
return None
gaps = [times[idx[i + 1]] - times[idx[i]] for i in range(len(idx) - 1)]
return float(np.median(gaps))
def main():
try:
from ansys.stk.core.stkruntime import STKRuntime
from ansys.stk.core.stkobjects import STKObjectType, PropagatorType
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90,
no_graphics=True)
except Exception as e: # noqa: BLE001
print("接不上 STK:%s" % str(e)[:120])
print("先跑 grpc_link_probe.py 找出斷在哪一層。")
return 1
root = app.new_object_root()
root.new_scenario("GrpcLinkStkVerify")
scen = root.current_scenario
# 一天:要量得到 GPS 的週期(約 11 小時 58 分),至少得涵蓋兩圈
ts = R.timestamps([0.0, 86400.0])
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"' % (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
v = Verifier(root)
# ---------------- 1. STK 的 GPS almanac ----------------
const = GPS.GpsConstellation(root, scen, step=300.0).build().fetch()
v.expect_true("STK 的 GPS almanac 至少給滿標稱的 %d 槽" % N_PRN_EXPECTED,
len(const.prns) >= N_PRN_EXPECTED,
"實得 %d 個 PRN" % len(const.prns))
r_mean, r_std = const.orbit_radius_km()
v.expect("GPS 軌道半徑 vs 公開標稱 26,560 km", r_mean, GPS.GPS_SMA_KM,
tol=0.01 * GPS.GPS_SMA_KM, unit=" km")
# 近圓軌道:半徑起伏應遠小於半徑本身
v.expect_true("GPS 軌道近圓(半徑起伏 < 2%)",
r_std / r_mean < 0.02,
"起伏 %.1f km / %.1f km = %.3f%%"
% (r_std, r_mean, 100 * r_std / r_mean))
# 週期:**從星曆量**,對公開的 11 小時 58 分
radii = np.linalg.norm(const.eph[:, 0, :], axis=1)
per = measure_period(const.times, radii)
if per is None:
v.custom("GPS 軌道週期(由星曆量)", None, "極值不足,無法量")
else:
v.expect("GPS 軌道週期(由星曆量)vs 公開 11h58m", per,
GPS.GPS_PERIOD_S, tol=600.0, unit=" s")
# 傾角:由星曆的 z 分量極值推得 —— 同樣不問 STK 的根數,自己量
z = const.eph[:, 0, 2]
inc = math.degrees(math.asin(min(1.0, float(np.max(np.abs(z))) / r_mean)))
v.expect("GPS 軌道傾角(由星曆量)vs 公開 55 度", inc, GPS_INC_DEG,
tol=3.0, unit=" deg")
# ---------------- 2. Realtime 注入的往返完整性 ----------------
ac, route, pts = R.make_realtime_aircraft(root, "VerifyUAV")
v.expect_true("飛機的 route_type 已切成 Realtime",
int(ac.route_type) == int(PropagatorType.REAL_TIME),
"route_type=%s" % ac.route_type)
t, lat, lon, alt = R.plant(n=60)
stamps = R.timestamps(t)
pts.add_position_batch(stamps, list(lat), list(lon), list(alt))
route.propagate()
# **只讀注入涵蓋的那一段。** 場景是 24 小時,注入只有 60 秒 ——
# 不指定區間的話會連沒有星曆的部分一起取回來(實測 1860 列),
# 而擬圓半徑會從 5,480 m 變成 2,951,519 m。
rb_lat, rb_lon = R.readback(ac, scen, start=stamps[0], stop=stamps[-1])
v.expect("注入 60 點,讀回列數", len(rb_lat), 60, tol=1)
# 逐點比對:注入什麼就該讀回什麼。**這一項是整支的地基** ——
# 它不過,後面所有「STK 算出來的東西」都沒有意義。
worst = 0.0
for i in range(min(len(rb_lat), len(lat))):
dlat = (rb_lat[i] - lat[i]) * 111320.0
dlon = (rb_lon[i] - lon[i]) * 111320.0 * math.cos(math.radians(lat[i]))
worst = max(worst, math.hypot(dlat, dlon))
v.expect_true("注入的軌跡讀回來逐點一致(< 1 m)", worst < 1.0,
"最大差 %.4f m" % worst)
# 獨立對照:STK 裡的軌跡去對**外部閉合式**,不是對 plant 自己
r_stk = R.fitted_radius(rb_lat, rb_lon)
v.expect("STK 內軌跡的擬圓半徑 vs 閉合式 V²/(g tanφ)",
r_stk, R.turn_radius(), tol=0.02 * R.turn_radius(), unit=" m")
# ---------------- 3. 存取約束:設下去 STK 有照做嗎 ----------------
fac = scen.children.new(STKObjectType.FACILITY, "VerifyGS")
fac.position.assign_geodetic(CL.GS_LAT, CL.GS_LON, CL.GS_ALT_KM)
acc0 = ac.get_access_to_object(fac)
acc0.compute_access()
n0 = _n_intervals(acc0)
from ansys.stk.core.stkobjects import AccessConstraintType
c = fac.access_constraints.add_constraint(
AccessConstraintType.ELEVATION_ANGLE)
c.enable_minimum = True
c.minimum = 45.0 # 刻意設得很高,一定要看得出差別
# 回讀:ACK 不等於生效,這是這個教材庫從頭到尾的那一條
got = fac.access_constraints.get_active_constraint(
AccessConstraintType.ELEVATION_ANGLE)
v.expect("仰角約束回讀", float(got.minimum), 45.0, tol=1e-6, unit=" deg")
acc1 = ac.get_access_to_object(fac)
acc1.compute_access()
n1 = _n_intervals(acc1)
# **失敗注入式的檢查**:約束若沒生效,兩次的可見窗會一模一樣。
v.expect_true("加上 45 度仰角約束後,可見窗確實縮短",
n1 < n0 or (n0 == 0 and n1 == 0),
"無約束 %d 段 -> 45 度約束 %d 段" % (n0, n1))
v.expect_true("45 度約束下可見窗比無約束少(不是兩者都空)",
n0 > 0, "無約束時有 %d 段" % n0)
# ---------------- 4. 閉迴路真的改變了飛行 ----------------
# 這一項是整支的重點:前面全部通過但軌跡沒變,那就白做了。
# **步數要長到 GPS 真的掉下來過。** 實測 2 小時裡只有 3.6% 的時刻
# 定位品質不足,所以跑 200 步(3 分鐘)兩條軌跡會一模一樣 ——
# 那不是回饋沒作用,是還沒輪到它作用 —— 這一項很容易因此誤判成紅的。
n_fly = GPS.N_STEPS
open_track = _fly(lambda _ok: CL.BANK_DEG, n=n_fly)
closed_track = _fly(None, n=n_fly, const=const)
d = _max_offset(open_track, closed_track)
v.expect_true("閉迴路的軌跡與開迴路**不同**(回饋真的起作用)",
d > 1.0, "最大差 %.1f m" % d)
# 而且差距要夠大到不可能是數值雜訊
v.expect_true("差距遠大於數值雜訊(> 100 m)", d > 100.0,
"最大差 %.1f m" % d)
# ---------------- 5. STK 的可見顆數 vs 本地 DOP 幾何 ----------------
sv = const.at(0.0)
nvis, dop, _elev = GPS.visible_and_dop(R.LAT0, R.LON0, 1.5, sv,
GPS.MASK_DEG)
v.expect_true("在 STK 的星座下,起點可見顆數 >= 4(否則無解)",
nvis >= GPS.MIN_SVS, "可見 %d 顆" % nvis)
if dop:
ok_id, (e1, e2) = GPS.dop_identities(dop)
v.expect_true("用 STK 星曆算出的 DOP 仍滿足恆等式", ok_id,
"殘差 %.2e / %.2e" % (e1, e2))
v.expect_between("用 STK 星曆算出的 GDOP(遮蔽 %.0f 度)"
% GPS.MASK_DEG, dop["GDOP"], 1.5, 30.0)
# ---------------- 6. RF 鏈路:STK 的路徑損耗 vs 閉合式 ----------------
# 這是整支裡**最強的一項對照**:自由空間路徑損耗有閉合式,
# 而鏈路預算最容易出錯的地方(單位、方向、dB 與 dBm)
# 不會讓數字變離譜,只會讓它偏十幾 dB —— 看起來還很合理。
_uav, _gs, ack_c, nack_c, fails_c = CM.build_link(root, scen,
uav_name="VerifyUAV",
gs_name="VerifyCommGS")
v.expect("建鏈路的 Connect 指令全數 ACK", nack_c, 0, tol=0)
if nack_c:
for c, m in fails_c[:3]:
print(" NACK %s -> %s" % (c[:70], m))
else:
cols, nrow = CM.link_budget(
root, "%s/Sensor/UavAnt/Transmitter/UavTx" % _uav,
"%s/Sensor/GsAnt/Receiver/GsRx" % _gs,
scen.start_time, stamps[-1], 10.0)
v.expect_true("鏈路預算讀得到資料", nrow > 0, "%d 列" % nrow)
if nrow and "Range" in cols and "Free Space Loss" in cols:
worst = 0.0
for i in range(nrow):
worst = max(worst, abs(abs(float(cols["Free Space Loss"][i]))
- CM.fspl_db(float(cols["Range"][i]))))
v.expect_true("STK 的自由空間損耗 = 20log10(4 pi d f / c)",
worst < 0.05, "最大差 %.5f dB(%d 列)" % (worst, nrow))
rr = [float(x) for x in cols["Range"]]
v.expect_true("鏈路距離落在合理範圍(1-100 km)",
1.0 <= min(rr) and max(rr) <= 100.0,
"%.2f - %.2f km" % (min(rr), max(rr)))
# ---------------- 7. 地形(有 DEM 才驗)----------------
dem = TR.dem_path()
if not dem or not os.path.isfile(dem):
v.custom("地形載入與基準面偏移", None,
"沒有設 GRPC_LINK_DEM,略過(本教材不附 DEM)")
else:
try:
from ansys.stk.core.stkobjects import (TerrainFileType,
AltitudeReferenceType)
arr, tf, bounds, _crs = TR.read_dem(dem)
pts, _tried = TR.sample_points(arr, tf, bounds, n=80)
v.expect_true("DEM 取得到夠多有高度的取樣點", len(pts) >= 20,
"%d 點" % len(pts))
tobj, earth = TR.load_terrain(root, dem,
TerrainFileType.TIFF_TERRAIN_FILE)
# STK 回報的涵蓋範圍要對得上檔案本身(容許半格)
half = abs(tf.a)
v.expect("STK 回報的西邊界 vs 檔案", tobj.southwest_longitude,
bounds.left, tol=half, unit=" deg")
v.expect("STK 回報的北邊界 vs 檔案", tobj.northeast_latitude,
bounds.top, tol=half, unit=" deg")
# 同基準來回:平均差要接近 0(STK 會跟自己的宣告一致)
_d, st = TR.compare(earth, pts, arr, tf,
AltitudeReferenceType.ELLIPSOID)
v.expect_true("宣告橢球、用橢球回讀 -> 平均差接近 0",
abs(st["mean"]) < 0.5,
"平均 %.3f m,標準差 %.3f m" % (st["mean"], st["std"]))
# 換一個基準回讀 -> 應該出現**系統性**偏移(大地起伏)
_d2, st2 = TR.compare(earth, pts, arr, tf,
AltitudeReferenceType.MEAN_SEA_LEVEL)
v.expect_true("換基準回讀 -> 出現系統性偏移(大地起伏)",
abs(st2["mean"]) > 3.0,
"平均 %.3f m" % st2["mean"])
# **而且偏移要遠大於內插雜訊**,否則分不出是選錯還是雜訊
v.expect_true("偏移遠大於內插雜訊(至少 3 倍)",
abs(st2["mean"]) > 3.0 * st2["std"],
"偏移 %.2f m vs 雜訊 %.2f m"
% (abs(st2["mean"]), st2["std"]))
except ImportError:
v.custom("地形載入與基準面偏移", None, "缺 rasterio,略過")
ok = v.report("STK gRPC 通聯測試 — STK 驗證段")
print()
print(" 這一支驗的是 **STK 自己**:almanac 給的星座、Realtime 注入的")
print(" 往返完整性、存取約束有沒有生效、以及閉迴路有沒有真的改變飛行。")
print(" 不需要 STK 的那 136 項在 grpc_link_verify.py。")
print(" 本段總數會變:地形那組有 DEM 時 6 項、沒有時併成 1 項 SKIP,")
print(" 所以你會看到 27 或 22 —— 上面那行印的才是這次真的跑了幾項。")
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0 if ok else 1
def _n_intervals(acc):
try:
return len(acc.computed_access_interval_times.to_array(0, -1))
except Exception: # noqa: BLE001
return 0
def _fly(law, n=200, const=None):
"""飛一段。`law` 給 None 就用 GPS 回饋的閉迴路。回傳軌跡。"""
plant = CL.Plant(dt=CL.DT)
out, ok = [], True
for _i in range(n):
bank = law(ok) if law is not None else GPS.guidance_gps(ok)
f = plant.step(bank)
out.append(f)
if const is not None:
nv, dop, _e = GPS.visible_and_dop(f[1], f[2], f[3],
const.at(f[0]), GPS.MASK_DEG)
ok = (dop is not None and nv >= GPS.MIN_SVS
and dop["HDOP"] <= GPS.HDOP_MAX)
return out
def _max_offset(a, b):
worst = 0.0
for (_t1, la1, lo1, _a1), (_t2, la2, lo2, _a2) in zip(a, b):
dlat = (la1 - la2) * 111320.0
dlon = (lo1 - lo2) * 111320.0 * math.cos(math.radians(la1))
worst = max(worst, math.hypot(dlat, dlon))
return worst
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_stkviews.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 由 STK 實際計算的資料畫成多視角圖。
## 為什麼不是「STK 的螢幕截圖」
這一支原本要做的是抓 STK 的 3D 畫面。**兩次都失敗,而且原因不同**:
1. 相機控制:`ObjectView` / `SetCameraPosition` / `ViewOffset` 全 NACK,
`View Zoom In` 會 ACK 但鏡頭不動(前一輪的紀錄)。
2. 影像匯出:`VO SnapFrame` / `Window2D SaveImg` / `SoftVTR Frame`
在 `userControl=False` 啟動的 Desktop 上全部 NACK ——
因為那個模式根本沒有開 2D/3D 視窗(實測主視窗只有 159x27 像素)。
3. 桌面擷取:硬體加速的子視窗擷出來是全黑。
要真的拿到 3D 畫面,得用 `userControl=True` 讓完整 UI 佔住整個螢幕,
再處理相機與擷取 —— 代價高,而且**產物的資訊量其實比較低**:
一張 3D 地球看起來漂亮,但它回答不了「可見窗在哪幾段」
「天空中衛星怎麼分布」「仰角什麼時候掉到門檻以下」。
所以這一支改成:**把 STK 算出來的資料畫成能回答問題的圖**。
資料是 STK 的,呈現是我們的 —— 而且每一張都對應一個具體的問題。
## 四個視角,四個問題
(a) 地面軌跡 + 可見段 任務走到哪裡時鏈路還在?
(b) GPS 天空圖 此刻天上有幾顆、分布好不好?
(c) 仰角時序 + 可見窗 什麼時候掉到門檻以下?
(d) DOP 時序 定位品質怎麼隨幾何變化?
**(b) 的天空圖是 GNSS 的標準視角** —— 同心圓是仰角、輻射線是方位,
遮蔽角畫成一圈。衛星擠在一邊的時候,DOP 會壞,而那在天空圖上一眼看得出來。
python grpc_link_stkviews.py 需要 STK(用 Runtime,無介面)
python grpc_link_stkviews.py --no-stk 用本地幾何代替,只為了看版面
"""
import math
import os
import sys
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt # noqa: E402
import numpy as np # noqa: E402
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
sys.path.insert(0, os.path.join(HERE, os.pardir, "tools"))
import grpc_link_closedloop as CL # noqa: E402
import grpc_link_gps as GPS # noqa: E402
import grpc_link_realtime as R # noqa: E402
OUT = os.path.join(HERE, "stk_multiview.png")
C_BLUE, C_ORANGE, C_AQUA = "#2a78d6", "#eb6834", "#1baf7a"
INK, MUTED, GRID = "#14181d", "#5a6876", "#dde4ec"
WARN = "#b4431c"
GS_LAT, GS_LON = CL.GS_LAT, CL.GS_LON
MASK = 10.0 # 天空圖上畫的遮蔽角
N_STEPS = 900 # 15 分鐘 @ 1 s
def gather(use_stk=True):
"""取資料。回傳 (軌跡, 仰角序列, 衛星方位仰角, DOP 序列, 來源說明)。"""
t, lat, lon, alt = R.plant(n=N_STEPS)
elev = [CL.elevation_deg(la, lo, al, GS_LAT, GS_LON, 0.0)
for la, lo, al in zip(lat, lon, alt)]
if not use_stk:
return (t, lat, lon, alt), elev, [], [], "本地幾何(--no-stk)"
from ansys.stk.core.stkruntime import STKRuntime
app = STKRuntime.start_application(grpc_host="0.0.0.0",
grpc_port=R.RT_PORT,
grpc_timeout_sec=90, no_graphics=True)
try:
root = app.new_object_root()
root.new_scenario("MultiView")
scen = root.current_scenario
ts = R.timestamps([0.0, 6.5 * 3600.0])
root.execute_command('SetAnalysisTimePeriod * "%s" "%s"' % (ts[0], ts[-1]))
root.execute_command("SetUnits / km")
const = GPS.GpsConstellation(root, scen, step=300.0).build().fetch()
# 天空圖:起點時刻,所有衛星的方位與仰角(不套遮蔽角,才看得出被擋掉哪些)
sv = const.at(0.0)
rx = GPS.ecef_from_geodetic(lat[0], lon[0], alt[0])
e, n, u = GPS.enu_basis(lat[0], lon[0])
sky = []
for s in sv:
d = s - rx
nd = np.linalg.norm(d)
if nd == 0:
continue
d = d / nd
up = float(np.dot(d, u))
el = math.degrees(math.asin(max(-1.0, min(1.0, up))))
az = math.degrees(math.atan2(float(np.dot(d, e)),
float(np.dot(d, n)))) % 360.0
sky.append((az, el))
# DOP 時序。**視窗要以小時計。**
# 飛行只有 15 分鐘,而 GPS 星座的幾何以小時為尺度變化 ——
# 只畫飛行那一段的話,DOP 幾乎是一條直線(實測 GDOP 只在 2.4-2.5 之間),
# 圖上看不出任何東西。所以這一格改成在**地面站位置**看六小時,
# 那才是「定位幾何怎麼隨時間變化」的答案。
dops = []
for tk in range(0, 6 * 3600, 300):
svk = const.at(min(tk, const.times[-1]))
nv, dop, _ = GPS.visible_and_dop(GS_LAT, GS_LON, 0.15, svk, MASK)
dops.append((tk / 3600.0, nv,
dop["HDOP"] if dop else float("nan"),
dop["GDOP"] if dop else float("nan")))
src = "STK 內建 GPS almanac(%d 個 PRN)" % len(const.prns)
finally:
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return (t, lat, lon, alt), elev, sky, dops, src
def draw(track, elev, sky, dops, src, path=OUT):
t, lat, lon, alt = track
fig = plt.figure(figsize=(14.0, 9.4))
fig.patch.set_facecolor("white")
gs = fig.add_gridspec(2, 2, hspace=0.32, wspace=0.26,
left=0.07, right=0.97, top=0.90, bottom=0.07)
# ---- (a) 地面軌跡,可見段用顏色分開 ----
a = fig.add_subplot(gs[0, 0])
x = [(lo - GS_LON) * 111320 * math.cos(math.radians(GS_LAT)) / 1000.0
for lo in lon]
y = [(la - GS_LAT) * 111320 / 1000.0 for la in lat]
vis = [e >= MASK for e in elev]
# 分段畫:可見與不可見用不同顏色,**不用圖例靠顏色認**,直接標字
seg_x, seg_y, seg_v = [], [], vis[0]
for i in range(len(x)):
if vis[i] != seg_v and seg_x:
# **斷鏈段畫粗一點。** 它常常很短,與可見段同粗的話在圖上看不見 ——
# 而它正是這張圖要講的東西。
a.plot(seg_x, seg_y, color=C_BLUE if seg_v else WARN,
lw=2.2 if seg_v else 4.5, zorder=3 if seg_v else 4)
seg_x, seg_y = [x[i - 1]], [y[i - 1]]
seg_v = vis[i]
seg_x.append(x[i])
seg_y.append(y[i])
if seg_x:
a.plot(seg_x, seg_y, color=C_BLUE if seg_v else WARN,
lw=2.2 if seg_v else 4.5, zorder=3 if seg_v else 4)
a.plot([0], [0], "^", color=INK, ms=11)
a.annotate("地面站", (0, 0), xytext=(6, -14), textcoords="offset points",
fontsize=9, color=INK)
# **不可見的轉折點直接標記** —— 那一段可能很短,只靠顏色會看不見
for i in range(1, len(vis)):
if vis[i] != vis[i - 1]:
a.plot([x[i]], [y[i]], "x", color=WARN, ms=9, mew=2.2, zorder=4)
n_lost = sum(1 for e in elev if e < MASK)
if n_lost:
a.annotate("斷鏈 %.0f 秒" % n_lost,
(x[elev.index(min(elev))], y[elev.index(min(elev))]),
xytext=(10, -18), textcoords="offset points",
fontsize=9, color=WARN,
arrowprops=dict(arrowstyle="-", color=WARN, lw=0.9))
a.plot([x[0]], [y[0]], "o", color=C_AQUA, ms=8)
a.annotate("起點", (x[0], y[0]), xytext=(8, 6),
textcoords="offset points", fontsize=9, color=C_AQUA)
a.set_aspect("equal")
a.set_title("(a) 地面軌跡 —— 藍段鏈路在,紅段仰角低於 %.0f°" % MASK,
fontsize=11.5, color=INK, loc="left")
a.set_xlabel("東 (km)", fontsize=9.5, color=MUTED)
a.set_ylabel("北 (km)", fontsize=9.5, color=MUTED)
# ---- (b) GPS 天空圖 ----
b = fig.add_subplot(gs[0, 1], projection="polar")
b.set_theta_zero_location("N")
b.set_theta_direction(-1)
# **半徑用 90 - 仰角**,所以天頂(90°)落在中心、地平(0°)在外圈 ——
# 那是天空圖的慣例。用 set_rlim(90, 0) 去反轉軸的話,
# 結果刻度標籤與實際位置對不起來(0° 跑到中心),看圖的人會讀反。
b.set_rlim(0, 90)
b.set_rgrids([0, 30, 60, 90], labels=["90°", "60°", "30°", "0°"],
fontsize=8, color=MUTED)
b.set_thetagrids([0, 90, 180, 270], labels=["北", "東", "南", "西"],
fontsize=9, color=MUTED)
if sky:
az_v = [math.radians(a_) for a_, e_ in sky if e_ >= MASK]
el_v = [90.0 - e_ for _a, e_ in sky if e_ >= MASK]
az_h = [math.radians(a_) for a_, e_ in sky if 0 <= e_ < MASK]
el_h = [90.0 - e_ for _a, e_ in sky if 0 <= e_ < MASK]
b.scatter(az_v, el_v, s=70, color=C_BLUE, zorder=3)
b.scatter(az_h, el_h, s=45, color=GRID, edgecolors=MUTED,
linewidths=0.8, zorder=2)
th = np.linspace(0, 2 * math.pi, 200)
b.plot(th, [90.0 - MASK] * len(th), color=WARN, ls="--",
lw=1.3, zorder=4)
b.set_title("(b) GPS 天空圖(天頂在中心)—— 可見 %d 顆,虛線為 %.0f° 遮蔽角"
% (len(az_v), MASK), fontsize=11.5, color=INK, loc="left",
pad=16)
else:
b.set_title("(b) GPS 天空圖(需要 STK)", fontsize=11.5, color=MUTED,
loc="left", pad=16)
# ---- (c) 仰角時序 + 可見窗 ----
c = fig.add_subplot(gs[1, 0])
c.plot(t, elev, color=C_BLUE, lw=1.8, zorder=3)
c.axhline(MASK, color=WARN, ls="--", lw=1.3, zorder=2)
c.text(t[-1], MASK + 1.2, "遮蔽角 %.0f°" % MASK, ha="right", fontsize=9,
color=WARN)
# 不可見的區間塗底 —— 直接看得出「什麼時候斷」
k0 = None
for i, e in enumerate(elev):
if e < MASK and k0 is None:
k0 = i
elif e >= MASK and k0 is not None:
c.axvspan(t[k0], t[i], color="#fdeeea", zorder=1)
k0 = None
if k0 is not None:
c.axvspan(t[k0], t[-1], color="#fdeeea", zorder=1)
c.set_title("(c) 對地面站的仰角 —— 淺紅為低於門檻的區間",
fontsize=11.5, color=INK, loc="left")
c.set_xlabel("時間 (s)", fontsize=9.5, color=MUTED)
c.set_ylabel("仰角 (deg)", fontsize=9.5, color=MUTED)
# ---- (d) DOP 時序 ----
d = fig.add_subplot(gs[1, 1])
if dops:
td = [r[0] for r in dops]
hd = [r[2] for r in dops]
gd = [r[3] for r in dops]
nv = [r[1] for r in dops]
d.plot(td, gd, color=C_ORANGE, lw=1.9, zorder=3)
d.plot(td, hd, color=C_BLUE, lw=1.9, zorder=3)
d.annotate("GDOP", (td[len(td) // 2], gd[len(gd) // 2]),
xytext=(6, 8), textcoords="offset points",
fontsize=9.5, color=C_ORANGE)
d.annotate("HDOP", (td[len(td) // 2], hd[len(hd) // 2]),
xytext=(6, -14), textcoords="offset points",
fontsize=9.5, color=C_BLUE)
d.set_title("(d) 定位幾何(地面站位置,六小時)—— 可見 %d–%d 顆(遮蔽角 %.0f°)"
% (min(nv), max(nv), MASK), fontsize=11.5, color=INK,
loc="left")
d.set_xlabel("時間 (小時,自場景起點)", fontsize=9.5, color=MUTED)
d.set_ylabel("DOP(無因次)", fontsize=9.5, color=MUTED)
else:
d.set_title("(d) 定位幾何(需要 STK)", fontsize=11.5, color=MUTED,
loc="left")
d.axis("off")
for ax in (a, c, d):
ax.set_facecolor("white")
for s_ in ("top", "right"):
ax.spines[s_].set_visible(False)
for s_ in ("left", "bottom"):
ax.spines[s_].set_color(GRID)
ax.tick_params(colors=MUTED, labelsize=8.5)
ax.grid(True, color=GRID, lw=0.7, zorder=0)
ax.set_axisbelow(True)
fig.suptitle("STK 多視角 —— 資料由 STK 計算,呈現以「能回答什麼問題」為準",
fontsize=14.5, color=INK, x=0.07, ha="left", y=0.975)
fig.text(0.07, 0.935, "來源:%s" % src, fontsize=9.5, color=MUTED,
ha="left")
fig.savefig(path, dpi=130, facecolor="white")
plt.close(fig)
def main():
use_stk = "--no-stk" not in sys.argv
try:
import cjk
cjk.use_cjk()
except Exception: # noqa: BLE001
pass
matplotlib.rcParams["axes.unicode_minus"] = False
fams = list(matplotlib.rcParams.get("font.sans-serif") or [])
if "DejaVu Sans" not in fams:
matplotlib.rcParams["font.sans-serif"] = fams + ["DejaVu Sans"]
print("=" * 74)
print(" STK 多視角圖")
print("=" * 74)
try:
track, elev, sky, dops, src = gather(use_stk)
except Exception as e: # noqa: BLE001
print(" 取資料失敗:%s" % str(e)[:110])
print(" 可用 --no-stk 先看版面。")
return 1
draw(track, elev, sky, dops, src)
print(" 來源:%s" % src)
print(" 仰角 %.2f - %.2f deg;天空圖 %d 顆;DOP 取樣 %d 點"
% (min(elev), max(elev), len(sky), len(dops)))
print(" 已存到 %s(%.0f KB)"
% (os.path.basename(OUT), os.path.getsize(OUT) / 1024))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_swarm.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 協同:任務分配、避撞、隊形。
## 這一節補的缺口
`grpc_link_fleetloop.py` 讓 N 架各自閉迴路,但**每架都不知道其他架的存在**:
兩架可以被派去同一個目標、可以撞在一起、也排不出隊形。
協同就是把「彼此知道」這件事加進去。
三件事各有一個**可以證明**的判準,而不是「看起來合理」:
| | 做什麼 | 怎麼證明 |
|---|---|---|
| 任務分配 | N 架對 M 個目標 | **窮舉法**對照:小規模時最佳解可以算到底 |
| 避撞 | 最小間距不得低於下限 | **注入對撞航線**:偵測不到就是壞的 |
| 隊形 | 保持幾何圖形 | **解析式**:協調轉彎中各機所需速度 v_i = ω r_i |
## 一、任務分配:貪婪 vs 最佳,差多少是可以量的
最常見的做法是**貪婪最近鄰**:每個目標挑最近的可用機。
它簡單、可解釋,而且**不保證全域最優**。
本檔用 Hungarian 演算法(`scipy.optimize.linear_sum_assignment`)解最佳指派,
再做兩件事:
1. **對照窮舉法。** N <= 7 時所有排列都算得完,最佳解沒有爭議 ——
所以這是個「對就是對」的檢查,不是容差檢查。
2. **量貪婪的代價。** 隨機案例上跑兩者,回報成本差。
「要不要換成最佳化」這個問題只能這樣回答,不能用講的。
成本函數是可替換的:預設用「到達時間」(距離 / 巡航速度),
接上 STK 時可以換成含地形遮蔽的可見度或鏈路餘裕 ——
**那才是 STK 在協同裡的位置**:它提供 Simulink 與飛控算不出來的成本。
## 二、避撞:偵測與化解要分開驗
偵測與化解是兩件事,混在一起驗會把「沒偵測到」誤判成「化解成功」
(因為兩者都表現成「沒有警報」)。所以:
- **偵測**:注入一組**必定相撞**的航線,偵測不到就是壞的。
- **不誤報**:注入一組**必定安全**的航線,報警就是壞的。
- **化解**:化解後的最小間距必須 >= 下限。
化解手法刻意用最簡單的**高度分層**:衝突的兩架分到不同高度層。
它不是好的避撞律(真實的要考慮爬升率、油耗、任務優先序),
但它讓「化解真的生效」在數字上無法造假。
## 三、隊形:轉彎時的速度差是解析可算的
一個剛性隊形繞共同圓心轉彎時,外側機的軌跡半徑較大,
所以**必須飛得比較快**:
v_i = omega * r_i
全隊同速的話隊形會在轉彎中變形。這是個閉合式,所以隊形控制律
算出來的速度指令可以逐機對照 —— 不必等模擬跑完再看圖。
python grpc_link_swarm.py 三項都跑(不需 STK)
python grpc_link_swarm.py --assign-only 只做任務分配與窮舉對照
python grpc_link_swarm.py --trials 200 分配的隨機案例數
"""
import io
import itertools
import json
import math
import os
import random
import sys
import numpy as np
from scipy.optimize import linear_sum_assignment
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
import grpc_link_ardupilot as AP # noqa: E402
SWARM_OUT = os.path.join(HERE, "grpc_link_swarm.json")
CRUISE_MS = 18.0 # 與 MAVLink 模擬器的設計點一致
SEP_MIN_M = 60.0 # 最小水平間距下限
SEP_ALT_M = 40.0 # 高度分層的層距
HORIZON_S = 120.0 # 避撞的預視時間
BRUTE_MAX_N = 7 # 窮舉法的規模上限(7! = 5040,還算得完)
# --------------------------------------------------------------- 一、分配
def cost_matrix(vehicles, targets, cruise=CRUISE_MS):
"""到達時間成本(秒)。vehicles / targets 都是 (lat, lon)。
**成本函數是這一節唯一該由 STK 提供的東西。** 這裡的預設值只是幾何距離,
接上 STK 之後可以換成含地形遮蔽的可見度、鏈路餘裕、或感測器覆蓋品質 ——
那些是 Simulink 與飛控算不出來的,而分配的品質完全取決於成本對不對。
"""
c = np.zeros((len(vehicles), len(targets)))
for i, (vlat, vlon) in enumerate(vehicles):
for j, (tlat, tlon) in enumerate(targets):
dn = (tlat - vlat) * AP.M_PER_DEG_LAT
de = (tlon - vlon) * AP.M_PER_DEG_LAT * math.cos(math.radians(vlat))
c[i, j] = math.hypot(dn, de) / cruise
return c
def assign_optimal(cost):
"""Hungarian 最佳指派。回傳 (配對清單, 總成本)。"""
r, c = linear_sum_assignment(cost)
pairs = list(zip(r.tolist(), c.tolist()))
return pairs, float(cost[r, c].sum())
def assign_greedy(cost):
"""貪婪最近鄰:每次挑全域最小的可用配對。回傳 (配對清單, 總成本)。
這是最常見的做法,而且**不保證全域最優**。本檔量的就是它差多少。
"""
n, m = cost.shape
used_v, used_t, pairs, total = set(), set(), [], 0.0
order = sorted(((cost[i, j], i, j) for i in range(n) for j in range(m)))
for val, i, j in order:
if i in used_v or j in used_t:
continue
used_v.add(i)
used_t.add(j)
pairs.append((i, j))
total += float(val)
if len(pairs) == min(n, m):
break
return pairs, total
def assign_brute(cost):
"""窮舉所有指派,回傳 (配對清單, 總成本)。**只用來驗證,不要用在正式流程。**
N <= BRUTE_MAX_N 才叫得動 —— 複雜度是 O(N!)。
它存在的唯一理由是:Hungarian 的最佳性不該靠相信,該靠對照。
"""
n, m = cost.shape
if n > BRUTE_MAX_N or m > BRUTE_MAX_N:
raise ValueError("窮舉法規模上限是 %d,收到 %dx%d" % (BRUTE_MAX_N, n, m))
best, best_pairs = float("inf"), None
k = min(n, m)
for vsel in itertools.permutations(range(n), k):
for tsel in itertools.permutations(range(m), k):
tot = sum(cost[vsel[x], tsel[x]] for x in range(k))
if tot < best:
best, best_pairs = tot, list(zip(vsel, tsel))
return best_pairs, float(best)
# --------------------------------------------------------------- 二、避撞
def min_separation(tracks):
"""所有配對在所有時刻的最小水平間距(公尺)。
`tracks` 是 [[(t, lat, lon, alt_km), ...], ...],每架一條,時間對齊。
回傳 (最小間距, (i, j, 索引))。
"""
worst, where = float("inf"), None
n = len(tracks)
for i in range(n):
for j in range(i + 1, n):
a, b = tracks[i], tracks[j]
for k in range(min(len(a), len(b))):
dn = (a[k][1] - b[k][1]) * AP.M_PER_DEG_LAT
de = ((a[k][2] - b[k][2]) * AP.M_PER_DEG_LAT
* math.cos(math.radians(a[k][1])))
d = math.hypot(dn, de)
if d < worst:
worst, where = d, (i, j, k)
return worst, where
def detect_conflicts(tracks, sep_min=SEP_MIN_M):
"""找出所有水平間距低於下限的配對。回傳 [(i, j, 最小間距), ...]。
**只偵測,不化解。** 兩件事分開,才驗得出「沒偵測到」與「化解成功」
的差別 —— 它們都表現成「沒有警報」。
"""
out = []
n = len(tracks)
for i in range(n):
for j in range(i + 1, n):
a, b = tracks[i], tracks[j]
worst = float("inf")
for k in range(min(len(a), len(b))):
dn = (a[k][1] - b[k][1]) * AP.M_PER_DEG_LAT
de = ((a[k][2] - b[k][2]) * AP.M_PER_DEG_LAT
* math.cos(math.radians(a[k][1])))
worst = min(worst, math.hypot(dn, de))
if worst < sep_min:
out.append((i, j, worst))
return out
def resolve_by_altitude(tracks, conflicts, layer_m=SEP_ALT_M):
"""高度分層化解。回傳新的 tracks(只改高度)。
**刻意用最笨的方法。** 真實的避撞要考慮爬升率、油耗、任務優先序;
這裡要的是「化解真的生效」在數字上無法造假 ——
分層之後垂直間距就是層距,而那是可以直接驗的。
"""
layer = {}
for i, j, _d in conflicts:
layer.setdefault(i, 0)
layer[j] = layer.get(j, 0) + 1 if j not in layer else layer[j]
# 逐一指派層號:衝突對中的第二架往上疊一層
nxt = {}
for i, j, _d in conflicts:
nxt.setdefault(i, 0)
if j not in nxt:
nxt[j] = nxt[i] + 1
out = []
for idx, tr in enumerate(tracks):
lv = nxt.get(idx, 0)
out.append([(t, la, lo, alt + lv * layer_m / 1000.0)
for (t, la, lo, alt) in tr])
return out
def vertical_separation(tracks):
"""所有配對的最小垂直間距(公尺)。"""
worst = float("inf")
n = len(tracks)
for i in range(n):
for j in range(i + 1, n):
a, b = tracks[i], tracks[j]
for k in range(min(len(a), len(b))):
worst = min(worst, abs(a[k][3] - b[k][3]) * 1000.0)
return worst
# --------------------------------------------------------------- 三、隊形
def formation_turn_speeds(radii_m, omega_rad_s):
"""剛性隊形繞共同圓心轉彎時,各機所需的速度(m/s)。
**閉合式:v_i = omega * r_i。** 外側機必須飛得比較快,
全隊同速的話隊形會在轉彎中變形。
所以隊形控制律算出來的速度指令可以逐機對照,
不必等模擬跑完再看圖 —— 而「看圖覺得還行」正是這類東西最常見的驗法。
"""
return [omega_rad_s * r for r in radii_m]
def formation_offsets(n, spacing_m=120.0, shape="v"):
"""隊形的相對位置(相對長機,單位公尺,(右, 前))。
`v`:雁形;`line`:橫隊。兩者在轉彎時的行為不同 ——
橫隊的內外側半徑差最大,所以速度差也最大。
"""
out = []
for i in range(n):
if i == 0:
out.append((0.0, 0.0))
continue
k = (i + 1) // 2
side = 1.0 if i % 2 == 1 else -1.0
if shape == "line":
out.append((side * k * spacing_m, 0.0))
else:
out.append((side * k * spacing_m, -k * spacing_m))
return out
def min_turn_radius_for_envelope(offsets, v_cmd, v_min, v_max,
turn_sign=1.0):
"""速度包絡容許的**最小轉彎半徑**(公尺)。
隊形轉彎的速度差是 `v_i = omega * r_i`,而 `omega = v_cmd / R`。
最外側機的需速是 `v_cmd * (R + d_out) / R`,最內側是
`v_cmd * (R - d_in) / R`。兩者都要落在 [v_min, v_max] 裡,解出 R:
R >= v_cmd * d_out / (v_max - v_cmd) 外側不能超速
R >= v_cmd * d_in / (v_cmd - v_min) 內側不能失速
**這是隊形的硬限制,不是控制律調得好就能繞過的。**
隊形越寬、轉彎越緊,需要的速度差越大;超出包絡就只能鬆開隊形。
"""
d_out = max(0.0, max(-turn_sign * right for (right, _f) in offsets))
d_in = max(0.0, max(turn_sign * right for (right, _f) in offsets))
r_out = (v_cmd * d_out / (v_max - v_cmd)) if v_max > v_cmd else float("inf")
r_in = (v_cmd * d_in / (v_cmd - v_min)) if v_cmd > v_min else float("inf")
return max(r_out, r_in)
def turn_radii(offsets, centre_radius_m, turn_sign=1.0):
"""各機在協調轉彎中的軌跡半徑(公尺)。
圓心在長機的**右**側(turn_sign=+1)時,offset 的「右」分量
直接減少半徑 —— 內側機轉得比較緊。
"""
return [max(1.0, centre_radius_m - turn_sign * right)
for (right, _fwd) in offsets]
# --------------------------------------------------------------- 主流程
def run_assignment(trials=120, seed=20260927):
"""隨機案例上比 Hungarian / 貪婪 / 窮舉。回傳統計。"""
rng = random.Random(seed)
exact_ok = 0
gaps = []
worst_gap = 0.0
for _ in range(trials):
n = rng.randint(2, 5)
m = rng.randint(n, min(BRUTE_MAX_N, n + 2))
veh = [(24.9 + rng.random() * 0.2, 121.2 + rng.random() * 0.2)
for _ in range(n)]
tgt = [(24.9 + rng.random() * 0.2, 121.2 + rng.random() * 0.2)
for _ in range(m)]
c = cost_matrix(veh, tgt)
_p_opt, opt = assign_optimal(c)
_p_bru, bru = assign_brute(c)
_p_grd, grd = assign_greedy(c)
if abs(opt - bru) < 1e-9:
exact_ok += 1
gap = (grd - opt) / opt if opt > 0 else 0.0
gaps.append(gap)
worst_gap = max(worst_gap, gap)
return {"trials": trials, "hungarian_matches_brute": exact_ok,
"greedy_gap_mean": sum(gaps) / len(gaps),
"greedy_gap_worst": worst_gap,
"greedy_gap_nonzero": sum(1 for g in gaps if g > 1e-9)}
def straight_track(lat0, lon0, hdg_deg, n=60, dt=1.0, v=CRUISE_MS,
alt_km=0.150):
"""直線航跡,用來造對撞與安全的測試案例。"""
out = []
la, lo = lat0, lon0
h = math.radians(hdg_deg)
for i in range(n):
out.append((i * dt, la, lo, alt_km))
la += (v * math.cos(h) * dt) / AP.M_PER_DEG_LAT
lo += (v * math.sin(h) * dt) / (AP.M_PER_DEG_LAT
* math.cos(math.radians(la)))
return out
def main():
trials = 120
if "--trials" in sys.argv:
trials = int(sys.argv[sys.argv.index("--trials") + 1])
only_assign = "--assign-only" in sys.argv
print("=" * 78)
print(" 協同:任務分配、避撞、隊形")
print("=" * 78)
# ---- 一、分配 ----
a = run_assignment(trials)
print()
print(" 一、任務分配(%d 個隨機案例)" % a["trials"])
print(" Hungarian 與**窮舉法**一致:%d / %d"
% (a["hungarian_matches_brute"], a["trials"]))
print(" -> 最佳性不靠相信,靠對照。小規模時窮舉算得完,沒有爭議。")
print(" 貪婪最近鄰比最佳解貴:平均 %.2f%%,最差 %.2f%%"
% (a["greedy_gap_mean"] * 100, a["greedy_gap_worst"] * 100))
print(" %d / %d 個案例貪婪不是最優"
% (a["greedy_gap_nonzero"], a["trials"]))
print(" -> 「要不要換成最佳化」這個問題只能這樣回答。")
out = {"assignment": a}
if only_assign:
io.open(SWARM_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(out, ensure_ascii=False, indent=1) + "\n")
print()
print(" (--assign-only)結果已存到 %s" % os.path.basename(SWARM_OUT))
return 0
# ---- 二、避撞 ----
print()
print(" 二、避撞(偵測與化解分開驗)")
# 對撞:兩架相向而行,同一條線上
head_on = [straight_track(24.99, 121.30, 90.0),
straight_track(24.99, 121.30 + 0.012, 270.0)]
d0, where = min_separation(head_on)
conf = detect_conflicts(head_on)
print(" 注入**必定相撞**的航線 -> 最小間距 %.1f m,偵測到 %d 組衝突"
% (d0, len(conf)))
# 安全:兩架平行,橫向間隔 600 m
safe = [straight_track(24.99, 121.30, 90.0),
straight_track(24.99 + 600.0 / AP.M_PER_DEG_LAT, 121.30, 90.0)]
d1, _ = min_separation(safe)
conf_safe = detect_conflicts(safe)
print(" 注入**必定安全**的航線 -> 最小間距 %.1f m,偵測到 %d 組衝突"
% (d1, len(conf_safe)))
fixed = resolve_by_altitude(head_on, conf)
vsep = vertical_separation(fixed)
hsep, _ = min_separation(fixed)
print(" 高度分層化解後 -> 水平仍 %.1f m,但**垂直間距 %.1f m**"
% (hsep, vsep))
print(" -> 水平沒有變是對的:分層只動高度。化解是否生效看垂直。")
out["avoidance"] = {"head_on_min_m": d0, "head_on_conflicts": len(conf),
"safe_min_m": d1, "safe_conflicts": len(conf_safe),
"after_vertical_m": vsep, "after_horizontal_m": hsep}
# ---- 三、隊形 ----
print()
print(" 三、隊形(轉彎時的速度差是解析可算的)")
n_f = 5
offs = formation_offsets(n_f, shape="line")
R = 1000.0
omega = CRUISE_MS / R
radii = turn_radii(offs, R)
speeds = formation_turn_speeds(radii, omega)
print(" 橫隊 %d 架、間距 120 m、轉彎半徑 %.0f m、角速度 %.5f rad/s"
% (n_f, R, omega))
for i, (off, r, v) in enumerate(zip(offs, radii, speeds)):
print(" 第 %d 架 右偏 %+7.1f m 軌跡半徑 %7.1f m 需速 %6.2f m/s"
% (i + 1, off[0], r, v))
print(" 內外側速度差 %.2f m/s(%.1f%%)—— 全隊同速的話隊形會變形"
% (max(speeds) - min(speeds),
100.0 * (max(speeds) - min(speeds)) / CRUISE_MS))
# 速度包絡:這是隊形的**硬限制**,控制律再好也繞不過
v_min, v_max = 12.0, 25.0 # 小型固定翼的代表值(失速 / 最大平飛)
r_min = min_turn_radius_for_envelope(offs, CRUISE_MS, v_min, v_max)
print()
print(" 速度包絡 %.0f - %.0f m/s 下,這個隊形的**最小轉彎半徑 %.0f m**"
% (v_min, v_max, r_min))
print(" -> 比這更緊的彎,外側會超速或內側會失速。")
print(" -> 隊形越寬、彎越緊,需要的速度差越大 ——")
print(" 超出包絡就只能鬆開隊形,沒有第三條路。")
out_env = {"v_min": v_min, "v_max": v_max, "r_min_m": r_min}
out["formation"] = {"n": n_f, "radius_m": R, "omega": omega,
"offsets": offs, "radii": radii, "speeds": speeds,
"envelope": out_env}
io.open(SWARM_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(out, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(SWARM_OUT))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_terrain.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 地形:把真實 DEM 載進 STK,並驗它真的載對了。
## 這一節補的缺口
前面所有的「鏈路通不通」都只看**幾何仰角**:地球是個光滑的橢球,
沒有山。對低空飛行的無人機來說那是最不真實的一個假設 ——
台灣的無人機任務失去鏈路,多半不是因為飛太遠,是因為中間有座山。
所以這一支把真實的數值地形(DEM)載進 STK,讓 access 含地形遮蔽。
## 地形檔不在版本庫裡,而且**不應該在**
本教材不附 DEM,理由有三個:
1. 地形資料有它自己的來源與使用條款,不該被一份教材順手打包帶走。
2. 一塊有用的地形動輒數 MB 到數十 MB,進了版本庫就永遠在那裡。
3. 每個人要測的地區不一樣。
所以路徑由環境變數 `GRPC_LINK_DEM` 指定,或用 `--dem <路徑>` 傳進來。
本檔只讀它,不複製、不改寫。
set GRPC_LINK_DEM=D:\data\your_area.tif
python grpc_link_terrain.py
## STK 吃哪些格式
`TerrainFileType` 列了二十種,而**其中有兩個是同一種檔案的兩種解讀**:
TIFF_TERRAIN_FILE 檔案裡的高程以**參考橢球**為基準
TIFF_TERRAIN_FILE_IN_MEAN_SEA_LEVEL 檔案裡的高程以**平均海平面**為基準
選錯不會報錯,只會讓整片地形上下平移 —— 台灣一帶的大地起伏
(geoid undulation)約十幾到二十幾公尺。對「飛機會不會撞山」那是幾秒的差別,
對「山會不會擋住鏈路」則可能整段可見窗都不一樣。
**所以本檔不猜,兩種都載一次,拿 STK 回讀的高程去對檔案裡的值,
看哪一種對得上。** 這是可以量的問題,不該用假設回答。
## 涵蓋範圍是個會讓 STK 停住的陷阱
`tirem_coastal` 章記錄過:物件離開地形檔的涵蓋範圍時,
**ComputeAccess 再也不回來** —— 不是回錯誤、不是回哨兵值,是整個停住,
而文件明文承諾會給錯誤訊息。
所以本檔在放任何東西之前先算涵蓋範圍,並**拒絕**把物件放在範圍外。
寧可拒絕,也不要讓使用者去查一個永遠不會回來的呼叫。
python grpc_link_terrain.py --dem <路徑> 載入、驗證、對帳
python grpc_link_terrain.py --dem <路徑> --access 再加地形遮蔽的 access 對照
"""
import io
import json
import math
import os
import sys
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
TERRAIN_OUT = os.path.join(HERE, "grpc_link_terrain.json")
PORT = 40704
# 取樣點數。夠多才看得出系統性偏移與隨機差異的差別 ——
# 只取一兩點的話,插值造成的幾公尺差會被誤當成基準面選錯。
N_SAMPLES = 200
# 只在涵蓋範圍內縮一圈取樣:邊緣的插值本來就沒有完整的鄰居
EDGE_MARGIN = 0.02 # 佔整個範圍的比例
def dem_path(argv=None):
"""DEM 的路徑。--dem 優先,其次環境變數。**沒有預設值。**
不給預設路徑是刻意的:預設路徑會讓人以為教材附了地形檔,
然後在別人的機器上得到一個看不懂的「檔案不存在」。
"""
argv = sys.argv if argv is None else argv
if "--dem" in argv:
return argv[argv.index("--dem") + 1]
return os.environ.get("GRPC_LINK_DEM")
def read_dem(path):
"""讀 DEM。回傳 (陣列, 仿射轉換, 範圍)。需要 rasterio。"""
import rasterio
with rasterio.open(path) as d:
arr = d.read(1)
return arr, d.transform, d.bounds, d.crs
def bilinear(arr, transform, lat, lon):
"""在 DEM 上做雙線性內插。
**不是取最近的格點。** STK 內部也是內插的,拿最近格點去比
會把插值差誤判成基準面差 —— 在陡坡上那可以差好幾十公尺。
"""
inv = ~transform
col, row = inv * (lon, lat)
c0, r0 = int(math.floor(col - 0.5)), int(math.floor(row - 0.5))
fc, fr = (col - 0.5) - c0, (row - 0.5) - r0
h, w = arr.shape
if not (0 <= c0 < w - 1 and 0 <= r0 < h - 1):
return None
v00 = float(arr[r0, c0])
v01 = float(arr[r0, c0 + 1])
v10 = float(arr[r0 + 1, c0])
v11 = float(arr[r0 + 1, c0 + 1])
return ((v00 * (1 - fc) + v01 * fc) * (1 - fr)
+ (v10 * (1 - fc) + v11 * fc) * fr)
def sample_points(arr, transform, bounds, n=N_SAMPLES, margin=EDGE_MARGIN):
"""在涵蓋範圍內均勻取樣。回傳 [(lat, lon, dem_h), ...]。
刻意**避開邊緣**,而且**偏向有高度的地方**:全是海面的話,
所有基準面都給 0 附近的值,對帳就分不出任何東西。
"""
h, w = arr.shape
lo_r, hi_r = int(h * margin) + 1, int(h * (1 - margin)) - 2
lo_c, hi_c = int(w * margin) + 1, int(w * (1 - margin)) - 2
pts, tries = [], 0
step_r = max(1, (hi_r - lo_r) // int(math.sqrt(n) * 2))
step_c = max(1, (hi_c - lo_c) // int(math.sqrt(n) * 2))
for r in range(lo_r, hi_r, step_r):
for c in range(lo_c, hi_c, step_c):
tries += 1
v = float(arr[r, c])
if v <= 0.5: # 海面/無資料,對帳沒有鑑別力
continue
lon, lat = transform * (c + 0.5, r + 0.5)
pts.append((float(lat), float(lon), v))
if len(pts) >= n:
return pts, tries
return pts, tries
def load_terrain(root, path, file_type):
"""把 DEM 載進 STK。回傳 (地形物件, Earth 元素)。"""
earth = root.current_scenario.terrain.get_item_by_name("Earth")
coll = earth.terrain_collection
coll.remove_all()
t = coll.add(path, file_type)
t.use_terrain = True
return t, earth
def compare(earth, pts, arr, transform, alt_ref):
"""STK 回讀 vs DEM 內插。回傳 (差值清單, 統計字典)。"""
diffs = []
for lat, lon, _v in pts:
ref = bilinear(arr, transform, lat, lon)
if ref is None:
continue
try:
got = earth.get_altitude(float(lat), float(lon), alt_ref) * 1000.0
except Exception: # noqa: BLE001
continue
diffs.append(got - ref)
if not diffs:
return diffs, {}
n = len(diffs)
mean = sum(diffs) / n
var = sum((d - mean) ** 2 for d in diffs) / n
return diffs, {"n": n, "mean": mean, "std": math.sqrt(var),
"min": min(diffs), "max": max(diffs),
"absmax": max(abs(d) for d in diffs)}
def inside(bounds, lat, lon, margin_deg=0.005):
"""在涵蓋範圍內嗎(留一點邊)。
**這個檢查不能省。** 物件離開地形檔範圍時 STK 的 ComputeAccess
不會回來 —— 不是回錯誤,是整個停住。寧可在這裡拒絕。
"""
return (bounds.bottom + margin_deg <= lat <= bounds.top - margin_deg
and bounds.left + margin_deg <= lon <= bounds.right - margin_deg)
def main():
path = dem_path()
if not path:
print("=" * 78)
print(" 地形:沒有指定 DEM")
print("=" * 78)
print(" 本教材**不附**地形檔(它有自己的來源與使用條款,而且很大)。")
print(" 請用下列任一種方式指定:")
print(" set GRPC_LINK_DEM=D:\\data\\your_area.tif")
print(" python grpc_link_terrain.py --dem D:\\data\\your_area.tif")
print()
print(" STK 讀得動的格式見 TerrainFileType,常見的是 GeoTIFF 與 DTED。")
return 1
if not os.path.isfile(path):
print(" 找不到 DEM:%s" % path)
return 1
try:
arr, transform, bounds, crs = read_dem(path)
except ImportError:
print(" 需要 rasterio 才能讀 DEM 做對帳:pip install rasterio")
return 1
print("=" * 78)
print(" 地形:把真實 DEM 載進 STK,並驗它真的載對了")
print("=" * 78)
print(" 檔案 %s" % os.path.basename(path))
print(" 大小 %d x %d 格" % (arr.shape[1], arr.shape[0]))
print(" 範圍 %.5f - %.5f E / %.5f - %.5f N"
% (bounds.left, bounds.right, bounds.bottom, bounds.top))
print(" 解析度 %.8f deg(約 %.1f m)"
% (abs(transform.a), abs(transform.a) * 111320.0))
print(" 高程 %.1f - %.1f m" % (float(arr.min()), float(arr.max())))
print(" 座標系 %s" % str(crs).split("\n")[0][:60])
pts, tried = sample_points(arr, transform, bounds)
print(" 對帳取樣 %d 點(掃過 %d 格,只取高程 > 0 的)" % (len(pts), tried))
if len(pts) < 10:
print(" **可用的取樣點太少** —— 這塊地形幾乎全是水面或無資料,")
print(" 拿它做基準面對帳沒有鑑別力。換一塊有地形起伏的。")
return 1
try:
from ansys.stk.core.stkruntime import STKRuntime
from ansys.stk.core.stkobjects import (TerrainFileType,
AltitudeReferenceType)
app = STKRuntime.start_application(grpc_host="0.0.0.0", grpc_port=PORT,
grpc_timeout_sec=90,
no_graphics=True)
except Exception as e: # noqa: BLE001
print(" 接不上 STK:%s" % str(e)[:110])
return 1
root = app.new_object_root()
root.new_scenario("TerrainCheck")
root.execute_command("SetUnits / km")
# ---- 兩種解讀各載一次,看哪一種對得上檔案 ----
cases = [
("TIFF(橢球基準)", TerrainFileType.TIFF_TERRAIN_FILE),
("TIFF(平均海平面基準)",
TerrainFileType.TIFF_TERRAIN_FILE_IN_MEAN_SEA_LEVEL),
]
refs = [("ELLIPSOID", AltitudeReferenceType.ELLIPSOID),
("MEAN_SEA_LEVEL", AltitudeReferenceType.MEAN_SEA_LEVEL)]
print()
print(" **基準面不用猜,量出來** —— 兩種解讀 x 兩種回讀參考:")
print(" %-24s %-16s %9s %9s %9s"
% ("載入時宣告的基準", "回讀時要求的", "平均差", "標準差", "最大差"))
results = {}
extent = None
for cname, ftype in cases:
t, earth = load_terrain(root, path, ftype)
if extent is None:
extent = (t.southwest_latitude, t.southwest_longitude,
t.northeast_latitude, t.northeast_longitude)
for rname, aref in refs:
_d, st = compare(earth, pts, arr, transform, aref)
if not st:
continue
results["%s|%s" % (cname, rname)] = st
print(" %-24s %-16s %8.2f m %8.2f m %8.2f m"
% (cname, rname, st["mean"], st["std"], st["absmax"]))
print()
print(" STK 回報的涵蓋範圍 SW %.5f,%.5f NE %.5f,%.5f"
% extent)
print(" 檔案本身的範圍 SW %.5f,%.5f NE %.5f,%.5f"
% (bounds.bottom, bounds.left, bounds.top, bounds.right))
# **挑「平均差最接近 0」是錯的判準。** 實測會有**兩個**組合並列在 0:
# 宣告橢球、用橢球回讀,和宣告 MSL、用 MSL 回讀 —— 因為
# **STK 永遠跟自己的宣告一致**。
near_zero = sorted(results.items(), key=lambda kv: abs(kv[1]["mean"]))
tied = [k for k, v in near_zero if abs(v["mean"]) < 0.5]
offset = max((abs(v["mean"]) for _k, v in results.items()), default=0.0)
noise = min((v["std"] for _k, v in results.items()), default=0.0)
print()
print(" **這組量測回答了什麼,又沒回答什麼 —— 這一段是本節的重點**")
print()
print(" 回答了:基準面選錯造成的**系統性偏移是 %.2f m**。" % offset)
print(" 那是這塊地形所在位置的大地起伏(geoid undulation)。")
print(" 內插與取樣造成的隨機差只有 %.2f m,兩者差了一個數量級,"
% noise)
print(" 所以偏移不是雜訊,是真的選錯了。")
print()
if len(tied) > 1:
print(" **沒有回答:哪一個宣告才是對的。**")
print(" 有 %d 個組合的平均差都落在 0 附近:" % len(tied))
for k in tied:
print(" - %s" % k)
print(" 因為 **STK 永遠跟自己的宣告一致** ——")
print(" 你說檔案是橢球基準,它就用橢球回讀給你;")
print(" 你說是平均海平面,它就用平均海平面回讀給你。")
print(" **光問 STK 是問不出來的。**")
print()
print(" 正確答案只能來自 DEM 本身的來源說明:")
print(" SRTM / ASTER GDEM / 多數公開 DEM -> 平均海平面(EGM96)")
print(" 光達或測量成果 -> 看規格書,兩種都有")
print(" 查清楚再宣告。猜錯的代價就是上面那 %.2f m。" % offset)
else:
print(" 唯一落在 0 附近的組合:%s" % (tied[0] if tied else "(無)"))
out = {"dem": os.path.basename(path),
"shape": [int(arr.shape[0]), int(arr.shape[1])],
"bounds": [bounds.left, bounds.bottom, bounds.right, bounds.top],
"stk_extent": list(extent),
"resolution_deg": abs(transform.a),
"samples": len(pts), "tied_near_zero": tied,
"datum_offset_m": offset, "interp_noise_m": noise,
"stats": {k: v for k, v in results.items()}}
io.open(TERRAIN_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps(out, ensure_ascii=False, indent=1) + "\n")
print()
print(" 結果已存到 %s" % os.path.basename(TERRAIN_OUT))
try:
app.shutdown()
except Exception: # noqa: BLE001
pass
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_transport.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 三種傳輸的定位與實測成本。
## 三種接法,不是三個選項而是三種用途
STK Engine 行程內。Python 直接載入 STK 的引擎,呼叫不出這個行程。
沒有序列化、沒有網路。**最快,但只能在本機、無介面。**
STK Desktop 接上桌面版。走 COM(用 pid)或 gRPC(用 /grpcServer On)。
要看到 3D 畫面、要與人共用同一個場景時用它。
STK Runtime 只走 gRPC。無介面、可裝進容器、可跨機器。
**自動化與遠端佈署用它。**
## 要驗的斷言:「gRPC 比 COM 快上數倍」
這句話在網路上的 STK 架構文章裡很常見,而它**是反的** —— 但反得有條件,
所以值得量,不能只說「錯」。實測同一組操作:
| 操作 | 行程內 COM | 跨行程 gRPC | 比值 |
|---|---|---|---|
| 讀一個屬性 | 約 0.047 ms | 約 1.70 ms | gRPC **慢約 36 倍** |
| 送一條 Connect 指令 | 約 16.3 ms | 約 14.0 ms | **幾乎相同** |
兩列的差別解釋了全部:
- **讀屬性**幾乎不做事,所以量到的幾乎全是傳輸成本。序列化 + 迴文迴路
的 TCP 往返就是那 1.7 ms,而行程內呼叫只是一次虛擬函式跳轉。
- **Connect 指令**要 STK 真的去建物件、重算,十幾毫秒都花在 STK 自己身上,
傳輸那 1.7 ms 沉在雜訊裡。
所以正確的說法是:**gRPC 的成本只在細粒度存取上顯現。**
而 gRPC 換到的不是速度,是**位置自由** —— 跨機器、跨平台、進容器。
拿速度當理由去選 gRPC,會在第一支逐點存取的腳本上被打回來;
拿佈署彈性當理由,才站得住。
## 這件事怎麼影響寫法
既然單次往返約 1.7 ms、而它只在細粒度時才要緊,結論就很清楚:
1. 迴圈裡不要放屬性存取。把物件提到迴圈外(見 `grpc_link_bench.py` 的鏈式量測)。
2. 大量寫入用批次(`new_grpc_call_batcher` / `add_position_batch`)。
3. 大量讀取用 DataProvider 一次取一段,不要逐點問 —— 但要留意訊息上限。
三條都是同一件事:**把往返次數壓下來**,而不是讓每次往返變快。
python grpc_link_transport.py # 量 COM 與 gRPC
python grpc_link_transport.py --table # 只印定位表,不連 STK
"""
import io
import json
import os
import sys
import time
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
TRANSPORT_OUT = os.path.join(HERE, "grpc_link_transport.json")
PORT = 40704
# (名稱, 傳輸, 有介面, 跨機器, 可容器化, 適用場合)
TRANSPORTS = [
("STK Engine", "行程內", False, False, False, "本機自動化,要最低延遲"),
("STK Desktop + COM", "COM(pid)", True, False, False, "要看畫面,人機共用場景"),
("STK Desktop + gRPC", "gRPC", True, True, False, "要看畫面,但控制端在別台"),
("STK Runtime", "gRPC", False, True, True, "無介面批次、容器、遠端佈署"),
]
def print_table():
print(" %-20s %-10s %-6s %-8s %-8s %s"
% ("接法", "傳輸", "有介面", "跨機器", "可容器", "適用"))
for name, tr, gui, remote, container, use in TRANSPORTS:
print(" %-20s %-10s %-6s %-8s %-8s %s"
% (name, tr, "是" if gui else "否", "是" if remote else "否",
"是" if container else "否", use))
def _name_of(scen):
"""兩代 API 的屬性名不同,量測要能吃兩種。"""
return (scen.InstanceName if hasattr(scen, "InstanceName")
else scen.instance_name)
def bench_property(root, n=400):
"""讀屬性的平均耗時(ms)。前幾次丟掉 —— 第一次含通道/介面暖機。"""
scen = (root.CurrentScenario if hasattr(root, "CurrentScenario")
else root.current_scenario)
for _ in range(10):
_name_of(scen)
t0 = time.time()
for _ in range(n):
_name_of(scen)
return (time.time() - t0) / n * 1000.0
def bench_command(root, n=200, prefix="T"):
"""送 Connect 指令的平均耗時(ms)。
刻意選 `New / */Satellite`:它讓 STK 真的做事,所以這一列量到的
主要是 STK 自己的成本 —— 與讀屬性那一列對照,才看得出傳輸佔多少。
"""
ex = root.ExecuteCommand if hasattr(root, "ExecuteCommand") else root.execute_command
t0 = time.time()
for i in range(n):
ex("New / */Satellite %s%d" % (prefix, i))
return (time.time() - t0) / n * 1000.0
def measure_engine(n_prop=400, n_cmd=200):
"""STK Engine:行程內,沒有 gRPC。"""
from agi.stk12.stkengine import STKEngine
app = STKEngine.StartApplication(noGraphics=True)
root = app.NewObjectRoot()
root.NewScenario("TransportEngine")
out = (bench_property(root, n_prop), bench_command(root, n_cmd, "E"))
app.ShutDown()
return out
def measure_runtime(n_prop=400, n_cmd=200, port=PORT):
"""STK Runtime:跨行程 gRPC。"""
from agi.stk12.stkruntime import STKRuntime
app = STKRuntime.StartApplication(grpc_host="0.0.0.0", grpc_port=port,
grpc_timeout_sec=90, noGraphics=True)
root = app.NewObjectRoot()
root.NewScenario("TransportRuntime")
out = (bench_property(root, n_prop), bench_command(root, n_cmd, "R"))
app.ShutDown()
return out
def main():
print("=" * 78)
print(" STK 三種傳輸 — 定位與實測成本")
print("=" * 78)
print()
print_table()
if "--table" in sys.argv:
print()
print(" (--table:只印定位表,不連 STK)")
return 0
print()
print(" 量測中(會依序啟動 STK Engine 與 STK Runtime)...")
try:
com_prop, com_cmd = measure_engine()
except Exception as e: # noqa: BLE001
print(" STK Engine 起不來:%s" % str(e)[:100])
return 1
try:
g_prop, g_cmd = measure_runtime()
except Exception as e: # noqa: BLE001
print(" STK Runtime 起不來:%s" % str(e)[:100])
return 1
print()
print(" %-18s %14s %14s %12s" % ("操作", "行程內 COM", "跨行程 gRPC", "gRPC / COM"))
print(" %-18s %11.4f ms %11.4f ms %11.1fx"
% ("讀一個屬性", com_prop, g_prop, g_prop / com_prop))
print(" %-18s %11.4f ms %11.4f ms %11.1fx"
% ("送 Connect 指令", com_cmd, g_cmd, g_cmd / com_cmd))
print()
print(" 讀屬性幾乎不做事,量到的就是傳輸成本 -> gRPC 慢 %.0f 倍。"
% (g_prop / com_prop))
print(" Connect 指令由 STK 自己主導,傳輸沉在雜訊裡 -> 兩者幾乎相同。")
print(" => 「gRPC 比 COM 快」是反的;gRPC 換到的是**位置自由**,不是速度。")
io.open(TRANSPORT_OUT, "w", encoding="utf-8", newline="\n").write(
json.dumps({"com_property_ms": com_prop, "grpc_property_ms": g_prop,
"com_command_ms": com_cmd, "grpc_command_ms": g_cmd,
"property_ratio": g_prop / com_prop,
"command_ratio": g_cmd / com_cmd,
"transports": [list(t) for t in TRANSPORTS]},
ensure_ascii=False, indent=1) + "\n")
print()
print(" 量測值已存到 %s" % os.path.basename(TRANSPORT_OUT))
return 0
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/grpc_link_udp_recv.m
function [link_ok, elev_deg, age_s, t_fb] = grpc_link_udp_recv(t_now, port)
% GRPC_LINK_UDP_RECV 取回 Python 橋接送回來的最新一筆回饋。**不阻塞。**
%
% 回饋封包格式與 grpc_link_closedloop.py 的 FB_FRAME 必須一致:
%
% 3 個 float64,little-endian,共 24 bytes:t(秒) link_ok(0/1) 仰角(deg)
%
% ## 為什麼一定要「不阻塞」而且「只取最新一筆」
%
% 閉迴路裡 Simulink 這一側是**主時鐘**:它每一步都要往前走,不能停下來
% 等網路。所以這支的規則是:
%
% * 緩衝區裡有幾筆就全部讀掉,**只留最後一筆**。
% 留舊的沒有意義 —— 回饋是「現在的狀態」,不是一個佇列。
% * 一筆都沒有就沿用上一次的值,並把 `age_s` 加上一步。
% **沿用是刻意的**:回饋掉一拍就把 link_ok 當成 false,
% 會讓飛機因為一個掉包而亂轉。
% * `age_s` 一定要回傳出去。老掉的回饋和沒有回饋是兩回事,
% 而控制律有權知道自己用的是多舊的資訊。
% **把 age 藏起來是這類橋接最常見的坑**:迴路看起來在跑,
% 其實用的是三十秒前的判斷,而且沒有任何跡象。
%
% ## 收工
%
% grpc_link_udp_recv('close');
%
% ## 驗證狀態
%
% 已與 `grpc_link_closedloop.py --udp` 實測對接過(見 GrpcLink_Simulink.md)。
persistent u last_ok last_elev last_t last_age;
if nargin == 1 && (ischar(t_now) || isstring(t_now))
if strcmpi(string(t_now), "close")
if ~isempty(u), clear u; u = []; end
last_ok = []; last_elev = []; last_t = []; last_age = [];
link_ok = true; elev_deg = NaN; age_s = NaN; t_fb = NaN;
return
end
error('grpc_link_udp_recv: 不認得的控制指令 "%s"', string(t_now));
end
if nargin < 2 || isempty(port), port = 41001; end
if isempty(u)
% 'byte' 模式 = 原始位元組。
% **Timeout 不能設 0** —— udpport 會擋下來
%("Expected TIMEOUT to be positive")。給一個極小的正值就好:
% 本支一律先檢查 NumBytesAvailable 再讀,從不走阻塞的路徑,
% 所以這個值實際上不會被用到。
u = udpport("byte", "LocalPort", port, "Timeout", 0.001);
last_ok = true; % 開場假設可用 —— 還沒有任何回饋可依據
last_elev = NaN;
last_t = NaN;
last_age = 0;
end
got = false;
% 把緩衝區清空,只留最後一筆。**不要一次只讀一筆** ——
% Python 那側可能送得比 Simulink 的步長快,佇列會越積越舊。
while u.NumBytesAvailable >= 24
raw = read(u, 24, "uint8");
v = typecast(uint8(raw), 'double'); % [t, link_ok, elev]
last_t = v(1);
last_ok = v(2) >= 0.5;
last_elev = v(3);
got = true;
end
if got
last_age = 0;
else
% 沒有新回饋:沿用舊值,但把年齡記上去
last_age = last_age + 1;
end
link_ok = last_ok;
elev_deg = last_elev;
age_s = last_age;
t_fb = last_t;
if nargin >= 1 && isnumeric(t_now) && ~isempty(t_now) && ~isnan(last_t)
% 用送出去的時戳算真正的延遲(秒),比「掉了幾拍」精確
age_s = t_now - last_t;
end
end
grpc_link/grpc_link_udp_send.m
function n = grpc_link_udp_send(t, lat, lon, alt_km, host, port)
% GRPC_LINK_UDP_SEND 把一筆狀態用 UDP 送給 Python 橋接程式。
%
% 封包格式與 grpc_link_simulink.py 的 FRAME 必須一致:
%
% 4 個 float64,little-endian,共 32 bytes:t(秒) lat(deg) lon(deg) alt(km)
%
% **代價要講清楚**:UDP 不保證送達也不保證順序,所以 Python 那側
% 一律用封包裡的 t 當時間依據,不看到達順序;漏包由 t 的缺口算出來。
%
% 為什麼用 udpport 而不是 Simulink 的 UDP Send 區塊:
% 兩個都行,而這一支是給「不想在模型裡放工具箱區塊」的情況用的 ——
% 它是純 .m,可以從 MATLAB 命令列直接測,也可以被 MATLAB Function
% 區塊用 coder.extrinsic 呼叫。先用它把介接調通,再決定要不要換成區塊。
%
% socket 用 persistent 保存:**每一步都開一個新 socket 會把埠耗光**,
% 而且在 Windows 上會留下一堆 TIME_WAIT。開一次、重複用。
% 要關掉請呼叫 grpc_link_udp_send('close')。
%
% 用法:
% grpc_link_udp_send(0, 24.8, 121.0, 1.5); % 送一筆
% grpc_link_udp_send('close'); % 收工
%
% 回傳送出的位元組數(關閉時回 0)。
%
% ## 驗證狀態
%
% **已在 MATLAB R2021b 實際執行過。** 單獨呼叫回傳 32 bytes,而同時掛在
% UDP :41000 的 Python 橋接(`grpc_link_simulink.py --udp`)確實收到了那一點 ——
% 所以 MATLAB -> Python 這一段是**端到端驗過的**,不只是編譯得過。
%
% 位元組排列因此也一併驗到了:橋接用 `struct.unpack('<4d', ...)` 解出來的
% 值能對上送出去的值,表示兩側的 little-endian IEEE-754 雙精度排列一致。
%
% 想自己再確認一次的話很容易 —— 在 MATLAB 裡:
%
% typecast(double([0 24.8 121.0 1.5]), 'uint8')
%
% 在 Python 裡:
%
% list(struct.pack('<4d', 0.0, 24.8, 121.0, 1.5))
%
% 兩串 32 個數字要一模一樣。**不一樣的話先修這裡,不要往下查 STK** ——
% 位元組錯位會讓軌跡變成亂數,而症狀看起來像「STK 收到了但畫錯」。
persistent u;
% 收工:單一字串參數當控制指令
if nargin == 1 && (ischar(t) || isstring(t))
if strcmpi(string(t), "close")
if ~isempty(u)
clear u; % udpport 物件解構時會關掉 socket
u = [];
end
n = 0;
return
end
error('grpc_link_udp_send: 不認得的控制指令 "%s"', string(t));
end
if nargin < 5 || isempty(host), host = '127.0.0.1'; end
if nargin < 6 || isempty(port), port = 41000; end
if isempty(u)
% 'byte' 模式 = 原始位元組,不做任何字串轉換。
u = udpport("byte", "LocalPort", 0);
end
% typecast 到 uint8 就是 little-endian 的 IEEE-754 雙精度 ——
% 這正是 Python 那側 struct.unpack('<4d', ...) 期待的排列。
% **不要用 sprintf 送文字**:兩側的小數位數一旦不一致,
% 誤差會悄悄進到軌跡裡,而且事後查不出來。
payload = typecast(double([t, lat, lon, alt_km]), 'uint8');
assert(numel(payload) == 32, '封包必須是 32 bytes,實際 %d', numel(payload));
write(u, payload, "uint8", host, port);
n = numel(payload);
end
grpc_link/grpc_link_verify.py
# -*- coding: utf-8 -*-
r"""STK 與 gRPC 通聯測試 — 驗證段。
本章要驗的東西大半根本不在 STK 裡:協定常數、API 名稱、診斷邏輯、
封包格式、攢批算術。把它們綁在授權上,只會讓最該常跑的檢查變成最少跑的。
分六類,而第 3 類是這一段的重心:
1. **協定與格式常數** —— 外部事實,不是本章算出來的
gRPC 預設埠 40704(去內省兩代 API 拿,不手抄)、
gRPC 預設訊息上限 4 MiB、封包 32 bytes
2. **API 對照表與裝好的套件一致** —— 純內省
兩代的名字都在;流傳範例的錯名確實不存在、校正後的確實存在
3. **診斷梯真的分得出成因** —— **失敗注入**
這一類是本章的核心主張(「第 1 層的拒絕與逾時是兩件不同的事」)。
所以不能只讀程式碼確認它寫了 if,要**真的造出四種失敗**,
看它有沒有指到對的成因。其中「埠有人聽但不是 gRPC」這一項
靠本檔自己起一個只 listen 不說話的 socket 來造。
4. **plant 對閉合式** —— 獨立對照
協調轉彎的地面軌跡半徑 R = V^2/(g tan phi) 是閉合式,
拿它對 plant 積出來的軌跡,不是拿 plant 對自己。
5. **橋接的算術與統計** —— 失敗注入
攢批的往返數;漏包統計要在**刻意丟包**之下報對數字。
6. **時戳** —— 絕對時間,不是相對時間
7. **MAVLink / ArduPilot 併入** --- 預測界驗證
GLOBAL_POSITION_INT 的整數欄位造成的量化誤差**事先算得出來**,
所以判準是那個界:超出是 bug,而**剛好為零也可疑**(表示量化沒發生,
可能根本沒走 MAVLink 編碼)。
第 1、2、4、7 類是外部對照,第 3、5 類是失敗注入。
**只有第 1 類以外的「程式自己算給程式看」不算驗證** ——
這是這個教材庫反覆踩過的坑。
執行:python grpc_link_verify.py
離開碼 0 = 全部通過,1 = 有項目對不上
"""
import inspect
import math
import os
import socket
import struct
import sys
import threading
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
sys.path.insert(0, HERE)
sys.path.insert(0, os.path.join(HERE, os.pardir, "tools"))
from stk_verify import Verifier # noqa: E402
import grpc_link_api as API # noqa: E402
import grpc_link_ardupilot as AP # noqa: E402
import grpc_link_closedloop as CL # noqa: E402
import grpc_link_comm as CM # noqa: E402
import grpc_link_fleetloop as FL # noqa: E402
import grpc_link_gps as GPS # noqa: E402
import grpc_link_sixdof as SD # noqa: E402
G0_LOCAL = 9.80665
import grpc_link_swarm as SW # noqa: E402
import grpc_link_mavsim as MS # noqa: E402
import grpc_link_terrain as TR # noqa: E402
import grpc_link_integration as I # noqa: E402
import grpc_link_bench as B # noqa: E402
import grpc_link_probe as P # noqa: E402
import grpc_link_realtime as R # noqa: E402
import grpc_link_simulink as S # noqa: E402
def free_port():
"""拿一個確定沒人在聽的埠:綁了就馬上放掉。
比隨便挑一個數字可靠 —— 硬寫的埠號有一天會撞到別的服務,
那時這一項會變成隨機失敗,而看起來像診斷邏輯壞了。
"""
s = socket.socket()
s.bind(("127.0.0.1", 0))
port = s.getsockname()[1]
s.close()
return port
class DumbListener:
"""只 accept 不說話的 TCP 伺服器 —— 用來造「埠有人聽但不是 gRPC」。
這是第 3 類裡最有價值的一項:它是**唯一**能把
「伺服器沒開」與「開錯了東西」分開的測法,而這兩種的處置完全不同。
"""
def __init__(self):
self.sock = socket.socket()
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.sock.bind(("127.0.0.1", 0))
self.sock.listen(8)
self.port = self.sock.getsockname()[1]
self._stop = False
self._th = threading.Thread(target=self._serve, daemon=True)
self._th.start()
def _serve(self):
self.sock.settimeout(0.5)
held = []
while not self._stop:
try:
c, _a = self.sock.accept()
held.append(c) # 收下連線然後**什麼都不說**
except socket.timeout:
continue
except OSError:
break
for c in held:
try:
c.close()
except OSError:
pass
def close(self):
self._stop = True
try:
self.sock.close()
except OSError:
pass
def main():
v = Verifier(None)
# ---------------- 1. 協定與格式常數(外部事實)----------------
# 埠號不手抄:去兩代 API 的簽章裡拿。手抄的常數會跟著套件改版過期。
for label, path in (
("舊代 agi.stk12", "agi.stk12.stkruntime:STKRuntime.AttachToApplication"),
("新代 PySTK",
"ansys.stk.core.stkruntime:STKRuntime.attach_to_application")):
found, fn = API.resolve(path)
if not found:
v.custom("%s 的 grpc_port 預設值" % label, None, "套件沒裝,略過")
continue
got = inspect.signature(fn).parameters["grpc_port"].default
v.expect("%s 的 grpc_port 預設 = 40704" % label, got,
P.DEFAULT_PORT, tol=0)
# 流傳範例常誤用 gRPC 教學埠 50051 —— 兩者必須不同,不然這一章的
# 校正表就沒有意義了
v.expect_true("STK 的預設埠與 gRPC 教學埠 50051 不同",
API.STK_DEFAULT_GRPC_PORT != API.GRPC_TUTORIAL_PORT,
"%d vs %d" % (API.STK_DEFAULT_GRPC_PORT,
API.GRPC_TUTORIAL_PORT))
# gRPC 的預設訊息上限是 4 MiB(gRPC 的文件值,不是 STK 訂的)
v.expect("gRPC 預設單一訊息上限 = 4 MiB",
B.GRPC_DEFAULT_MAX_MESSAGE, 4 * 1024 * 1024, tol=0, unit=" bytes")
# 封包格式:4 個 float64 必須剛好 32 bytes,而且要能原封不動往返
v.expect("UDP 封包長度 = 32 bytes", S.FRAME_SIZE, 32, tol=0)
probe_vals = (12.5, 24.8, 121.0, 1.5)
back = struct.unpack(S.FRAME, struct.pack(S.FRAME, *probe_vals))
v.expect_true("封包 pack/unpack 往返不失真", back == probe_vals,
"%s" % (back,))
# ---------------- 2. API 對照表與裝好的套件一致 ----------------
missing = [lab for lab, o, n in API.check_renames() if not (o and n)]
v.expect_true("兩代 API 的對應名稱都存在(%d 組)" % len(API.RENAMES),
not missing, "缺:%s" % missing if missing else "全部都在")
stale = [lab for lab, _b, _bf, _g, _gf, ok in API.check_myths() if not ok]
v.expect_true("流傳範例的校正表仍然成立(%d 項)" % len(API.MYTHS),
not stale, "要改:%s" % stale if stale else "錯的不在、對的在")
for lab, param, has_new, has_old in API.check_new_only():
v.expect_true("%s(%s)只有新代才有" % (lab, param),
has_new and not has_old,
"新代 %s / 舊代 %s" % (has_new, has_old))
# ---------------- 3. 診斷梯真的分得出成因(失敗注入)----------------
# (a) 沒人在聽 -> 必須判「refused」
port = free_port()
r = P.layer_tcp("127.0.0.1", port) # 用預設 timeout(10 s)
v.expect_true("注入『伺服器沒開』-> 判為 refused",
r["ok"] is False and r["cause"] == "refused",
"cause=%s detail=%s" % (r["cause"], r["detail"]))
# 真正的判別證據:被拒是**提早**回來(遠早於 timeout),逾時是**剛好等滿**。
# 不是「瞬間」—— Windows 回報被拒本身就要約 2 秒(見 probe 檔頭的量測)。
v.expect_true("被拒絕明顯早於 timeout 就回來(不是等滿)",
r["ms"] is not None
and r["ms"] < 0.6 * P.DEFAULT_TCP_TIMEOUT * 1000.0,
"%.0f ms vs timeout %.0f ms"
% (r["ms"] or -1, P.DEFAULT_TCP_TIMEOUT * 1000.0))
# 預設 timeout 必須留足餘裕,否則關閉的埠會被誤診成防火牆丟包
v.expect_true("預設 timeout 對 OS 回報『被拒』留有餘裕",
P.DEFAULT_TCP_TIMEOUT
>= P.REFUSE_REPORT_S * P.REFUSE_MARGIN,
"%.1fs vs 需要 > %.2fs"
% (P.DEFAULT_TCP_TIMEOUT, P.REFUSE_REPORT_S * P.REFUSE_MARGIN))
# (a2) **回歸測試**:timeout 調到餘裕不足時,關閉的埠曾被誤診成 filtered。
# 修好之後它必須回報「分不出來」,而不是給一個聽起來很確定的錯答案。
r_short = P.layer_tcp("127.0.0.1", free_port(), timeout=1.0)
v.expect_true("注入『timeout 太短』-> 不謊報 filtered,而是 timeout_too_short",
r_short["ok"] is False
and r_short["cause"] == "timeout_too_short",
"cause=%s" % r_short["cause"])
# (b) 名稱解不出 -> 必須判「dns」
r = P.layer_tcp("no-such-host.invalid", P.DEFAULT_PORT, timeout=5.0)
v.expect_true("注入『host 解不出』-> 判為 dns",
r["ok"] is False and r["cause"] == "dns",
"cause=%s" % r["cause"])
# (c) 埠有人聽但不是 gRPC -> TCP 要過、通道要不過,判「not_grpc」
dumb = DumbListener()
try:
r1 = P.layer_tcp("127.0.0.1", dumb.port, timeout=3.0)
v.expect_true("注入『非 gRPC 伺服器』-> 第 1 層 TCP 仍然通",
r1["ok"] is True, r1["detail"])
r2 = P.layer_channel("127.0.0.1", dumb.port, timeout=3.0)
v.expect_true("注入『非 gRPC 伺服器』-> 第 2 層判為 not_grpc",
r2["ok"] is False and r2["cause"] == "not_grpc",
"cause=%s detail=%s" % (r2["cause"], r2["detail"]))
finally:
dumb.close()
# (d) 成因表要有話可說 —— 診斷指到一個沒有說明的成因等於沒診斷
causes_used = {"deps_missing", "refused", "filtered", "dns", "not_grpc",
"handshake", "om_failed", "no_stk_api"}
v.expect_true("每一個成因都有對應的處置說明",
causes_used <= set(P.CAUSES),
"缺:%s" % (causes_used - set(P.CAUSES)))
# (e) 整體結論的門檻:沒測到物件模型就**不能**算通
part = [P._record("依賴", True), P._record("TCP", True),
P._record("通道", True)]
ok_partial, bad = P.verdict(part)
v.expect_true("只到第 2 層不算通聯成功(--no-stk 不能謊報成功)",
ok_partial is False and bad is None,
"verdict=%s" % ok_partial)
full = part + [P._record("交握", True), P._record("物件模型", True)]
v.expect_true("到第 4 層才算通", P.verdict(full)[0] is True)
# 第 5 層是能力清單,不是是非題 —— 它回 None 不該把結論拉紅
full5 = full + [P._record("Connect 面", None, "3/7 ACK")]
v.expect_true("第 5 層(能力面)不影響通聯結論",
P.verdict(full5)[0] is True)
# ---------------- 4. plant 對閉合式(獨立對照)----------------
t, lat, lon, alt = R.plant()
r_closed = R.turn_radius()
r_fit = R.fitted_radius(lat, lon)
v.expect("協調轉彎半徑:擬圓 vs 閉合式 V^2/(g tan phi)",
r_fit, r_closed, tol=0.01 * r_closed, unit=" m")
# 閉合式本身也要對得上獨立算的一次:V=120、phi=15 deg
v.expect("閉合式半徑(V=120 m/s, phi=15 deg)", r_closed,
120.0 ** 2 / (9.80665 * math.tan(math.radians(15.0))),
tol=1e-6, unit=" m")
# 爬升角固定 -> 高度必須嚴格線性遞增
dh = [alt[i + 1] - alt[i] for i in range(len(alt) - 1)]
v.expect_true("固定爬升角 -> 高度等差遞增",
max(dh) - min(dh) < 1e-12 and dh[0] > 0,
"每步 %.6f km" % dh[0])
# 轉彎一圈的週期 = 2 pi V / (g tan phi):拿它驗 psi 的積分
t_turn = 2 * math.pi * R.V_TAS / (R.G0 * math.tan(math.radians(R.BANK_DEG)))
v.expect_between("轉一圈的時間(2 pi V / (g tan phi))", t_turn,
280.0, 300.0, unit=" s")
v.expect_true("plant 長度與步長一致", len(t) == R.N_STEPS
and abs(t[1] - t[0] - R.DT) < 1e-12,
"%d 點,步長 %.3f s" % (len(t), t[1] - t[0]))
# ---------------- 5. 橋接的算術與統計(失敗注入)----------------
# 攢批:n 點、chunk c -> ceil(n/c) 次往返
for n, c in ((117, 20), (120, 10), (7, 1)):
br = S.Bridge(None, c)
for i in range(n):
br.feed((i * R.DT, 24.8, 121.0, 1.5))
br.flush()
want = -(-n // c) # ceil 除法
v.expect("攢批 %d 點 / chunk %d -> 往返數" % (n, c),
br.report()["grpc_calls"], want, tol=0)
# 漏包統計:**刻意**挖掉 5 個時間點,統計要報 5
br = S.Bridge(None, 10)
holes = {3, 11, 12, 40, 77}
for i in range(120):
if i in holes:
continue
br.feed((i * R.DT, 24.8, 121.0, 1.5))
br.flush()
v.expect("注入 5 個漏包 -> 漏包統計", br.report()["gaps"], len(holes), tol=0)
# 亂序送達不該被誤判成漏包 —— UDP 不保證順序
br = S.Bridge(None, 10)
order = list(range(60))
order[10], order[40] = order[40], order[10]
for i in order:
br.feed((i * R.DT, 24.8, 121.0, 1.5))
br.flush()
v.expect("亂序送達不算漏包(統計靠 t,不靠到達順序)",
br.report()["gaps"], 0, tol=0)
# chunk 1 必須退回逐點
br = S.Bridge(None, 1)
for i in range(9):
br.feed((i * R.DT, 24.8, 121.0, 1.5))
br.flush()
v.expect("chunk 1 退回逐點", br.report()["grpc_calls"], 9, tol=0)
# ---------------- 6. 時戳 ----------------
ts = R.timestamps([0.0, 1.0, 59.5, 3600.0])
v.expect_true("時戳是絕對時間,不是 '+N sec' 相對時間",
all(not s.startswith("+") for s in ts), ts[0])
v.expect_true("時戳跨分鐘正確進位", ts[2].endswith("09:00:59.500"), ts[2])
v.expect_true("時戳跨小時正確進位", ts[3].endswith("10:00:00.000"), ts[3])
v.expect_true("時戳與輸入等長且遞增",
len(ts) == 4 and ts == sorted(ts), "%d 筆" % len(ts))
# ---------------- 7. MAVLink / ArduPilot 併入(預測界驗證)----------------
# 這一類的判準是**事先算得出來的量化界**,不是量完回頭湊的容差。
# GLOBAL_POSITION_INT 的 lat/lon 是 int32、單位 1e-7 度,所以誤差有上界。
v.expect("MAVLink lat/lon 刻度 = 1e-7 度", AP.DEG_E7, 1e-7, tol=0)
v.expect("MAVLink alt 刻度 = 1 公釐", AP.MM, 1e-3, tol=0)
bound = AP.quantization_bound_m()
# 手算:緯度 1e-7 x 111320 = 1.1132 cm;經度再乘 cos(24.8 deg);取向量和
want = math.hypot(1e-7 * 111320.0,
1e-7 * 111320.0 * math.cos(math.radians(R.LAT0)))
v.expect("量化界與手算一致", bound, want, tol=1e-9, unit=" m")
v.expect_between("量化界落在公分等級(整數欄位的必然後果)", bound,
0.010, 0.020, unit=" m")
# 真的用 pymavlink 編碼再解碼 —— 驗的是**線上格式**,不是本檔的算術
try:
from pymavlink import mavutil # noqa: F401
worst, b, ngot = AP.compare_paths(n=40)
v.expect_true("MAVLink 編解碼收到預期點數", ngot == 40, "%d 點" % ngot)
# 兩個方向都要驗:**超出界是 bug,剛好是零也可疑**
v.expect_true("MAVLink 往返誤差不超過預測的量化界",
worst <= b * 1.05,
"%.4f m vs 界 %.4f m" % (worst, b))
v.expect_true("MAVLink 往返誤差不為零(為零表示量化沒發生)",
worst > 0.0, "%.4f m" % worst)
except ImportError:
v.custom("MAVLink 編解碼往返", None, "pymavlink 沒裝,略過")
# 橋接吃 MAVLink 與吃 Simulink 封包用的是**同一個 Bridge** ——
# 這是本章架構主張的自我檢查:換資料來源,下游不動。
v.expect_true("ArduPilot 與 Simulink 共用同一個 Bridge 類別",
AP.S.Bridge is S.Bridge,
"%s" % AP.S.Bridge.__module__)
# ---------------- 8. 四方整合:每條路的預期差各自不同 ----------------
# **用同一個容差套三條路,等於什麼都沒驗。** 每條路的有損步驟不一樣,
# 所以它的預期差也不一樣,而且都算得出來。
cb = I.csv_bound_m()
v.expect("CSV(%%.%df)的量化界與手算一致" % I.CSV_DECIMALS, cb,
math.hypot(1e-9 * AP.M_PER_DEG_LAT,
1e-9 * AP.M_PER_DEG_LAT * math.cos(math.radians(R.LAT0))),
tol=1e-12, unit=" m")
# 兩個界的比值**正好是 100**:CSV 量化在 1e-9 度,MAVLink 在 1e-7 度,
# 而兩者的幾何換算(緯度、經度、取向量和)完全相同,所以比值只剩刻度比。
# 寫成「小兩個數量級以上」(嚴格大於 100 倍)會放掉這個關係 ——
# 這裡該驗的是等式,不是不等式。
v.expect("CSV 界 / MAVLink 界 = 刻度比 1e-9 / 1e-7 = 1/100",
AP.quantization_bound_m() / cb, 100.0, tol=1e-9)
direct = I.gather_direct(n=40)
sim, _path = I.gather_simulink(n=40)
d_sim = I.worst_offset_m(direct, sim)
v.expect_true("直接 vs Simulink(CSV)落在 CSV 的量化界內",
d_sim <= cb * 1.05, "%.6f m vs 界 %.6f m" % (d_sim, cb))
# 同一條軌跡跟自己比必須是 0 —— 若不是,worst_offset_m 本身就壞了
v.expect("軌跡與自己比對必須是 0(驗比對函式本身)",
I.worst_offset_m(direct, direct), 0.0, tol=0.0, unit=" m")
# 而刻意偏移 1e-6 度的軌跡必須被抓出來 —— 驗它抓得到差
shifted = [(t, la + 1e-6, lo, al) for t, la, lo, al in direct]
v.expect("注入 1e-6 度偏移 -> 比對函式要報出約 0.111 m",
I.worst_offset_m(direct, shifted), 1e-6 * AP.M_PER_DEG_LAT,
tol=1e-6, unit=" m")
# ---------------- 9. GPS 可用度:數學恆等式 + 外部對照 ----------------
# 這一組**完全不需要 STK**:DOP 是純幾何,用合成的衛星方向就驗得動。
import numpy as _np
# (a) ENU 基底必須是單位正交 —— 這是定義,不是近似
e, nn, u = GPS.enu_basis(R.LAT0, R.LON0)
for lab, vec in (("E", e), ("N", nn), ("U", u)):
v.expect("ENU 基底 %s 是單位向量" % lab,
float(_np.linalg.norm(vec)), 1.0, tol=1e-12)
for lab, a, b in (("E·N", e, nn), ("N·U", nn, u), ("U·E", u, e)):
v.expect("ENU 基底正交 %s" % lab, float(_np.dot(a, b)), 0.0, tol=1e-12)
# (b) 合成幾何:四顆均勻分布 + 一顆天頂。DOP 的兩條恆等式必須成立。
def _sv(az_deg, el_deg, rng_km=20200.0):
"""從接收機看出去的方向造一顆衛星(ECEF)。"""
ee, n2, uu = GPS.enu_basis(R.LAT0, R.LON0)
a, el = math.radians(az_deg), math.radians(el_deg)
d = (math.cos(el) * math.sin(a) * ee
+ math.cos(el) * math.cos(a) * n2
+ math.sin(el) * uu)
return GPS.ecef_from_geodetic(R.LAT0, R.LON0, 0.0) + rng_km * d
good = _np.array([_sv(0, 70), _sv(90, 20), _sv(180, 25),
_sv(270, 20), _sv(45, 45)])
nvis, dop, _el = GPS.visible_and_dop(R.LAT0, R.LON0, 0.0, good, 10.0)
v.expect("合成幾何的可見顆數", nvis, 5, tol=0)
ok_id, (r1, r2) = GPS.dop_identities(dop)
v.expect_true("DOP 恆等式 GDOP²=PDOP²+TDOP²、PDOP²=HDOP²+VDOP²",
ok_id, "殘差 %.2e / %.2e" % (r1, r2))
# 良好幾何的 GDOP 應落在公開常引用的 1.5-6(開闊天空、完整星座)
v.expect_between("良好幾何的 GDOP(公開典型 1.5-6)", dop["GDOP"],
1.5, 6.0)
# HDOP <= PDOP <= GDOP —— 由恆等式直接推得的排序,不是經驗
v.expect_true("HDOP <= PDOP <= GDOP",
dop["HDOP"] <= dop["PDOP"] <= dop["GDOP"],
"%.3f <= %.3f <= %.3f"
% (dop["HDOP"], dop["PDOP"], dop["GDOP"]))
# (c) **不足四顆要回 None,不是回一個很大的數字。**
# 「無解」與「解很差」是兩件事,把前者偽裝成後者,下游就分不出來。
three = good[:3]
n3, dop3, _e3 = GPS.visible_and_dop(R.LAT0, R.LON0, 0.0, three, 10.0)
v.expect_true("只有 3 顆 -> 回 None(無解,不是解很差)",
dop3 is None and n3 == 3, "n=%d dop=%s" % (n3, dop3))
# (d) 遮蔽角要真的把低仰角的衛星擋掉
low = _np.array([_sv(0, 70), _sv(90, 5), _sv(180, 5),
_sv(270, 5), _sv(45, 45)])
n_low, _d, _e = GPS.visible_and_dop(R.LAT0, R.LON0, 0.0, low, 30.0)
v.expect("遮蔽角 30 度擋掉 5 度仰角的三顆", n_low, 2, tol=0)
# (e) 退化幾何(全部擠在同一個方向)不能給出一個看起來正常的數字
deg = _np.array([_sv(0, 80), _sv(1, 80), _sv(2, 80), _sv(3, 80),
_sv(4, 80)])
_n, dop_deg, _e = GPS.visible_and_dop(R.LAT0, R.LON0, 0.0, deg, 10.0)
v.expect_true("退化幾何 -> 回 None 或極大的 GDOP(不能看起來正常)",
dop_deg is None or dop_deg["GDOP"] > 50.0,
"GDOP=%s" % (None if dop_deg is None
else "%.1f" % dop_deg["GDOP"]))
# (f) 公開的 GPS 常數要自洽:半長軸 = 地球半徑 + 標稱高度
v.expect("GPS 標稱半長軸 = 地球半徑 + 標稱高度",
GPS.GPS_SMA_KM, R.RE_M / 1000.0 + GPS.GPS_ALT_KM,
tol=30.0, unit=" km")
# 克卜勒第三定律:由半長軸算出的週期要對上公開的 11 小時 58 分
mu = 398600.4418
t_kep = 2 * math.pi * math.sqrt(GPS.GPS_SMA_KM ** 3 / mu)
v.expect("由標稱半長軸算的週期 vs 公開值(約 11 h 58 m)",
t_kep, GPS.GPS_PERIOD_S, tol=60.0, unit=" s")
# (g) 閉迴路的控制律:兩個方向都要對
v.expect("GPS 可用 -> 維持坡度角",
GPS.guidance_gps(True), CL.BANK_DEG, tol=0, unit=" deg")
v.expect("GPS 不可用 -> 停止轉彎(航位推算)",
GPS.guidance_gps(False), 0.0, tol=0, unit=" deg")
v.expect("地面站斷鏈 -> 反向轉彎",
CL.guidance(False), -CL.BANK_DEG, tol=0, unit=" deg")
# ---------------- 10. RF 與地形的純函式(不需 STK)----------------
# (a) 自由空間路徑損耗:兩條**定義的直接後果**,對就是對
base = CM.fspl_db(10.0)
v.expect("距離加倍 -> FSPL +6.0206 dB",
CM.fspl_db(20.0) - base, 20.0 * math.log10(2.0), tol=1e-12,
unit=" dB")
v.expect("頻率加倍 -> FSPL +6.0206 dB",
CM.fspl_db(10.0, CM.FREQ_HZ * 2) - base, 20.0 * math.log10(2.0),
tol=1e-12, unit=" dB")
# 手算對照:1 km、2.4 GHz
want = 20.0 * math.log10(4.0 * math.pi * 1000.0 * 2.4e9 / CM.C_LIGHT)
v.expect("FSPL(1 km, 2.4 GHz)與手算一致", CM.fspl_db(1.0, 2.4e9), want,
tol=1e-12, unit=" dB")
# 光速用定義值,不是近似值 —— 差 0.1% 就會在 FSPL 上留下 0.01 dB
v.expect("光速用的是定義值 299,792,458 m/s", CM.C_LIGHT, 299792458.0, tol=0)
# (b) 雙線性內插:格點上要**完全**等於格點值,格點之間要線性
import numpy as _np2
from affine import Affine
grid = _np2.array([[0.0, 10.0], [20.0, 30.0]], dtype=float)
# 1 度一格,左上角在 (lon=0, lat=1)
tf = Affine(1.0, 0.0, 0.0, 0.0, -1.0, 1.0)
v.expect("內插在格點上等於格點值", TR.bilinear(grid, tf, 0.5, 0.5),
0.0, tol=1e-12)
v.expect_true("格點之間是線性的(中點 = 四角平均)",
TR.bilinear(grid, tf, 0.0, 1.0) is None
or abs(TR.bilinear(grid, tf, 0.0, 1.0) - 15.0) < 1e-9,
"%s" % TR.bilinear(grid, tf, 0.0, 1.0))
v.expect_true("超出格網 -> 回 None(不外插)",
TR.bilinear(grid, tf, 9.0, 9.0) is None,
"%s" % TR.bilinear(grid, tf, 9.0, 9.0))
# (c) 涵蓋範圍守衛:**出界時 STK 的 ComputeAccess 不會回來**,
# 所以這個守衛是拿來擋住一個「不報錯、直接停住」的失敗
class _B:
left, right, bottom, top = 118.0, 118.5, 24.4, 24.6
v.expect_true("範圍內的點通過守衛", TR.inside(_B, 24.5, 118.25))
v.expect_true("範圍外的點被擋下", not TR.inside(_B, 24.8, 121.0))
v.expect_true("貼著邊界的點也被擋下(留了邊)",
not TR.inside(_B, 24.4001, 118.25))
# ---------------- 11. 外部 MAVLink 模擬器的整合(不需模擬器)----------------
# 這一組驗的是**本檔這一側的算術與架構**,不需要模擬器在場。
# 模擬器真的在場時的對照(位置序列速度 vs VFR_HUD)在 grpc_link_mavsim.py 裡跑。
# (a) 從位置序列算速度:造一條**正北、每秒 10 公尺**的軌跡
v_true = 10.0
dlat = v_true / AP.M_PER_DEG_LAT
track = [(float(i), R.LAT0 + i * dlat, R.LON0, 1.5) for i in range(12)]
got = MS.track_speed(track)
v.expect("位置序列算出的地速(造一條 10 m/s 的軌跡)", got, v_true,
tol=1e-6, unit=" m/s")
# 正東同理,而且要記得乘 cos(lat) —— 漏掉就會偏 9%
dlon = v_true / (AP.M_PER_DEG_LAT * math.cos(math.radians(R.LAT0)))
track_e = [(float(i), R.LAT0, R.LON0 + i * dlon, 1.5) for i in range(12)]
v.expect("正東方向也要對(經度要乘 cos(lat))", MS.track_speed(track_e),
v_true, tol=1e-6, unit=" m/s")
# 點太少就回 None,不要硬算一個數字出來
v.expect_true("點數不足 -> 回 None(不硬算)",
MS.track_speed(track[:2]) is None,
"%s" % MS.track_speed(track[:2]))
# 時間跳掉的區間要略過,不要把它算成瞬移
jumped = track[:5] + [(100.0, R.LAT0 + 5 * dlat, R.LON0, 1.5)]
v.expect("時間跳掉的區間要略過(不算成瞬移)", MS.track_speed(jumped),
v_true, tol=1e-6, unit=" m/s")
# (b) 架構:這一支**沿用**既有的 Bridge,沒有另寫一套
# —— 這是「介面是資料,不是函式呼叫」那個主張的檢查
v.expect_true("MAVLink 模擬器這一節沿用同一個 Bridge",
MS.S.Bridge is S.Bridge, MS.S.Bridge.__module__)
v.expect_true("也沿用同一組 MAVLink 刻度常數",
MS.AP.DEG_E7 == AP.DEG_E7 and MS.AP.MM == AP.MM)
# (c) **每架一個 Bridge**,不共用 —— 這是多機成本下界的來源。
# 檢查方式是**真的建一個 Fleet**,不是去 grep 原始碼:
# grep 只證明那行字還在,證明不了行為。
try:
fl = MS.Fleet(3, gcs_base=25550).attach_dummy()
try:
bs = fl.bridges
v.expect("Fleet(3) 配出 3 個 Bridge", len(bs), 3, tol=0)
v.expect_true("三個 Bridge 是**不同的物件**(攢批不會跨載具合併)",
len({id(b) for b in bs}) == 3,
"%d 個相異物件" % len({id(b) for b in bs}))
v.expect_true("每個都是同一個 Bridge 類別",
all(isinstance(b, S.Bridge) for b in bs))
finally:
fl.close()
except ImportError:
v.custom("Fleet 為每架各配一個 Bridge", None, "pymavlink 沒裝,略過")
# (d) 模擬器路徑沒有預設值 —— 教材不附它
os_env = os.environ.pop("GRPC_LINK_MAVSIM", None)
try:
v.expect_true("沒設環境變數也沒給 --sim 時回 None(教材不附模擬器)",
MS.sim_path(["x"]) is None, "%s" % MS.sim_path(["x"]))
v.expect_true("--sim 優先於環境變數",
MS.sim_path(["x", "--sim", "P"]) == "P")
finally:
if os_env is not None:
os.environ["GRPC_LINK_MAVSIM"] = os_env
# ---------------- 12. 多機閉迴路的純函式與設計一致性 ----------------
# (a) 距離:正北 1000 公尺、正東 1000 公尺,各自對得上
dlat = 1000.0 / AP.M_PER_DEG_LAT
v.expect("正北 1000 m 的距離",
FL.dist_m(R.LAT0, R.LON0, R.LAT0 + dlat, R.LON0), 1000.0,
tol=1e-6, unit=" m")
dlon = 1000.0 / (AP.M_PER_DEG_LAT * math.cos(math.radians(R.LAT0)))
v.expect("正東 1000 m 的距離(要乘 cos(lat))",
FL.dist_m(R.LAT0, R.LON0, R.LAT0, R.LON0 + dlon), 1000.0,
tol=1e-6, unit=" m")
v.expect("同一點的距離是 0",
FL.dist_m(R.LAT0, R.LON0, R.LAT0, R.LON0), 0.0, tol=0.0, unit=" m")
# (b) 「有沒有往家飛」的判準:嚴格下降要過,上升要不過,
# 而且**容許少量回彈** —— 要求嚴格單調會因為 GPS 雜訊假紅,
# 然後人就會把它調鬆到失去意義。
# 序列要真的「回到家」:淨下降 > 2 x 待命繞圈半徑,**而且**最近距離
# 要降到起點的 40% 以下。這兩條都是 came_home 的判準,
# 所以測試資料也要照著寫:只降到 42% 的序列會被判成
# 「沒回到夠近的地方」,那是判準正常運作,不是 bug。
down = [1000.0 - 35.0 * i for i in range(30)]
ok_d, frac_d = FL.came_home(down)
v.expect_true("嚴格下降 -> 判為回家", ok_d and frac_d > 0.99,
"下降佔 %.2f" % frac_d)
up = [100.0 + 20.0 * i for i in range(30)]
ok_u, frac_u = FL.came_home(up)
v.expect_true("持續上升 -> 判為沒有回家", not ok_u,
"下降佔 %.2f" % frac_u)
# 下降中夾雜幾次回彈仍要過(這是真實訊號的樣子)
noisy = [1000.0 - 35.0 * i + (15.0 if i % 7 == 0 else 0.0)
for i in range(30)]
ok_n, frac_n = FL.came_home(noisy)
v.expect_true("下降中夾雜回彈仍判為回家", ok_n,
"下降佔 %.2f" % frac_n)
# 樣本太少不要下結論
v.expect_true("樣本太少 -> 不判定", not FL.came_home([5.0, 4.0])[0])
# **轉向那幾秒距離會先增加** —— 所以單調不是對的要求。
# 這一條餵一段「先遠離再回家」的序列,它必須判為回家。
turn = [500.0 + 15.0 * i for i in range(4)] + [560.0 - 25.0 * i for i in range(22)]
ok_turn, frac_turn = FL.came_home(turn)
v.expect_true("RTL 後先轉向(距離先增)仍判為回家", ok_turn,
"下降佔 %.2f,%.0f -> %.0f m"
% (frac_turn, turn[0], min(turn)))
# **抵達之後的待命尾巴不該稀釋判準** —— 這是實測修過的:
# 一段 463 -> 126 m 的回程,接上待命繞圈的尾巴之後下降佔比掉到 55%,
# 被判成「沒有往家飛」。回程在抵達時就結束了。
tail = ([1000.0 - 40.0 * i for i in range(20)] # 回程 1000 -> 240
+ [240.0 + 50.0 * math.sin(i) for i in range(40)]) # 待命擺盪
ok_t, frac_t = FL.came_home(tail)
v.expect_true("回程後接待命尾巴,仍判為回家", ok_t,
"回程段下降佔 %.2f" % frac_t)
# 純擺盪(淨下降不足)不能被當成回程
osc = [300.0 + 50.0 * math.sin(i) for i in range(40)]
v.expect_true("純待命擺盪 -> 不算回家(淨下降不足)",
not FL.came_home(osc)[0])
# (c) **遮蔽角與觀測時間要對得起來**。門檻挑得太低的話,
# 載具在收集時間內根本飛不到觸發距離,RTL 一次都不會觸發 ——
# 而那看起來會像「閉迴路沒作用」。
alt_m = FL.PATROL_ALT_M # 巡航高度(不是模擬器預設的 90 m,見 FL 檔頭)
trig_m = alt_m / math.tan(math.radians(FL.MIN_ELEV_DEG))
v.expect_between("遮蔽角換算的觸發距離(90 m 高度)", trig_m,
800.0, 1500.0, unit=" m")
cruise = MS.SIM_CRUISE_MS
t_out = trig_m / cruise
# 觀測窗要裝得下:等星曆長出來 + 出程 + 回程
need = FL.MIN_SPAN_S_FOR_ACCESS + 2.0 * t_out
v.expect_true("收集時間裝得下「等星曆 + 出程 + 回程」",
FL.COLLECT_S > need,
"需要 %.0f s,給了 %.0f s" % (need, FL.COLLECT_S))
v.expect_true("外派距離遠大於觸發距離",
FL.OUTBOUND_M > 2.0 * trig_m,
"外派 %.0f m vs 觸發 %.0f m" % (FL.OUTBOUND_M, trig_m))
# (d) **三態決策:通 / 斷 / 還算不出來。** 這是實測踩到的錯:
# 星曆太短時 compute_access() 回空清單,被讀成「鏈路斷了」,
# RTL 在載具離家 42 公尺時就觸發(那裡的仰角約 24 度,根本不該斷)。
# 「資料不足」與「結果是否定的」長得一模一樣,但含意相反。
short, long_ = 2.0, 120.0 # 星曆跨度(秒)
v.expect_true("星曆太短 -> 回 None(還算不出來,**不是**斷線)",
FL.decide_link(short, [], 10.0) is None,
"%s" % FL.decide_link(short, [], 10.0))
v.expect_true("星曆太短時,就算有區間也還是 None(不搶先下結論)",
FL.decide_link(short, [(0.0, 100.0)], 10.0) is None)
v.expect_true("跨度夠 + 沒有區間 -> False(真的不可見)",
FL.decide_link(long_, [], 10.0) is False,
"%s" % FL.decide_link(long_, [], 10.0))
v.expect_true("跨度夠 + 時刻落在區間內 -> True",
FL.decide_link(long_, [(0.0, 100.0)], 10.0) is True)
v.expect_true("跨度夠 + 時刻落在區間外 -> False",
FL.decide_link(long_, [(0.0, 5.0)], 10.0) is False)
v.expect_true("多段區間,落在第二段內也要 True",
FL.decide_link(long_, [(0.0, 5.0), (8.0, 20.0)], 10.0) is True)
v.expect_true("區間值壞掉 -> 跳過那一段,不當成可見",
FL.decide_link(long_, [("x", "y")], 10.0) is False)
# **守衛要設在「跨度」不是「點數」** —— 取樣率一變,點數的意義就變了
v.expect_true("門檻是星曆跨度(秒),不是點數",
FL.MIN_SPAN_S_FOR_ACCESS > 1.0
and not hasattr(FL, "MIN_POINTS_FOR_ACCESS"),
"門檻 %.0f s" % FL.MIN_SPAN_S_FOR_ACCESS)
v.expect_true("None 與 False 在布林上同為假(所以不能用 not ok 判斷)",
(not FL.decide_link(short, [], 10.0))
and (not FL.decide_link(long_, [], 10.0)),
"兩者都是假 —— 判斷必須寫成 `is False`")
# (e) **閉迴路不能攢批** —— chunk 必須是 1,而且這不是參數是定義
try:
fl2 = FL.FleetLoop(2, "local")
try:
v.expect("閉迴路的 Bridge chunk 固定為 1(不能攢批)",
fl2.fleet.bridges[0].chunk if fl2.fleet.bridges[0] else 1,
1, tol=0)
v.expect("FleetLoop 也是每架一條連線", len(fl2.fleet.conns), 2,
tol=0)
finally:
fl2.close()
except ImportError:
v.custom("閉迴路 chunk = 1", None, "pymavlink 沒裝,略過")
# ---------------- 13. 協同:分配、避撞、隊形 ----------------
import numpy as _np3
# (a) **Hungarian 的最佳性不靠相信,靠窮舉對照。**
# 小規模時所有排列都算得完,最佳解沒有爭議。
_rng = __import__("random").Random(7)
mismatch = 0
for _ in range(40):
n_v = _rng.randint(2, 4)
n_t = _rng.randint(n_v, n_v + 2)
c = _np3.array([[_rng.random() * 100 for _ in range(n_t)]
for _ in range(n_v)])
_p, opt = SW.assign_optimal(c)
_pb, bru = SW.assign_brute(c)
if abs(opt - bru) > 1e-9:
mismatch += 1
v.expect("Hungarian 與窮舉法的總成本一致(40 個隨機案例)",
mismatch, 0, tol=0)
# 貪婪**必定**在某些情形不是最優 —— 造一個經典的反例
trap = _np3.array([[1.0, 2.0], [1.1, 9.0]])
_pg, g = SW.assign_greedy(trap)
_po, o = SW.assign_optimal(trap)
v.expect_true("貪婪在構造的反例上比最佳解貴", g > o + 1e-9,
"貪婪 %.2f vs 最佳 %.2f" % (g, o))
v.expect("該反例的最佳總成本", o, 1.0 + 9.0 if False else 2.0 + 1.1,
tol=1e-9)
# (b) 避撞:偵測與誤報要分開驗
head_on = [SW.straight_track(24.99, 121.30, 90.0),
SW.straight_track(24.99, 121.30 + 0.012, 270.0)]
d_ho, _w = SW.min_separation(head_on)
v.expect_true("注入必定相撞的航線 -> 最小間距遠低於下限",
d_ho < SW.SEP_MIN_M, "%.1f m" % d_ho)
v.expect("必定相撞 -> 偵測到 1 組衝突",
len(SW.detect_conflicts(head_on)), 1, tol=0)
safe = [SW.straight_track(24.99, 121.30, 90.0),
SW.straight_track(24.99 + 600.0 / AP.M_PER_DEG_LAT, 121.30, 90.0)]
v.expect("必定安全 -> **不誤報**", len(SW.detect_conflicts(safe)), 0, tol=0)
fixed = SW.resolve_by_altitude(head_on, SW.detect_conflicts(head_on))
v.expect("高度分層後的垂直間距 = 層距",
SW.vertical_separation(fixed), SW.SEP_ALT_M, tol=1e-6, unit=" m")
v.expect_true("分層**不會**改變水平間距(只動高度)",
abs(SW.min_separation(fixed)[0] - d_ho) < 1e-9,
"%.3f vs %.3f m" % (SW.min_separation(fixed)[0], d_ho))
# (c) 隊形:v_i = omega * r_i 是閉合式,對就是對
om, radii = 0.018, [880.0, 1000.0, 1120.0]
sp = SW.formation_turn_speeds(radii, om)
for r_i, v_i in zip(radii, sp):
v.expect("隊形轉彎速度 v = omega * r(r=%.0f)" % r_i, v_i, om * r_i,
tol=1e-12, unit=" m/s")
# 速度包絡的最小轉彎半徑:用閉合式反推,再對函式
offs = SW.formation_offsets(3, spacing_m=120.0, shape="line")
r_min = SW.min_turn_radius_for_envelope(offs, 18.0, 12.0, 25.0)
want = max(18.0 * 120.0 / (25.0 - 18.0), 18.0 * 120.0 / (18.0 - 12.0))
v.expect("隊形的最小轉彎半徑與閉合式一致", r_min, want, tol=1e-9, unit=" m")
v.expect_true("隊形越寬,需要的最小轉彎半徑越大",
SW.min_turn_radius_for_envelope(
SW.formation_offsets(5, 120.0, "line"), 18.0, 12.0, 25.0)
> r_min)
# ---------------- 14. 六自由度:自洽、配平、模態 ----------------
# (a) 配平要解到**機器精度**,不是「夠小」
st_t, ct_t, info_t = SD.trim(18.0)
v.expect_true("配平收斂", info_t["converged"], info_t["msg"][:40])
v.expect_true("配平殘差在機器精度等級(< 1e-9)",
info_t["max_abs_residual"] < 1e-9,
"%.3e" % info_t["max_abs_residual"])
v.expect_between("配平攻角落在合理範圍", math.degrees(info_t["alpha"]),
0.0, 12.0, unit=" deg")
v.expect_between("配平油門落在 0-1", info_t["throttle"], 0.0, 1.0)
# (b) 四元數範數:**不做每步正規化**才驗得到積分本身
_tr, qerr = SD.integrate(st_t, ct_t, 0.01, 400, renorm=False)
v.expect_true("四元數範數在積分中守恆(不靠每步正規化)",
qerr < 1e-10, "%.3e" % qerr)
# (c) 縱向模態的**結構**要對(近似式很粗,所以不驗小數點)
a_lon, _b, _f = SD.linearise(st_t, ct_t)
md = SD.modes(a_lon)
osc = [(w, z) for w, z, lam in md if abs(lam.imag) > 1e-6]
v.expect_true("縱向出現兩組共軛複根(短週期 + 長週期)",
len(osc) >= 2, "%d 組" % len(osc))
if len(osc) >= 2:
(w_ph, z_ph), (w_sp, z_sp) = osc[0], osc[-1]
v.expect_true("短週期比長週期快一個數量級以上",
w_sp / w_ph > 5.0, "頻率比 %.1f" % (w_sp / w_ph))
v.expect_true("長週期的阻尼明顯小於短週期",
z_ph < 0.25 * z_sp, "%.4f vs %.4f" % (z_ph, z_sp))
# 對外部的解析近似:Lanchester 長週期 omega = g*sqrt(2)/V
v.expect_between("長週期頻率 vs Lanchester 近似(容許 2 倍)",
w_ph / (G0_LOCAL * math.sqrt(2.0) / 18.0), 0.5, 2.0)
# (d) **能量殘差要隨步長收斂** —— 這一項抓到過真錯
conv, ratios = SD.glide_energy_order(dts=(0.04, 0.02), span_s=4.0)
v.expect_true("能量殘差隨步長下降(不是模型不守恆)",
conv[-1][1] < conv[0][1] * 0.6,
"%.3e -> %.3e" % (conv[0][1], conv[-1][1]))
v.expect_true("收斂比接近二階(2-6 之間)",
ratios and 2.0 <= ratios[0] <= 6.0,
"%.2f" % (ratios[0] if ratios else -1))
v.expect_true("細步長下的能量殘差 < 1e-4", conv[-1][1] < 1e-4,
"%.3e" % conv[-1][1])
ok = v.report("STK gRPC 通聯測試 驗證段")
print()
print(" 這一段完全不碰 STK —— 協定常數、API 名稱、診斷邏輯、封包格式、")
print(" 攢批算術都不在 STK 裡,綁上授權只會讓最該常跑的檢查最少跑。")
print(" 第 3 與第 5 類是**失敗注入**:造出真的失敗,看診斷指得對不對。")
return 0 if ok else 1
if __name__ == "__main__":
_rc = main()
# 在 notebook 裡不要用 SystemExit 打斷 kernel;在終端機才回傳離開碼。
if "ipykernel" not in sys.modules:
raise SystemExit(_rc)
grpc_link/uav_de_deck.py
# -*- coding: utf-8 -*-
"""對外簡報:無人機研發摔機之外的選擇 —— STK 輔助軟體數位工程。
純資料,沒有副作用,不 import STK —— `tools/make_pptx.py` 會 import 它。
數字全部來自 grpc_link 各模組的實際輸出。
## 這一份與章節簡報的差別
`grpc_deck.py` 是給修課的人看的:它假設讀者會去跑那些程式。
這一份給不熟這套工具的人看,所以:
- 先講**為什麼會摔**,再講工具怎麼介入,最後才講數字。
- 每一個能力都配一個**可以證明的判準**,而不是一句宣稱。
- **明講做不到什麼。** 一份只講得到的東西的簡報,在評審席上撐不過三個問題。
## 刻意不放的東西
原始需求的標題是價值主張,而本教材會發佈,所以內文一律走技術與證據:
沒有「降低 X% 成本」「縮短 Y% 週期」這類無法從現況支持的數字。
效益頁換成「量得出來的能力與邊界」——
可量測的事實比價值主張耐得住檢驗,也才擋得住
「這個數字哪來的」這個問題。
"""
DECK = dict(
eyebrow="數位工程 · STK + MAVLink + Simulink + PySTK(gRPC)",
title="摔機之外的選擇",
subtitle="用可以證明的模擬,把失敗提前到地面上發生",
thesis="摔機貴,但真正貴的是**不知道為什麼摔**。"
"模擬要能取代一部分外場,靠的不是「跑起來很像」,"
"而是每一段都有**可以證明的判準** —— 這份簡報講的是那些判準長什麼樣。",
scenario=[
("要取代的", "低空飛行的鏈路中斷、GNSS 品質下降、地形遮蔽、多機衝突"),
("飛控與協定", "MAVLink(心跳、位置、姿態、電量、指令回應)"),
("動力學", "六自由度氣動模型(配平、模態、能量)"),
("環境與任務", "STK:地形、RF 鏈路預算、GPS 星座、可見度"),
("介接", "PySTK 走 gRPC;各段以**資料**為介面,不是函式呼叫"),
("閉迴路", "STK 算出來的判斷經 MAVLink 送回去,真的改變飛行"),
],
focus=[
"工具鏈本身就是一個失敗來源 —— 「連不上」有七個成因,處置完全不同。",
"環境模型放進迴路要付多少延遲?本地近似便宜多少、失真多少?",
"怎麼證明「STK 的判斷真的改變了飛行」,而不是只證明指令被收下?",
"多機的上限是什麼決定的?是實作差,還是結構性的?",
"六自由度模型怎麼驗?—— 配平、模態、能量,三層由硬到軟。",
],
figures=[
("grpc_link.png",
"四個量出來的結論:傳輸成本、即時預算、訊息上限、"
"以及「判準是例外型別不是耗時」"),
("sim_bidir_diagram.png",
"雙向閉迴路的 Simulink 模型(以腳本建出,可 diff、可審)"),
("sim_run_results.png",
"飛行軌跡對**外部閉合式**的偏差(±6.3 m),不是拿模型對自己"),
],
results_caption="全部為實測;單一機器、回文迴路。跨機器數字必然更差且尚未量過",
results=[
("量", "結果", "這代表什麼"),
("六層通聯診斷", "四種失敗成因各自可造出", "「連不上」變成可定位"),
("gRPC vs 行程內 COM", "讀屬性 1.70 vs 0.047 ms", "gRPC 換的是位置自由,不是速度"),
("閉迴路:STK 算 vs 本地近似", "13.2 vs 1.04 ms,判斷差 4.2%",
"忠實度與迴路頻率的取捨,可量化"),
("多機:開迴路 vs 閉迴路", "每架 12–13 次往返 vs 每幀 2.2/3.7/6.6 ms",
"閉迴路不能攢批,且要依序等"),
("斷鏈自動返航", "三架從 620–660 m 回到離家 31 m", "**行為證據**,不是旗標"),
("任務分配", "Hungarian 對窮舉 120/120 一致", "最佳性有證明"),
("貪婪的代價", "平均貴 4.8%,最差 52.3%", "40% 的案例不是最優"),
("隊形轉彎", "5 機橫隊需速差 48%", "速度包絡給出最小轉彎半徑 720 m"),
("6DOF 配平", "殘差 5.8e-13", "解到機器精度,不是「夠小」"),
("6DOF 模態", "短/長週期頻率比 11.4,阻尼 0.53 / 0.036", "結構對得上飛行力學"),
("RF 鏈路", "STK 損耗對閉合式差 0.0000 dB", "逐項對得上才敢信含地形的值"),
("地形基準面", "選錯偏移 10.82 m,內插雜訊 2.79 m", "差一個數量級,不是雜訊"),
],
findings=[
"A|**摔機的替代品不是「模擬看起來對」。** 這一章每一段都配一個"
"可以證明的判準:閉合式、數學恆等式、窮舉對照、失敗注入、步長收斂。"
"沒有判準的模擬只是動畫。",
"B|**工具鏈本身會失敗,而且不報錯。** 這一章踩到的坑裡,"
"沒有一個會丟出例外:約束掛在兩端只移除一端、把「算不出來」讀成"
"「斷了」、守衛設在錯的維度、門檻沒跟觀測時間對齊。"
"全部只表現成「行為看起來怪怪的」。",
"C|**證據要看行為,不看旗標。** 模擬器把 RTL 與 LOITER 送成同一個"
"custom_mode,「模式變了」在協定上看不出來;但三架都從 620–660 m"
"回到離家 31 m,那看得出來。旗標只證明指令被收下。",
"D|**多機的上限是結構性的。** 閉迴路不能攢批、每架要依序等,"
"所以每幀成本與架數線性。除非 STK 提供跨載具的批次入口,"
"否則再怎麼優化都繞不過每架一次往返。",
"E|**一個「很小」的殘差可能藏著真錯。** 6DOF 的能量殘差 3.5e-3"
"看起來很小,但**不隨步長收斂** —— 那不是離散誤差,是檢查本身漏了"
"俯仰率項。改用功-能定理後降到 3.9e-6,收斂比 3.25/3.64(二階)。",
],
verify=(136, 0, 0),
verify_note=[
"封包格式、DOP 恆等式、分配的最佳性、6DOF 的配平與模態 ——"
"這些都不在 STK 裡。所以最該常跑的檢查不必等授權。",
"其中多項是**失敗注入**:造出真的失敗(拒絕/逾時/DNS/非 gRPC 伺服器、"
"對撞航線、退化幾何、漏包),看診斷指不指得對。",
"另有 27 項**需要 STK**,驗的是 STK 自己:GPS almanac 給的星座對不對"
"(對公開標稱值,不對 STK 自己)、Realtime 注入的往返完整性、"
"存取約束有沒有生效,以及**閉迴路有沒有真的改變飛行**。",
],
run=[
"python grpc_link_probe.py 六層通聯診斷(不需 STK)",
"python grpc_link_fleetloop.py 多機閉迴路 + 返航行為證據",
"python grpc_link_swarm.py 分配(對窮舉)/ 避撞 / 隊形",
"python grpc_link_sixdof.py 配平、模態、能量收斂",
"python grpc_link_terrain.py --dem <路徑> 地形與基準面對帳",
"python grpc_link_verify.py 136 項(不需 STK)",
"python grpc_link_stk_verify.py 27 項(需 STK)",
],
files=[
("這套東西**做得到**", "把鏈路、地形、GPS、多機衝突的失效提前到地面上重現"),
("**做不到**(一)", "真實自駕韌體行為:EKF、失效保護、任務狀態機"),
("**做不到**(二)", "真機的操穩品質 —— 6DOF 用的是代表性係數,非風洞值"),
("**做不到**(三)", "硬體在環:目前全是軟體在環,沒有時序保證"),
("**做不到**(四)", "多路徑、蓄意干擾、GNSS 拒止的電磁層建模"),
("需要自備", "STK 授權;地形 DEM;MAVLink 模擬器(皆不隨教材附上)"),
("下一步", "接真 SITL、真風洞係數、HIL 時序、協同的任務級驗證"),
],
)