From 3e6490d07f23c48f684f7f0f380954b303d08954 Mon Sep 17 00:00:00 2001 From: lenting89 Date: Mon, 8 Jun 2026 18:37:20 +0800 Subject: [PATCH] =?UTF-8?q?=E8=99=95=E7=90=86=E4=B8=80=E9=96=8B=E5=A7=8Bdi?= =?UTF-8?q?sc=E6=89=BE=E4=B8=8D=E5=88=B0=E9=A3=9B=E6=A9=9F=E7=9A=84?= =?UTF-8?q?=E5=95=8F=E9=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/esp32.py | 219 ++++++++++---------------------------- src/unitdev04/udptest8.py | 196 ++++++++++------------------------ 2 files changed, 111 insertions(+), 304 deletions(-) diff --git a/src/unitdev04/esp32.py b/src/unitdev04/esp32.py index 3adb776..719b5c9 100644 --- a/src/unitdev04/esp32.py +++ b/src/unitdev04/esp32.py @@ -7,25 +7,27 @@ import gc # ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge # Packet-size TDMA + DONE + SYSID / GCS address 自動學習版 # -# 對應新版 GCS discovery 設計： -# 1. GCS 開機前 30 秒自動送 DISC。 -# 2. GCS 之後停止自動 DISC。 -# 3. 若後續新增 UAV，可在 GCS GUI 按 Scan UAV，送 DSCF 強制掃描。 +# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。 +# 現在只保留一種 discovery 封包，名稱統一叫 DISC。 # -# ESP32 端核心行為： -# 1. FC -> ESP32：持續讀 MAVLink bytes，存進 tx_buf。 -# 2. ESP32 從 FC MAVLink frame header 自動學自己的 MY_SYSID。 -# 3. ESP32 收到 GCS 的 DISC / DSCF / POLL 時，從 XBee 0x90 src64 自動學 DEST_64。 -# 4. 收到 DISC： -# 同一個 GCS 只回一次 HELO，避免 HELO 一直洗版。 -# 5. 收到 DSCF： -# 強制回 HELO，給 GCS 手動重新掃描用。 -# 6. 收到 POLL + SYSID + quota_bytes： -# 若 target_sysid == MY_SYSID，才根據 quota_bytes 傳完整 MAVLink frames。 -# 7. 傳完本輪資料後，獨立送 DONE report。 +# 重要： +# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。 +# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。 +# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING； +# 之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。 +# +# 封包格式： +# GCS -> UAV: +# DISC +# POLL + target_sysid(1) + grant_bytes(2) +# +# UAV -> GCS: +# HELO + sysid(1) +# DONE + sysid(1) + sent_len(2) + remain_len(2) # # 注意： -# - DONE / HELO 都是獨立 RF payload，不會接在 MAVLink stream 後面。 +# - HELO 不帶 remain。 +# - DONE 保留 remain。 # - MY_SYSID 不寫死，由飛控 MAVLink 自動學。 # - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。 # ========================================================= @@ -35,58 +37,40 @@ import gc FC_BAUDRATE = 115200 XB_BAUDRATE = 115200 -# 不寫死地面站 XBee address。 -# 收到 GCS 的 DISC / DSCF / POLL / 下行 MAVLink 時， -# 從 XBee 0x90 frame 的 src64 自動學到。 DEST_64 = None - -# 不寫死每台 UAV 的 SYSID。 -# 會從 FC -> ESP32 的 MAVLink frame header 自動學到。 MY_SYSID = None -# 為了避免雜訊誤判 SYSID，要求連續確認幾次相同 SYSID 才正式採用。 SYSID_LEARN_CONFIRM_COUNT = 3 _sysid_candidate = None _sysid_candidate_count = 0 -# XBee API Transmit Request 內每次 RF payload 最大切片。 -# 900HP / DigiMesh 實測常用 80~100 bytes 較穩。 +# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。 +# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。 +DISC_PENDING = False + XBEE_MAX_PAYLOAD = 100 -# GCS -> UAV discovery： -# DISC：一般 discovery，同一個 GCS 只回一次 HELO。 -# DSCF：force discovery，收到後強制回 HELO，給 GUI Scan UAV 按鍵用。 +# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。 DISC_MAGIC = b'DISC' -DISC_FORCE_MAGIC = b'DSCF' # UAV -> GCS hello： # HELO + sysid(1) HELLO_MAGIC = b'HELO' -# 一般 HELO 最小間隔，避免短時間內重複回太多。 HELLO_MIN_INTERVAL_MS = 1000 _last_hello_ms = 0 -# 對同一個 GCS 是否已經送過 HELO。 -# 收到 DISC 時，如果 HELLO_SENT=True，就不再回 HELO。 -# 收到 DSCF 時，不管 HELLO_SENT，都強制回 HELO。 +# 保留此變數只作狀態記錄；本版 DISC 會 force=True，所以不會被 HELLO_SENT 擋掉。 HELLO_SENT = False -# GCS -> UAV poll 格式： -# 舊版：b'POLL' + sysid(1) -# 新版：b'POLL' + sysid(1) + quota_bytes(2, big-endian) POLL_MAGIC = b'POLL' DEFAULT_GRANT_BYTES = 600 -# UAV -> GCS done 格式： -# b'DONE' + sysid(1) + sent_len(2) + remain_len(2) DONE_MAGIC = b'DONE' -# ESP32 RAM 有限，不要讓 FC 資料無限累積 MAX_BUF_SIZE = 6144 READ_FC_BYTES = 250 -# UART 腳位。若你的接線不同，只要改這裡。 FC_UART_ID = 1 FC_TX_PIN = 32 FC_RX_PIN = 33 @@ -126,16 +110,10 @@ def learn_dest64_from_src64(src64): """ UAV 收到 GCS 的 XBee 0x90 frame 時， 0x90 裡面的 src64 就是 GCS / coordinator XBee 的 64-bit address。 - - 若第一次學到 GCS address： - DEST_64 = src64 - HELLO_SENT = False - - 若後來收到不同 src64： - 視為地面站 XBee 改變，更新 DEST_64，並允許重新回 HELO。 """ global DEST_64 global HELLO_SENT + global DISC_PENDING if not is_valid_addr64(src64): return False @@ -150,6 +128,7 @@ def learn_dest64_from_src64(src64): if DEST_64 != src64: DEST_64 = src64 HELLO_SENT = False + DISC_PENDING = False return True return False @@ -160,18 +139,16 @@ def build_api_tx_frame(payload): 建立 XBee API frame type 0x10，把 payload 送到 DEST_64。 若 DEST_64 尚未學到，回傳 None。 """ - global DEST_64 - if DEST_64 is None: return None frame_content = bytearray() - frame_content.append(0x10) # Transmit Request - frame_content.append(0x00) # Frame ID = 0，不要求 XBee ACK，降低延遲 + frame_content.append(0x10) + frame_content.append(0x00) frame_content.extend(DEST_64) frame_content.extend(b'\xFF\xFE') - frame_content.append(0x00) # broadcast radius - frame_content.append(0x00) # options + frame_content.append(0x00) + frame_content.append(0x00) frame_content.extend(payload) length = len(frame_content) @@ -189,7 +166,6 @@ def build_api_tx_frame(payload): def send_to_xbee_chunked(payload): """ 把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。 - 若 DEST_64 尚未學到，直接不送，回傳 False。 """ if DEST_64 is None: return False @@ -207,8 +183,6 @@ def send_to_xbee_chunked(payload): return False uart_xb.write(pkt) - - # 很短的讓步，避免 ESP32 UART/XBee 本地 buffer 瞬間塞爆 time.sleep_ms(1) return True @@ -223,12 +197,7 @@ def send_hello_report(force=False): UAV 回報自己存在： HELO + MY_SYSID - force=False： - 若 HELLO_SENT=True，代表同一個 GCS 已經註冊過，不再回 HELO。 - - force=True： - 不管 HELLO_SENT，都重新回 HELO。 - 用於 GCS GUI 按 Scan UAV 送 DSCF 時。 + 本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。 """ global _last_hello_ms global HELLO_SENT @@ -259,11 +228,22 @@ def send_hello_report(force=False): return True -def send_done_report(sent_len, remain_len): +def try_send_pending_hello(): + """ + 若先收到 DISC、後學到 MY_SYSID，這裡會自動補送 HELO。 """ - 送出 TDMA DONE 標籤。 - 此封包不會轉給飛控，只給 GCS scheduler 使用。 + global DISC_PENDING + if DISC_PENDING and MY_SYSID is not None and DEST_64 is not None: + delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180) + time.sleep_ms(delay_ms) + + if send_hello_report(force=True): + DISC_PENDING = False + + +def send_done_report(sent_len, remain_len): + """ DONE payload: b'DONE' + sysid(1) + sent_len(2) + remain_len(2) """ @@ -292,7 +272,6 @@ def send_done_report(sent_len, remain_len): # ========================================================= def find_first_mavlink_magic(buf): - """找 MAVLink v1/v2 magic：0xFE 或 0xFD。""" pos_fe = buf.find(b'\xFE') pos_fd = buf.find(b'\xFD') @@ -304,17 +283,6 @@ def find_first_mavlink_magic(buf): def mavlink_frame_length(buf, start_idx): - """ - 回傳從 start_idx 開始的一包 MAVLink frame 長度。 - 若 header 還不完整或整包還沒收完，回傳 None。 - - MAVLink v1: - magic 0xFE, payload_len at byte 1, total = payload_len + 8 - - MAVLink v2: - magic 0xFD, payload_len at byte 1, incompat_flags at byte 2, - total = payload_len + 12，若 signed 則 +13 - """ if start_idx >= len(buf): return None @@ -344,23 +312,15 @@ def mavlink_frame_length(buf, start_idx): def get_mavlink_sysid(buf, start_idx): - """ - 從 MAVLink frame header 讀出 SYSID。 - 不做 CRC 驗證，但會搭配多次確認降低誤判。 - """ if start_idx >= len(buf): return None magic = buf[start_idx] - # MAVLink v1: - # 0xFE | len | seq | sysid | compid | msgid | payload | checksum if magic == 0xFE: if len(buf) - start_idx >= 6: return buf[start_idx + 3] - # MAVLink v2: - # 0xFD | len | incompat | compat | seq | sysid | compid | msgid(3) | payload | checksum if magic == 0xFD: if len(buf) - start_idx >= 10: return buf[start_idx + 5] @@ -373,7 +333,7 @@ def learn_my_sysid_from_tx_buf(): 從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame， 並自動學習飛控的 MAVLink SYSID。 - 為避免雜訊造成誤判，要求連續多次看到相同 SYSID 才正式設定 MY_SYSID。 + 若更改飛控 SYSID，建議重新上電 ESP32，讓 MY_SYSID 重新學習。 """ global MY_SYSID global _sysid_candidate @@ -402,8 +362,6 @@ def learn_my_sysid_from_tx_buf(): if sysid is not None and sysid > 0: if MY_SYSID is not None: - # 正常情況不任意改變已學到的 SYSID。 - # 如果你真的會在飛行中改 FC SYSID，再另行設計重學條件。 return if _sysid_candidate == sysid: @@ -414,8 +372,6 @@ def learn_my_sysid_from_tx_buf(): if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT: MY_SYSID = _sysid_candidate - - # 新學到 SYSID 後，允許對已知 GCS 回一次 HELO HELLO_SENT = False return @@ -428,18 +384,11 @@ def learn_my_sysid_from_tx_buf(): # ========================================================= def pop_mavlink_frames_by_quota(quota_bytes): - """ - 從 tx_buf 取出不超過 quota_bytes 的完整 MAVLink frames。 - 不會使用 rfind(0xFD) 亂切，避免 payload 中剛好有 0xFD 造成誤判。 - - 若第一個完整 MAVLink frame 比 quota 還大，仍會送出第一包，避免永遠卡住。 - """ global tx_buf if quota_bytes <= 0 or len(tx_buf) == 0: return b'' - # 丟掉 MAVLink magic 前面的雜訊 start = find_first_mavlink_magic(tx_buf) if start == -1: tx_buf = bytearray() @@ -451,7 +400,6 @@ def pop_mavlink_frames_by_quota(quota_bytes): out = bytearray() while len(tx_buf) > 0: - # 再次對齊 magic，避免中間出現雜訊 if tx_buf[0] not in (0xFE, 0xFD): start = find_first_mavlink_magic(tx_buf) if start == -1: @@ -461,14 +409,11 @@ def pop_mavlink_frames_by_quota(quota_bytes): frame_len = mavlink_frame_length(tx_buf, 0) if frame_len is None: - # 第一包尚未完整，留在 buffer 等下一輪 FC bytes break - # 正常情況：加了這包會超過 quota，就先停止 if len(out) > 0 and (len(out) + frame_len) > quota_bytes: break - # 第一包就比 quota 大：仍送出，避免 quota 設太小導致永遠送不出去 if len(out) == 0 and frame_len > quota_bytes: out.extend(tx_buf[:frame_len]) tx_buf = tx_buf[frame_len:] @@ -484,9 +429,6 @@ def pop_mavlink_frames_by_quota(quota_bytes): def trim_tx_buffer_if_needed(): - """ - tx_buf 過大時，丟掉較舊的完整 MAVLink frames，保留較新的資料。 - """ global tx_buf if len(tx_buf) <= MAX_BUF_SIZE: @@ -506,25 +448,16 @@ def trim_tx_buffer_if_needed(): frame_len = mavlink_frame_length(tx_buf, 0) if frame_len is None: - # 若剩下的是超大半包資料，直接保留最後 target_size bytes if len(tx_buf) > target_size: tx_buf = tx_buf[-target_size:] return - # 丟掉最舊的一包完整 MAVLink frame tx_buf = tx_buf[frame_len:] def flush_tx_buffer(grant_bytes): - """ - 收到自己 SYSID 的 POLL 後執行： - 1. 根據 grant_bytes 取出完整 MAVLink frames。 - 2. 用 XBee API frame 送出去。 - 3. 獨立送 DONE report，讓 GCS 立即進下一台。 - """ global tx_buf - # 尚未學到 SYSID 或地面站地址，不進行上行傳送。 if MY_SYSID is None: return @@ -549,13 +482,6 @@ def flush_tx_buffer(grant_bytes): # ========================================================= def parse_poll_payload(real_data): - """ - 支援兩種 poll： - 舊版：POLL + sysid，共 5 bytes - 新版：POLL + sysid + quota，共 7 bytes - - 回傳 (target_sysid, grant_bytes)，不是 poll 則回傳 (None, None)。 - """ if not real_data.startswith(POLL_MAGIC): return None, None @@ -571,28 +497,16 @@ def parse_poll_payload(real_data): def is_discovery_payload(real_data): - """ - 一般 discovery： - b'DISC' - """ return real_data.startswith(DISC_MAGIC) -def is_force_discovery_payload(real_data): - """ - 強制 discovery： - b'DSCF' - 用於 GCS GUI Scan UAV 按鍵。 - """ - return real_data.startswith(DISC_FORCE_MAGIC) - - # ========================================================= # XBee API RX parser # ========================================================= def process_xbee_buffer(): global rx_buf + global DISC_PENDING while True: start_pos = rx_buf.find(b'\x7E') @@ -609,7 +523,6 @@ def process_xbee_buffer(): pkt_len = (rx_buf[1] << 8) | rx_buf[2] total_len = pkt_len + 4 - # 避免亂資料讓 buffer 爆掉；XBee RX frame 通常不會太大 if pkt_len > 300: rx_buf = rx_buf[1:] continue @@ -629,33 +542,20 @@ def process_xbee_buffer(): real_data = packet[12:] # ------------------------------------------------- - # GCS 一般 DISC： - # 同一個 GCS 只回一次 HELO，避免一直 HELO。 + # DISC：本版唯一 discovery 封包。 + # 功能等同原本 DSCF：收到後強制回 HELO。 # ------------------------------------------------- if is_discovery_payload(real_data): learn_dest64_from_src64(src64) learn_my_sysid_from_tx_buf() - if MY_SYSID is not None and DEST_64 is not None: - # 加一點由 SYSID / 時間產生的延遲，降低多機同時 HELO 撞包機率。 - # 若 HELLO_SENT=True，send_hello_report() 會自己擋掉。 - delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180) - time.sleep_ms(delay_ms) - send_hello_report(force=False) - - # ------------------------------------------------- - # GCS 強制 DSCF： - # 不管 HELLO_SENT，重新回 HELO。 - # 用於 GCS GUI Scan UAV 按鍵。 - # ------------------------------------------------- - elif is_force_discovery_payload(real_data): - learn_dest64_from_src64(src64) - learn_my_sysid_from_tx_buf() - if MY_SYSID is not None and DEST_64 is not None: delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180) time.sleep_ms(delay_ms) send_hello_report(force=True) + DISC_PENDING = False + else: + DISC_PENDING = True else: # ------------------------------------------------- @@ -664,21 +564,15 @@ def process_xbee_buffer(): target_sysid, grant_bytes = parse_poll_payload(real_data) if target_sysid is not None: - # 收到 GCS 的 POLL，也可以從 src64 學地面站地址。 learn_dest64_from_src64(src64) - - # 嘗試從 FC buffer 學 MY_SYSID。 learn_my_sysid_from_tx_buf() + try_send_pending_hello() - # 只有 target_sysid 等於自己飛控的 SYSID，才回傳。 if MY_SYSID is not None and target_sysid == MY_SYSID: flush_tx_buffer(grant_bytes) else: - # ------------------------------------------------- # 一般 GCS -> FC MAVLink 下行資料 - # 只接受來自已知 GCS address 的資料，避免其他節點污染飛控。 - # ------------------------------------------------- if DEST_64 is None: learn_dest64_from_src64(src64) @@ -688,7 +582,6 @@ def process_xbee_buffer(): rx_buf = rx_buf[total_len:] else: - # checksum 錯誤，丟掉第一個 byte，重新對齊 rx_buf = rx_buf[1:] @@ -716,10 +609,8 @@ while True: if data: tx_buf.extend(data) - # 從飛控 MAVLink stream 自動學 MY_SYSID learn_my_sysid_from_tx_buf() - - # 避免 tx_buf 無限長大 + try_send_pending_hello() trim_tx_buffer_if_needed() # ------------------------------------------------- @@ -735,7 +626,6 @@ while True: time.sleep_ms(1) except MemoryError: - # OOM 時丟掉暫存資料，避免整顆 ESP32 死掉 tx_buf = bytearray() rx_buf = bytearray() @@ -747,5 +637,4 @@ while True: time.sleep_ms(10) except Exception: - # 現場飛行時避免因單次解析錯誤中止主迴圈 time.sleep_ms(2) diff --git a/src/unitdev04/udptest8.py b/src/unitdev04/udptest8.py index c23ba1f..db9011e 100644 --- a/src/unitdev04/udptest8.py +++ b/src/unitdev04/udptest8.py @@ -14,28 +14,28 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg # ========================================================= # GCS 端：XBee Serial <-> UDP Bridge -# Packet-size TDMA + DONE + RSSI + Auto Discovery + Force Scan +# Packet-size TDMA + DONE + RSSI + Auto Discovery # -# 對應 ESP32 端： -# - DISC：一般 discovery。UAV 對同一個 GCS 只回一次 HELO。 -# - DSCF：force discovery。GUI 按 Scan UAV 時送出，UAV 強制回 HELO。 -# - HELO：UAV -> GCS，回報 SYSID。 -# - POLL：GCS -> UAV，授權某台 SYSID 傳送 quota_bytes。 -# - DONE：UAV -> GCS，表示本輪資料已送完，GCS 可立即換下一台。 +# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。 +# 現在只保留一種 discovery 封包，名稱統一叫 DISC。 # -# 核心行為： -# 1. 程式啟動前 AUTO_DISCOVERY_DURATION_SEC 秒自動 broadcast DISC。 -# 2. 收到 HELO 後，GCS 從 XBee 0x90 src64 學 UAV XBee address， -# 從 HELO payload 學 MAVLink SYSID，建立 SYSID <-> src64 對應。 -# 3. TDMA scheduler 依 ACTIVE_SYSIDS 輪詢。 -# 4. send_poll() 若已知 SYSID 對應 src64，則 unicast；未知則 fallback broadcast。 -# 5. 收到 DONE 後立即查 ATDB，將 RSSI 歸屬到該 SYSID。 -# 6. GUI 的 Scan UAV 按鈕會送 DSCF burst，讓新加入或已存在的 UAV 強制回 HELO。 +# 重要： +# - DISC 的功能等同於原本的 DSCF，也就是強制 discovery。 +# - GCS 開機自動 discovery 與 GUI Scan UAV 都送 DISC。 +# - UAV 收到 DISC 後會重新回 HELO。 +# +# 封包格式： +# GCS -> UAV: +# DISC +# POLL + target_sysid(1) + grant_bytes(2) +# +# UAV -> GCS: +# HELO + sysid(1) +# DONE + sysid(1) + sent_len(2) + remain_len(2) # ========================================================= # ================= 多組設備設定 ================= -# Windows 測試時建議先只留你的實際 COM port，例如 COM15。 CONFIGS = [ {"serial_port": "/dev/ttyUSB0", "udp_port": 14551}, {"serial_port": "COM15", "udp_port": 14590}, @@ -46,52 +46,40 @@ CONFIGS = [ SERIAL_BAUDRATE = 115200 UDP_REMOTE_IP = '127.0.0.1' -# 若你要讓外部程式固定送 UDP 到本 bridge，可改成 True。 -# False：本程式只負責 sendto 到 udp_port；本地接收 port 由系統隨機配置。 -# True ：本程式綁定 local UDP port = config['udp_port']。 UDP_LISTEN_FIXED_PORT = False -# XBee broadcast address TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' -# 起始已知 SYSID。完全自動 discovery 可設空 list。 -# 若你想保留舊版行為，可改成 [3, 10, 15]。 INITIAL_ACTIVE_SYSIDS = [] ACTIVE_SYSIDS = list(INITIAL_ACTIVE_SYSIDS) -# 若你知道每台 UAV XBee 的 64-bit address，可以預先填入。 -# 即使不填，本程式也會從 HELO / DONE / MAVLink data 自動學習。 XBEE_ADDR64_TO_SYSID = {} AUTO_LEARN_XBEE_ADDR = True -# 控制封包 magic -DISC_MAGIC = b'DISC' # GCS -> UAV 一般 discovery -DISC_FORCE_MAGIC = b'DSCF' # GCS -> UAV 強制 discovery，GUI Scan UAV 用 -HELLO_MAGIC = b'HELO' # UAV -> GCS: HELO + sysid(1) -POLL_MAGIC = b'POLL' # GCS -> UAV: POLL + sysid(1) + quota_bytes(2) -DONE_MAGIC = b'DONE' # UAV -> GCS: DONE + sysid(1) + sent_len(2) + remain_len(2) +# 只保留 DISC；功能等同原本 DSCF。 +DISC_MAGIC = b'DISC' +HELLO_MAGIC = b'HELO' +POLL_MAGIC = b'POLL' +DONE_MAGIC = b'DONE' # ================= Discovery 參數 ================= AUTO_DISCOVERY_ENABLED = True AUTO_DISCOVERY_DURATION_SEC = 30.0 DISCOVERY_INTERVAL_SEC = 1.5 -# GUI 按 Scan UAV 時送出 DSCF burst -FORCE_DISCOVERY_BURST_COUNT = 5 -FORCE_DISCOVERY_BURST_GAP_SEC = 0.20 +# GUI 按 Scan UAV 時送出 DISC burst +DISCOVERY_BURST_COUNT = 5 +DISCOVERY_BURST_GAP_SEC = 0.20 # ================= TDMA 參數 ================= current_grant_bytes = 600 current_guard_ms = 20 INIT_GRANT_BYTES = 1200 -# 丟包率計算時間視窗 LOSS_TIME_WINDOW_SEC = 5.0 -# RSSI 最近 N 次平均 RSSI_AVG_WINDOW = 5 -# 是否也在資料封包時查 DB。預設 False：只在 DONE / HELO 後查 DB，較不干擾資料流。 RSSI_QUERY_ON_DATA_FRAME = False RSSI_DATA_QUERY_MIN_INTERVAL_SEC = 0.5 @@ -101,7 +89,7 @@ uav_states = {} for sid in ACTIVE_SYSIDS: uav_states[sid] = {"mode": "NORMAL"} -rssi_history = defaultdict(lambda: deque(maxlen=5000)) # 畫圖用：RSSI avgN +rssi_history = defaultdict(lambda: deque(maxlen=5000)) rssi_raw_windows = defaultdict(lambda: deque(maxlen=RSSI_AVG_WINDOW)) rssi_time_history = defaultdict(lambda: deque(maxlen=5000)) rssi_latest_stats = defaultdict(lambda: { @@ -117,18 +105,14 @@ packet_loss_time_history = defaultdict(lambda: deque(maxlen=1000)) mavlink_sequence_tracker = defaultdict(dict) packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0}) -# 自動學習到的 XBee address 對應 learned_addr64_to_sysid = {} learned_sysid_to_addr64 = {} -# 每個 SYSID 一個 asyncio.Event；收到 DONE 後 set，scheduler 立即換下一台。 tdma_done_events = {} tdma_last_reports = defaultdict(lambda: {'time': 0.0, 'sent_len': 0, 'remain_len': 0}) -# HELO 回報紀錄 hello_last_reports = defaultdict(lambda: {'time': 0.0, 'src64': None}) -# asyncio loop / serial protocols 給 GUI button 使用 ASYNC_LOOP = None SERIAL_PROTOCOLS = [] @@ -144,7 +128,6 @@ def format_addr64(addr): def ensure_active_sysid(sysid): - """若發現新的 SYSID，加入 ACTIVE_SYSIDS 並建立狀態。""" if sysid is None: return if sysid <= 0: @@ -162,7 +145,6 @@ def ensure_active_sysid(sysid): try: tdma_done_events[sysid] = asyncio.Event() except RuntimeError: - # 若從 GUI thread 呼叫時沒有 running loop，scheduler 之後會補建。 pass @@ -177,10 +159,6 @@ def infer_sysid_from_addr64(src64): def learn_xbee_source(sysid, src64): - """ - 建立 UAV XBee src64 <-> MAVLink SYSID 對應。 - GCS 端收到 UAV 的 0x90 frame 時，src64 是 UAV 的 XBee address。 - """ if not AUTO_LEARN_XBEE_ADDR: return if sysid is None or src64 is None: @@ -189,20 +167,29 @@ def learn_xbee_source(sysid, src64): return src64 = bytes(src64) + + old_sysid = learned_addr64_to_sysid.get(src64) + if old_sysid is not None and old_sysid != sysid: + print(f"[DISCOVERY] src64 {format_addr64(src64)} changed SYSID {old_sysid} -> {sysid}") + learned_sysid_to_addr64.pop(old_sysid, None) + if old_sysid in ACTIVE_SYSIDS: + ACTIVE_SYSIDS.remove(old_sysid) + uav_states.pop(old_sysid, None) + tdma_done_events.pop(old_sysid, None) + + old_src64 = learned_sysid_to_addr64.get(sysid) + if old_src64 is not None and old_src64 != src64: + learned_addr64_to_sysid.pop(old_src64, None) + learned_addr64_to_sysid[src64] = sysid learned_sysid_to_addr64[sysid] = src64 ensure_active_sysid(sysid) def get_poll_dest_addr64(sysid): - """ - 已知 SYSID -> src64 時，優先 unicast。 - 未知時 fallback broadcast。 - """ if sysid in learned_sysid_to_addr64: return learned_sysid_to_addr64[sysid] - # 使用者預填的 XBEE_ADDR64_TO_SYSID 反查 for addr, sid in XBEE_ADDR64_TO_SYSID.items(): if sid == sysid: return addr @@ -211,10 +198,6 @@ def get_poll_dest_addr64(sysid): def record_rssi(sysid, rssi_positive_db, src64=None): - """ - XBee ATDB 回傳通常是正值，例如 55 表示 -55 dBm。 - 這裡統一轉成負 dBm，並做最近 RSSI_AVG_WINDOW 次平均。 - """ if sysid is None: return @@ -238,8 +221,6 @@ def record_rssi(sysid, rssi_positive_db, src64=None): def calculate_packet_loss(sysid, compid, current_seq): - global mavlink_sequence_tracker, packet_loss_stats - ensure_active_sysid(sysid) tracker = mavlink_sequence_tracker[sysid] @@ -292,18 +273,11 @@ def calculate_packet_loss(sysid, compid, current_seq): def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x00) -> bytes: - """ - GCS XBee API TX frame type 0x10。 - """ frame = b'\x10' + struct.pack('>B', frame_id) + dest_addr64 + b'\xFF\xFE\x00\x00' + data return b'\x7E' + struct.pack('>H', len(frame)) + frame + struct.pack('B', 0xFF - (sum(frame) & 0xFF)) def estimate_tdma_timeout(grant_bytes): - """ - DONE 正常收到時不會等到 timeout。 - timeout 只在 DONE 掉包或該台沒回應時保護 scheduler。 - """ grant_bytes = max(0, int(grant_bytes)) max_payload = 100 chunks = max(1, (grant_bytes + max_payload - 1) // max_payload) @@ -314,7 +288,6 @@ def estimate_tdma_timeout(grant_bytes): async def grant_one_uav(serial_protocols, sysid, grant_bytes): - """授權一台 UAV；收到 DONE 立即結束，否則 timeout 後換下一台。""" ensure_active_sysid(sysid) ev = tdma_done_events.get(sysid) @@ -351,7 +324,7 @@ class SerialToUDP(asyncio.Protocol): self.current_poll_time = 0.0 self.at_frame_id = 0x20 - self.pending_db = {} # frame_id -> {'sysid', 'src64', 'time', 'reason'} + self.pending_db = {} self.last_data_db_query_time = 0.0 def connection_made(self, transport): @@ -360,16 +333,12 @@ class SerialToUDP(asyncio.Protocol): self.udp_protocol.set_serial_transport(self) print(f"[{self.serial_port}] Serial connection established.") - # -------------------- GCS -> UAV control -------------------- - - def send_discovery(self, force=False): + def send_discovery(self): """ - 發 discovery： - - force=False：DISC，一般 discovery；UAV 對同一 GCS 只回一次 HELO。 - - force=True ：DSCF，強制 discovery；UAV 會重新回 HELO。 + 發 DISC。 + 本版 DISC 功能等同原本 DSCF，UAV 收到後會強制回 HELO。 """ - payload = DISC_FORCE_MAGIC if force else DISC_MAGIC - api_frame = build_api_tx_frame(payload, TARGET_ADDR64, 0x00) + api_frame = build_api_tx_frame(DISC_MAGIC, TARGET_ADDR64, 0x00) self.transport.write(api_frame) def send_poll(self, target_sysid, grant_bytes=None): @@ -387,8 +356,6 @@ class SerialToUDP(asyncio.Protocol): self.transport.write(api_frame) - # -------------------- Serial RX parser -------------------- - def data_received(self, data): self.buffer.extend(data) @@ -407,7 +374,6 @@ class SerialToUDP(asyncio.Protocol): length = (self.buffer[1] << 8) | self.buffer[2] full_length = 3 + length + 1 - # 本程式使用 XBee RF payload 約 80~100 bytes，正常 length 不會很大。 if length > 700: self.buffer.pop(0) continue @@ -429,8 +395,6 @@ class SerialToUDP(asyncio.Protocol): frame_type = frame[3] if frame_type == 0x90: - # XBee Receive Packet: - # 7E | len(2) | 90 | src64(8) | src16(2) | options(1) | RF data | checksum src64 = frame[4:12] rf_data = frame[15:-1] self.handle_rx_packet(src64, rf_data) @@ -440,13 +404,9 @@ class SerialToUDP(asyncio.Protocol): self.handle_at_response(frame) return - # 其他如 0x8B Transmit Status 可視需求處理；目前忽略。 - def handle_rx_packet(self, src64, rf_data): - # 先用 src64 查。若尚未學習，HELO / DONE payload 會帶 sysid，可建立對應。 sysid_hint = infer_sysid_from_addr64(src64) - # 若在最近 poll 時槽內收到資料，也可作為備援歸屬。 if sysid_hint is None and self.current_poll_sysid is not None: if time.time() - self.current_poll_time <= 2.0: sysid_hint = self.current_poll_sysid @@ -454,7 +414,6 @@ class SerialToUDP(asyncio.Protocol): result = self.udp_protocol.process_rf_data(rf_data, src64=src64, sysid_hint=sysid_hint) if result is not None: - # result: ('DONE' or 'HELO', sysid) reason, sysid = result learn_xbee_source(sysid, src64) self.send_at_command_db(sysid, src64=src64, reason=reason) @@ -466,8 +425,6 @@ class SerialToUDP(asyncio.Protocol): self.send_at_command_db(sysid_hint, src64=src64, reason='DATA') self.last_data_db_query_time = now - # -------------------- ATDB RSSI -------------------- - def next_at_frame_id(self): self.at_frame_id += 1 if self.at_frame_id > 0xFE: @@ -475,10 +432,6 @@ class SerialToUDP(asyncio.Protocol): return self.at_frame_id def send_at_command_db(self, sysid, src64=None, reason=''): - """ - 查本地 XBee 的 DB 值。DB 代表本地 XBee 最近收到 RF packet 的 RSSI。 - 這裡在 DONE / HELO 後立即查，所以 RSSI 會歸屬到剛剛回報的 sysid。 - """ try: frame_id = self.next_at_frame_id() frame_type = 0x08 @@ -495,7 +448,6 @@ class SerialToUDP(asyncio.Protocol): 'reason': reason, } - # 清掉太舊 pending，避免長時間累積 now = time.time() stale = [fid for fid, info in self.pending_db.items() if now - info['time'] > 2.0] for fid in stale: @@ -506,7 +458,6 @@ class SerialToUDP(asyncio.Protocol): print(f"[{self.serial_port}] send ATDB failed: {e}") def handle_at_response(self, frame): - # AT Command Response: 0x88 | frame_id | AT(2) | status | value... if len(frame) < 9: return @@ -523,7 +474,6 @@ class SerialToUDP(asyncio.Protocol): info = self.pending_db.pop(frame_id, None) if info is None: - # 理論上不該發生；做保底：歸屬到目前 TDMA slot。 sysid = self.current_poll_sysid src64 = learned_sysid_to_addr64.get(sysid) else: @@ -533,17 +483,13 @@ class SerialToUDP(asyncio.Protocol): if sysid is not None: record_rssi(sysid, rssi_value, src64=src64) - # -------------------- UDP -> UAV downlink -------------------- - def write_to_serial(self, data): self.gcs_tx_queue.extend(data) def flush_gcs_queue(self): - """GCS -> UAV 下行資料，小批量送出，避免阻塞上行 TDMA。""" if not self.gcs_tx_queue: return - # 若還沒任何 UAV address，仍 broadcast。ESP32 端只接受已知 GCS 來源資料。 send_limit = min(len(self.gcs_tx_queue), 150) data_to_send = self.gcs_tx_queue[:send_limit] self.gcs_tx_queue = self.gcs_tx_queue[send_limit:] @@ -558,8 +504,6 @@ class SerialToUDP(asyncio.Protocol): chunk = data[sent_len:end_len] sent_len = end_len - # 下行命令目前 broadcast，所有 ESP32 收到後會寫給 FC。 - # 若你要精準下行到某台 UAV，需在 MAVLink router 層或 UI 選定 sysid 後 unicast。 api_frame = build_api_tx_frame(chunk, TARGET_ADDR64, 0x00) self.transport.write(api_frame) await asyncio.sleep(0.01) @@ -591,7 +535,6 @@ class UDPHandler(asyncio.DatagramProtocol): def handle_hello_report(self, rf_data, src64=None): """ HELO + sysid(1) - 回傳 sysid 或 None。 """ if len(rf_data) < 5 or not rf_data.startswith(HELLO_MAGIC): return None @@ -612,10 +555,6 @@ class UDPHandler(asyncio.DatagramProtocol): return None def handle_done_report(self, rf_data, src64=None): - """ - DONE + sysid(1) + sent_len(2) + remain_len(2) - 回傳 sysid 或 None。 - """ if len(rf_data) < 9 or not rf_data.startswith(DONE_MAGIC): return None @@ -640,13 +579,6 @@ class UDPHandler(asyncio.DatagramProtocol): return None def process_rf_data(self, rf_data, src64=None, sysid_hint=None): - """ - 處理 XBee 0x90 的 RF data。 - 回傳： - ('HELO', sysid) 若是 HELO - ('DONE', sysid) 若是 DONE - None 若是 MAVLink data - """ hello_sysid = self.handle_hello_report(rf_data, src64=src64) if hello_sysid is not None: return ('HELO', hello_sysid) @@ -655,7 +587,6 @@ class UDPHandler(asyncio.DatagramProtocol): if done_sysid is not None: return ('DONE', done_sysid) - # MAVLink data try: for byte in rf_data: msg = self.mav_decoder.parse_char(bytes([byte])) @@ -708,8 +639,7 @@ async def setup_bridge(config): async def auto_discovery_task(serial_protocols): """ 程式啟動前 AUTO_DISCOVERY_DURATION_SEC 秒自動送 DISC。 - 時間到後停止自動 DISC，避免 UAV 一直回 HELO。 - 後續新增 UAV 請用 GUI 的 Scan UAV 按鈕送 DSCF。 + 本版 DISC 是強制 discovery，UAV 收到後會重新回 HELO。 """ if not AUTO_DISCOVERY_ENABLED: return @@ -720,24 +650,23 @@ async def auto_discovery_task(serial_protocols): while time.time() - start_t < AUTO_DISCOVERY_DURATION_SEC: for sp in serial_protocols: if hasattr(sp, 'send_discovery'): - sp.send_discovery(force=False) + sp.send_discovery() await asyncio.sleep(DISCOVERY_INTERVAL_SEC) print("[DISCOVERY] Auto DISC stopped. Use GUI Scan UAV for new aircraft.") -async def force_discovery_burst(serial_protocols): +async def discovery_burst(serial_protocols): """ - GUI Scan UAV：送 DSCF burst。 - UAV 收到 DSCF 後會強制回 HELO。 + GUI Scan UAV：送 DISC burst。 """ - print("[DISCOVERY] Force scan started") - for _ in range(FORCE_DISCOVERY_BURST_COUNT): + print("[DISCOVERY] Manual DISC scan started") + for _ in range(DISCOVERY_BURST_COUNT): for sp in serial_protocols: if hasattr(sp, 'send_discovery'): - sp.send_discovery(force=True) - await asyncio.sleep(FORCE_DISCOVERY_BURST_GAP_SEC) - print("[DISCOVERY] Force scan finished") + sp.send_discovery() + await asyncio.sleep(DISCOVERY_BURST_GAP_SEC) + print("[DISCOVERY] Manual DISC scan finished") async def tdma_scheduler(serial_protocols): @@ -748,19 +677,16 @@ async def tdma_scheduler(serial_protocols): tdma_done_events[sysid] = asyncio.Event() while True: - # GCS 下行：每輪小批量送出，不再固定空等 100 ms for sp in serial_protocols: if hasattr(sp, 'flush_gcs_queue'): sp.flush_gcs_queue() await asyncio.sleep(current_guard_ms / 1000.0) - # 若尚未發現任何 UAV，避免空轉太快 if not ACTIVE_SYSIDS: await asyncio.sleep(0.2) continue - # UAV 上行：以 byte quota 授權，不以固定 time slot 等待 for sysid in list(ACTIVE_SYSIDS): ensure_active_sysid(sysid) @@ -798,24 +724,22 @@ async def async_main(): def start_gui(): root = tk.Tk() - root.title('UAV Packet-size TDMA Control Station - Auto Discovery / Force Scan') + root.title('UAV Packet-size TDMA Control Station - Auto Discovery') root.geometry('1350x900') - # --- 左側控制面板 --- control_frame = tk.Frame(root, width=390, bg='#f0f0f0', padx=20, pady=20) control_frame.pack(side=tk.LEFT, fill=tk.Y) tk.Label(control_frame, text='Packet-size TDMA 控制', font=('Arial', 14, 'bold'), bg='#f0f0f0').pack(pady=10) - # --- Discovery buttons --- def on_scan_uav(): if ASYNC_LOOP is not None and SERIAL_PROTOCOLS: - asyncio.run_coroutine_threadsafe(force_discovery_burst(SERIAL_PROTOCOLS), ASYNC_LOOP) - scan_label.config(text='已送出 DSCF 強制掃描') + asyncio.run_coroutine_threadsafe(discovery_burst(SERIAL_PROTOCOLS), ASYNC_LOOP) + scan_label.config(text='已送出 DISC 掃描') else: scan_label.config(text='尚未建立 serial / asyncio loop') - scan_btn = tk.Button(control_frame, text='Scan UAV / 發送 DSCF', + scan_btn = tk.Button(control_frame, text='Scan UAV / 發送 DISC', font=('Arial', 12, 'bold'), bg='#2196F3', fg='white', command=on_scan_uav) scan_btn.pack(pady=(5, 5), fill=tk.X) @@ -824,7 +748,6 @@ def start_gui(): bg='#f0f0f0', font=('Arial', 9), wraplength=330, justify=tk.LEFT) scan_label.pack(pady=(0, 15), anchor=tk.W) - # --- grant bytes slider --- grant_var = tk.IntVar(value=current_grant_bytes) def on_grant_change(val): @@ -838,7 +761,6 @@ def start_gui(): grant_label = tk.Label(control_frame, text=f'每台授權: {current_grant_bytes} bytes', bg='#f0f0f0') grant_label.pack() - # --- guard time slider --- guard_var = tk.IntVar(value=current_guard_ms) def on_guard_change(val): @@ -853,7 +775,6 @@ def start_gui(): guard_label = tk.Label(control_frame, text=f'切換保護時間: {current_guard_ms} ms', bg='#f0f0f0') guard_label.pack() - # --- UAV INIT mode --- tk.Label(control_frame, text='群機初始化控制 (INIT)', font=('Arial', 14, 'bold'), bg='#f0f0f0').pack(pady=(30, 10)) init_var = tk.IntVar(value=0) @@ -921,7 +842,6 @@ def start_gui(): command=toggle_tdma_mode) lock_btn.pack(pady=20, fill=tk.X) - # --- Reports --- report_title = tk.Label(control_frame, text='最近 HELO / DONE 回報', font=('Arial', 12, 'bold'), bg='#f0f0f0') report_title.pack(pady=(10, 5)) @@ -966,7 +886,6 @@ def start_gui(): for sid in ACTIVE_SYSIDS: ensure_rssi_row(sid) - # --- status updater --- def update_status_gui(): now = time.time() @@ -1011,7 +930,6 @@ def start_gui(): update_status_gui() - # --- 右側圖表區 --- plot_frame = tk.Frame(root) plot_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True)