diff --git a/src/unitdev04/esp32.py b/src/unitdev04/esp32.py index 54a0a48..3adb776 100644 --- a/src/unitdev04/esp32.py +++ b/src/unitdev04/esp32.py @@ -5,30 +5,72 @@ import gc # ========================================================= # ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge -# Packet-size TDMA + DONE 標籤版 +# Packet-size TDMA + DONE + SYSID / GCS address 自動學習版 # -# 核心行為： -# 1. FC -> ESP32：持續把 MAVLink bytes 存進 tx_buf。 -# 2. GCS -> ESP32：收到 POLL + SYSID + quota_bytes 才允許上行傳送。 -# 3. ESP32 -> GCS：每次最多送 quota_bytes，但只送「完整 MAVLink frame」。 -# 4. ESP32 -> GCS：送完後獨立送 DONE report，GCS 收到後可立即換下一台 UAV。 +# 對應新版 GCS discovery 設計： +# 1. GCS 開機前 30 秒自動送 DISC。 +# 2. GCS 之後停止自動 DISC。 +# 3. 若後續新增 UAV，可在 GCS GUI 按 Scan UAV，送 DSCF 強制掃描。 # -# 注意：DONE 不是接在 MAVLink bytes 後面，而是獨立 RF payload，避免污染 MAVLink stream。 +# 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。 +# +# 注意： +# - DONE / HELO 都是獨立 RF payload，不會接在 MAVLink stream 後面。 +# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。 +# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。 # ========================================================= + # ================= 設定區 ================= FC_BAUDRATE = 115200 XB_BAUDRATE = 115200 -# 這裡要改成你的地面站 / coordinator XBee 64-bit address -DEST_64 = b'\x00\x13\xA2\x00\x42\x5B\x9D\xC8' +# 不寫死地面站 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 較穩。 XBEE_MAX_PAYLOAD = 100 -# 每台 UAV 都要設定成自己的 SYSID -MY_SYSID = 15 # 第二台可改 10，第三台可改 15，依你的 MAVLink SYSID 設定 +# GCS -> UAV discovery： +# DISC：一般 discovery，同一個 GCS 只回一次 HELO。 +# DSCF：force discovery，收到後強制回 HELO，給 GUI Scan UAV 按鍵用。 +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。 +HELLO_SENT = False # GCS -> UAV poll 格式： # 舊版：b'POLL' + sysid(1) @@ -43,21 +85,86 @@ 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 + +XB_UART_ID = 2 +XB_TX_PIN = 25 +XB_RX_PIN = 26 # ========================================= -uart_fc = UART(1, baudrate=FC_BAUDRATE, tx=32, rx=33, rxbuf=4096) -uart_xb = UART(2, baudrate=XB_BAUDRATE, tx=25, rx=26, rxbuf=4096) + +uart_fc = UART(FC_UART_ID, baudrate=FC_BAUDRATE, tx=FC_TX_PIN, rx=FC_RX_PIN, rxbuf=4096) +uart_xb = UART(XB_UART_ID, baudrate=XB_BAUDRATE, tx=XB_TX_PIN, rx=XB_RX_PIN, rxbuf=4096) tx_buf = bytearray() # FC -> GCS 等待 TDMA poll 的 MAVLink stream rx_buf = bytearray() # XBee API frame parser buffer +# ========================================================= +# 基本工具函式 +# ========================================================= + def get_checksum(data): return 0xFF - (sum(data) & 0xFF) +def is_valid_addr64(addr): + if addr is None: + return False + if len(addr) != 8: + return False + if addr == b'\x00\x00\x00\x00\x00\x00\x00\x00': + return False + return True + + +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 + + if not is_valid_addr64(src64): + return False + + src64 = bytes(src64) + + if DEST_64 is None: + DEST_64 = src64 + HELLO_SENT = False + return True + + if DEST_64 != src64: + DEST_64 = src64 + HELLO_SENT = False + return True + + return False + + def build_api_tx_frame(payload): - """建立 XBee API frame type 0x10，把 payload 送到 DEST_64。""" + """ + 建立 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，降低延遲 @@ -68,17 +175,25 @@ def build_api_tx_frame(payload): frame_content.extend(payload) length = len(frame_content) + packet = bytearray() packet.append(0x7E) packet.append((length >> 8) & 0xFF) packet.append(length & 0xFF) packet.extend(frame_content) packet.append(get_checksum(frame_content)) + return packet def send_to_xbee_chunked(payload): - """把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。""" + """ + 把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。 + 若 DEST_64 尚未學到，直接不送，回傳 False。 + """ + if DEST_64 is None: + return False + total_len = len(payload) sent_len = 0 @@ -86,23 +201,96 @@ def send_to_xbee_chunked(payload): end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len) chunk = payload[sent_len:end_len] sent_len = end_len - uart_xb.write(build_api_tx_frame(chunk)) + + pkt = build_api_tx_frame(chunk) + if pkt is None: + return False + + uart_xb.write(pkt) + # 很短的讓步，避免 ESP32 UART/XBee 本地 buffer 瞬間塞爆 time.sleep_ms(1) + return True + + +# ========================================================= +# HELO / DONE 回報 +# ========================================================= + +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 時。 + """ + global _last_hello_ms + global HELLO_SENT + + if MY_SYSID is None: + return False + if DEST_64 is None: + return False + + if HELLO_SENT and not force: + return False + + now = time.ticks_ms() + if not force: + if time.ticks_diff(now, _last_hello_ms) < HELLO_MIN_INTERVAL_MS: + return False + + payload = HELLO_MAGIC + struct.pack('>B', MY_SYSID) + pkt = build_api_tx_frame(payload) + if pkt is None: + return False + + uart_xb.write(pkt) + _last_hello_ms = now + HELLO_SENT = True + + time.sleep_ms(1) + return True + def send_done_report(sent_len, remain_len): - """送出 TDMA DONE 標籤。此封包不會轉給飛控，只給 GCS scheduler 使用。""" + """ + 送出 TDMA DONE 標籤。 + 此封包不會轉給飛控，只給 GCS scheduler 使用。 + + DONE payload: + b'DONE' + sysid(1) + sent_len(2) + remain_len(2) + """ + if MY_SYSID is None: + return False + if DEST_64 is None: + return False + if sent_len > 65535: sent_len = 65535 if remain_len > 65535: remain_len = 65535 payload = DONE_MAGIC + struct.pack('>BHH', MY_SYSID, sent_len, remain_len) - uart_xb.write(build_api_tx_frame(payload)) + pkt = build_api_tx_frame(payload) + if pkt is None: + return False + + uart_xb.write(pkt) time.sleep_ms(1) + return True +# ========================================================= +# MAVLink frame 解析與 MY_SYSID 自動學習 +# ========================================================= + def find_first_mavlink_magic(buf): """找 MAVLink v1/v2 magic：0xFE 或 0xFD。""" pos_fe = buf.find(b'\xFE') @@ -122,6 +310,7 @@ def mavlink_frame_length(buf, start_idx): 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 @@ -132,7 +321,6 @@ def mavlink_frame_length(buf, start_idx): magic = buf[start_idx] if magic == 0xFE: - # v1 至少需要 magic + len if len(buf) - start_idx < 2: return None payload_len = buf[start_idx + 1] @@ -142,7 +330,6 @@ def mavlink_frame_length(buf, start_idx): return total_len if magic == 0xFD: - # v2 至少需要 magic + len + incompat_flags if len(buf) - start_idx < 3: return None payload_len = buf[start_idx + 1] @@ -156,6 +343,90 @@ def mavlink_frame_length(buf, start_idx): return None +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] + + return None + + +def learn_my_sysid_from_tx_buf(): + """ + 從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame， + 並自動學習飛控的 MAVLink SYSID。 + + 為避免雜訊造成誤判，要求連續多次看到相同 SYSID 才正式設定 MY_SYSID。 + """ + global MY_SYSID + global _sysid_candidate + global _sysid_candidate_count + global HELLO_SENT + global tx_buf + + if len(tx_buf) == 0: + return + + idx = 0 + checked = 0 + + while idx < len(tx_buf) and checked < 8: + sub = tx_buf[idx:] + rel_start = find_first_mavlink_magic(sub) + if rel_start == -1: + return + + start = idx + rel_start + frame_len = mavlink_frame_length(tx_buf, start) + if frame_len is None: + return + + sysid = get_mavlink_sysid(tx_buf, start) + + if sysid is not None and sysid > 0: + if MY_SYSID is not None: + # 正常情況不任意改變已學到的 SYSID。 + # 如果你真的會在飛行中改 FC SYSID，再另行設計重學條件。 + return + + if _sysid_candidate == sysid: + _sysid_candidate_count += 1 + else: + _sysid_candidate = sysid + _sysid_candidate_count = 1 + + if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT: + MY_SYSID = _sysid_candidate + + # 新學到 SYSID 後，允許對已知 GCS 回一次 HELO + HELLO_SENT = False + return + + idx = start + frame_len + checked += 1 + + +# ========================================================= +# tx_buf MAVLink frame 取出與裁切 +# ========================================================= + def pop_mavlink_frames_by_quota(quota_bytes): """ 從 tx_buf 取出不超過 quota_bytes 的完整 MAVLink frames。 @@ -173,6 +444,7 @@ def pop_mavlink_frames_by_quota(quota_bytes): if start == -1: tx_buf = bytearray() return b'' + if start > 0: tx_buf = tx_buf[start:] @@ -212,7 +484,9 @@ def pop_mavlink_frames_by_quota(quota_bytes): def trim_tx_buffer_if_needed(): - """tx_buf 過大時，丟掉較舊的完整 MAVLink frames，保留較新的資料。""" + """ + tx_buf 過大時，丟掉較舊的完整 MAVLink frames，保留較新的資料。 + """ global tx_buf if len(tx_buf) <= MAX_BUF_SIZE: @@ -225,6 +499,7 @@ def trim_tx_buffer_if_needed(): if start == -1: tx_buf = bytearray() return + if start > 0: tx_buf = tx_buf[start:] continue @@ -249,6 +524,13 @@ def flush_tx_buffer(grant_bytes): """ global tx_buf + # 尚未學到 SYSID 或地面站地址，不進行上行傳送。 + if MY_SYSID is None: + return + + if DEST_64 is None: + return + if grant_bytes <= 0: send_done_report(0, len(tx_buf)) return @@ -262,11 +544,16 @@ def flush_tx_buffer(grant_bytes): send_done_report(sent_len, len(tx_buf)) +# ========================================================= +# GCS control payload 解析 +# ========================================================= + 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): @@ -283,6 +570,27 @@ def parse_poll_payload(real_data): return None, None +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 @@ -313,45 +621,115 @@ def process_xbee_buffer(): checksum_recv = rx_buf[3 + pkt_len] if get_checksum(packet) == checksum_recv: - if packet[0] == 0x90: - # XBee Receive Packet 0x90：RF data 從 packet[12:] 開始 - real_data = packet[12:] - - target_sysid, grant_bytes = parse_poll_payload(real_data) - if target_sysid is not None: - if target_sysid == MY_SYSID: - flush_tx_buffer(grant_bytes) - else: - # 一般 GCS -> FC MAVLink 下行資料 - uart_fc.write(real_data) + if len(packet) > 0 and packet[0] == 0x90: + # XBee Receive Packet 0x90: + # packet = 90 | src64(8) | src16(2) | options(1) | RF data + if len(packet) >= 12: + src64 = bytes(packet[1:9]) + real_data = packet[12:] + + # ------------------------------------------------- + # GCS 一般 DISC： + # 同一個 GCS 只回一次 HELO，避免一直 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) + + else: + # ------------------------------------------------- + # 判斷是不是 POLL + # ------------------------------------------------- + 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() + + # 只有 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) + + if DEST_64 is not None and src64 == DEST_64: + uart_fc.write(real_data) rx_buf = rx_buf[total_len:] + else: + # checksum 錯誤，丟掉第一個 byte，重新對齊 rx_buf = rx_buf[1:] +# ========================================================= # 初始清理 -gc_ran = False +# ========================================================= + try: gc.collect() - gc_ran = True except Exception: pass + +# ========================================================= +# 主迴圈 +# ========================================================= + while True: try: + # ------------------------------------------------- # FC -> ESP32 tx buffer + # ------------------------------------------------- if uart_fc.any(): data = uart_fc.read(READ_FC_BYTES) if data: tx_buf.extend(data) + + # 從飛控 MAVLink stream 自動學 MY_SYSID + learn_my_sysid_from_tx_buf() + + # 避免 tx_buf 無限長大 trim_tx_buffer_if_needed() + # ------------------------------------------------- # XBee -> ESP32 + # ------------------------------------------------- if uart_xb.any(): chunk = uart_xb.read() if chunk: rx_buf.extend(chunk) + process_xbee_buffer() time.sleep_ms(1) @@ -360,7 +738,12 @@ while True: # OOM 時丟掉暫存資料，避免整顆 ESP32 死掉 tx_buf = bytearray() rx_buf = bytearray() - gc.collect() + + try: + gc.collect() + except Exception: + pass + time.sleep_ms(10) except Exception: diff --git a/src/unitdev04/udptest8.py b/src/unitdev04/udptest8.py index 4ae5ec2..c23ba1f 100644 --- a/src/unitdev04/udptest8.py +++ b/src/unitdev04/udptest8.py @@ -14,18 +14,27 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg # ========================================================= # GCS 端：XBee Serial <-> UDP Bridge -# Packet-size TDMA + DONE 標籤 + 精準 RSSI 歸屬 + 最近 5 次 RSSI 平均 +# Packet-size TDMA + DONE + RSSI + Auto Discovery + Force Scan +# +# 對應 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 可立即換下一台。 # # 核心行為： -# 1. GCS 對某個 SYSID 發 POLL + quota_bytes。 -# 2. UAV 收到自己的 POLL 後，送出最多 quota_bytes 的完整 MAVLink frames。 -# 3. UAV 送完後回 DONE，GCS 收到 DONE 後立即 poll 下一台。 -# 4. 收到 DONE 後，GCS 立即發 ATDB 查詢本地 XBee 最近一包 RF packet 的 RSSI。 -# 5. 因為 DONE payload 帶有 SYSID，且 XBee 0x90 frame 帶有 src64，RSSI 不再用 MAVLink SYSID 猜。 -# 6. RSSI 顯示採用「最近 5 次 DB RSSI」平均值，降低跳動。 +# 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。 # ========================================================= -# === 多組設備設定 === + +# ================= 多組設備設定 ================= # Windows 測試時建議先只留你的實際 COM port，例如 COM15。 CONFIGS = [ {"serial_port": "/dev/ttyUSB0", "udp_port": 14551}, @@ -37,36 +46,43 @@ CONFIGS = [ SERIAL_BAUDRATE = 115200 UDP_REMOTE_IP = '127.0.0.1' -# Broadcast：由 RF payload 內的 SYSID 決定哪台 ESP32 回應。 +# 若你要讓外部程式固定送 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' -ACTIVE_SYSIDS = [3, 10, 15] +# 起始已知 SYSID。完全自動 discovery 可設空 list。 +# 若你想保留舊版行為，可改成 [3, 10, 15]。 +INITIAL_ACTIVE_SYSIDS = [] +ACTIVE_SYSIDS = list(INITIAL_ACTIVE_SYSIDS) -# 如果你知道每台 UAV XBee 的 64-bit address，可以填在這裡。 -# 即使不填，本程式也會從 DONE frame 自動學習 src64 -> SYSID。 -# 範例： -# XBEE_ADDR64_TO_SYSID = { -# b'\x00\x13\xA2\x00\x42\x5B\x9D\xC8': 15, -# b'\x00\x13\xA2\x00\x42\x5B\x9D\xAA': 10, -# b'\x00\x13\xA2\x00\x42\x5B\x9D\xBB': 3, -# } +# 若你知道每台 UAV XBee 的 64-bit address，可以預先填入。 +# 即使不填，本程式也會從 HELO / DONE / MAVLink data 自動學習。 XBEE_ADDR64_TO_SYSID = {} AUTO_LEARN_XBEE_ADDR = True -# GCS -> UAV poll 格式：b'POLL' + sysid(1) + quota_bytes(2) -POLL_MAGIC = b'POLL' +# 控制封包 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) -# UAV -> GCS done 格式：b'DONE' + sysid(1) + sent_len(2) + remain_len(2) -DONE_MAGIC = b'DONE' +# ================= Discovery 參數 ================= +AUTO_DISCOVERY_ENABLED = True +AUTO_DISCOVERY_DURATION_SEC = 30.0 +DISCOVERY_INTERVAL_SEC = 1.5 -# === TDMA 參數 === -# 每次 poll 授權該 UAV 最多送多少 MAVLink 原始資料 bytes。 -current_grant_bytes = 600 +# GUI 按 Scan UAV 時送出 DSCF burst +FORCE_DISCOVERY_BURST_COUNT = 5 +FORCE_DISCOVERY_BURST_GAP_SEC = 0.20 -# 切換下一台 UAV 前的保護時間，避免最後一包還在 XBee/UART buffer 裡。 +# ================= TDMA 參數 ================= +current_grant_bytes = 600 current_guard_ms = 20 - -# INITIALIZING 模式給較大的 quota，用於參數載入。 INIT_GRANT_BYTES = 1200 # 丟包率計算時間視窗 @@ -75,18 +91,17 @@ LOSS_TIME_WINDOW_SEC = 5.0 # RSSI 最近 N 次平均 RSSI_AVG_WINDOW = 5 -# 是否也在資料封包時查 DB。預設 False：只在 DONE 後查 DB，比較不干擾資料流。 +# 是否也在資料封包時查 DB。預設 False：只在 DONE / HELO 後查 DB，較不干擾資料流。 RSSI_QUERY_ON_DATA_FRAME = False RSSI_DATA_QUERY_MIN_INTERVAL_SEC = 0.5 -uav_states = { - sysid: { - "mode": "NORMAL", - } for sysid in ACTIVE_SYSIDS -} -# === 狀態追蹤 === -rssi_history = defaultdict(lambda: deque(maxlen=5000)) # 畫圖用：RSSI avg5 +# ================= 全域狀態 ================= +uav_states = {} +for sid in ACTIVE_SYSIDS: + uav_states[sid] = {"mode": "NORMAL"} + +rssi_history = defaultdict(lambda: deque(maxlen=5000)) # 畫圖用：RSSI avgN rssi_raw_windows = defaultdict(lambda: deque(maxlen=RSSI_AVG_WINDOW)) rssi_time_history = defaultdict(lambda: deque(maxlen=5000)) rssi_latest_stats = defaultdict(lambda: { @@ -110,7 +125,17 @@ learned_sysid_to_addr64 = {} 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 = [] + + +# ========================================================= +# 工具函式 +# ========================================================= def format_addr64(addr): if not addr: @@ -118,6 +143,29 @@ def format_addr64(addr): return ''.join(f'{b:02X}' for b in addr) +def ensure_active_sysid(sysid): + """若發現新的 SYSID，加入 ACTIVE_SYSIDS 並建立狀態。""" + if sysid is None: + return + if sysid <= 0: + return + + if sysid not in ACTIVE_SYSIDS: + ACTIVE_SYSIDS.append(sysid) + ACTIVE_SYSIDS.sort() + print(f"[DISCOVERY] New SYSID discovered: {sysid}") + + if sysid not in uav_states: + uav_states[sysid] = {"mode": "NORMAL"} + + if sysid not in tdma_done_events: + try: + tdma_done_events[sysid] = asyncio.Event() + except RuntimeError: + # 若從 GUI thread 呼叫時沒有 running loop，scheduler 之後會補建。 + pass + + def infer_sysid_from_addr64(src64): if not src64: return None @@ -129,14 +177,37 @@ 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: return if sysid <= 0: return + + src64 = bytes(src64) 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 + + return TARGET_ADDR64 def record_rssi(sysid, rssi_positive_db, src64=None): @@ -147,6 +218,8 @@ def record_rssi(sysid, rssi_positive_db, src64=None): if sysid is None: return + ensure_active_sysid(sysid) + raw_dbm = -int(rssi_positive_db) win = rssi_raw_windows[sysid] win.append(raw_dbm) @@ -166,6 +239,9 @@ 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] now = time.time() @@ -216,6 +292,9 @@ 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)) @@ -236,6 +315,8 @@ 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) if ev is None: ev = asyncio.Event() @@ -254,6 +335,10 @@ async def grant_one_uav(serial_protocols, sysid, grant_bytes): await asyncio.sleep(current_guard_ms / 1000.0) +# ========================================================= +# SerialToUDP：XBee Serial API parser +# ========================================================= + class SerialToUDP(asyncio.Protocol): def __init__(self, udp_protocol, serial_port): self.udp_protocol = udp_protocol @@ -269,6 +354,24 @@ class SerialToUDP(asyncio.Protocol): self.pending_db = {} # frame_id -> {'sysid', 'src64', 'time', 'reason'} self.last_data_db_query_time = 0.0 + def connection_made(self, transport): + self.transport = transport + if hasattr(self.udp_protocol, 'set_serial_transport'): + self.udp_protocol.set_serial_transport(self) + print(f"[{self.serial_port}] Serial connection established.") + + # -------------------- GCS -> UAV control -------------------- + + def send_discovery(self, force=False): + """ + 發 discovery： + - force=False：DISC，一般 discovery；UAV 對同一 GCS 只回一次 HELO。 + - force=True ：DSCF，強制 discovery；UAV 會重新回 HELO。 + """ + payload = DISC_FORCE_MAGIC if force else DISC_MAGIC + api_frame = build_api_tx_frame(payload, TARGET_ADDR64, 0x00) + self.transport.write(api_frame) + def send_poll(self, target_sysid, grant_bytes=None): if grant_bytes is None: grant_bytes = current_grant_bytes @@ -278,14 +381,13 @@ class SerialToUDP(asyncio.Protocol): self.current_poll_time = time.time() poll_payload = POLL_MAGIC + struct.pack('>BH', target_sysid, grant_bytes) - api_frame = build_api_tx_frame(poll_payload, TARGET_ADDR64, 0x00) + + dest_addr64 = get_poll_dest_addr64(target_sysid) + api_frame = build_api_tx_frame(poll_payload, dest_addr64, 0x00) + self.transport.write(api_frame) - def connection_made(self, transport): - self.transport = transport - if hasattr(self.udp_protocol, 'set_serial_transport'): - self.udp_protocol.set_serial_transport(self) - print(f"[{self.serial_port}] Serial connection established.") + # -------------------- Serial RX parser -------------------- def data_received(self, data): self.buffer.extend(data) @@ -306,8 +408,7 @@ class SerialToUDP(asyncio.Protocol): full_length = 3 + length + 1 # 本程式使用 XBee RF payload 約 80~100 bytes，正常 length 不會很大。 - # 若你把 XBEE_MAX_PAYLOAD 調大，可同步放寬此限制。 - if length > 600: + if length > 700: self.buffer.pop(0) continue @@ -329,7 +430,7 @@ class SerialToUDP(asyncio.Protocol): if frame_type == 0x90: # XBee Receive Packet: - # whole API frame: 7E | len(2) | 90 | src64(8) | src16(2) | options(1) | RF data | checksum + # 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) @@ -342,7 +443,7 @@ class SerialToUDP(asyncio.Protocol): # 其他如 0x8B Transmit Status 可視需求處理；目前忽略。 def handle_rx_packet(self, src64, rf_data): - # 先用 src64 查。若尚未學習，DONE payload 會帶 sysid，可建立對應。 + # 先用 src64 查。若尚未學習，HELO / DONE payload 會帶 sysid，可建立對應。 sysid_hint = infer_sysid_from_addr64(src64) # 若在最近 poll 時槽內收到資料，也可作為備援歸屬。 @@ -350,12 +451,13 @@ class SerialToUDP(asyncio.Protocol): if time.time() - self.current_poll_time <= 2.0: sysid_hint = self.current_poll_sysid - done_sysid = self.udp_protocol.process_rf_data(rf_data, src64=src64, sysid_hint=sysid_hint) + result = self.udp_protocol.process_rf_data(rf_data, src64=src64, sysid_hint=sysid_hint) - if done_sysid is not None: - # DONE payload 明確帶 SYSID；這裡可精準學習該 SYSID 對應的 XBee src64。 - learn_xbee_source(done_sysid, src64) - self.send_at_command_db(done_sysid, src64=src64, reason='DONE') + 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) return if RSSI_QUERY_ON_DATA_FRAME and sysid_hint is not None: @@ -364,6 +466,8 @@ 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: @@ -373,7 +477,7 @@ class SerialToUDP(asyncio.Protocol): def send_at_command_db(self, sysid, src64=None, reason=''): """ 查本地 XBee 的 DB 值。DB 代表本地 XBee 最近收到 RF packet 的 RSSI。 - 這裡在 DONE 後立即查，所以 RSSI 會歸屬到剛完成 TDMA slot 的 sysid。 + 這裡在 DONE / HELO 後立即查，所以 RSSI 會歸屬到剛剛回報的 sysid。 """ try: frame_id = self.next_at_frame_id() @@ -429,6 +533,8 @@ 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) @@ -436,6 +542,8 @@ class SerialToUDP(asyncio.Protocol): """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:] @@ -450,6 +558,8 @@ 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) @@ -457,6 +567,10 @@ class SerialToUDP(asyncio.Protocol): pass +# ========================================================= +# UDPHandler：UDP / MAVLink / HELO / DONE +# ========================================================= + class UDPHandler(asyncio.DatagramProtocol): def __init__(self, udp_port): self.udp_port = udp_port @@ -474,35 +588,74 @@ class UDPHandler(asyncio.DatagramProtocol): if self.serial_transport: self.serial_transport.write_to_serial(data) - def handle_done_report(self, rf_data): + 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 + + try: + sysid = rf_data[4] + + hello_last_reports[sysid] = { + 'time': time.time(), + 'src64': src64, + } + + learn_xbee_source(sysid, src64) + print(f"[HELO] SYSID {sysid}, src64={format_addr64(src64)}") + return sysid + + except Exception: + 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 try: sysid, sent_len, remain_len = struct.unpack('>BHH', rf_data[4:9]) + tdma_last_reports[sysid] = { 'time': time.time(), 'sent_len': sent_len, 'remain_len': remain_len, } + learn_xbee_source(sysid, src64) + ev = tdma_done_events.get(sysid) if ev is not None: ev.set() return sysid + except Exception: return None def process_rf_data(self, rf_data, src64=None, sysid_hint=None): """ 處理 XBee 0x90 的 RF data。 - 回傳：若是 DONE，回傳 done_sysid；若是 MAVLink data，回傳 None。 + 回傳： + ('HELO', sysid) 若是 HELO + ('DONE', sysid) 若是 DONE + None 若是 MAVLink data """ - done_sysid = self.handle_done_report(rf_data) + hello_sysid = self.handle_hello_report(rf_data, src64=src64) + if hello_sysid is not None: + return ('HELO', hello_sysid) + + done_sysid = self.handle_done_report(rf_data, src64=src64) if done_sysid is not None: - return done_sysid + return ('DONE', done_sysid) + # MAVLink data try: for byte in rf_data: msg = self.mav_decoder.parse_char(bytes([byte])) @@ -513,11 +666,11 @@ class UDPHandler(asyncio.DatagramProtocol): if sysid == 0: continue - # MAVLink sysid 用於 packet loss 計算；XBee src64 用於 RSSI 歸屬。 if src64 is not None: learn_xbee_source(sysid, src64) calculate_packet_loss(sysid, compid, seq) + except Exception: pass @@ -527,8 +680,13 @@ class UDPHandler(asyncio.DatagramProtocol): return None +# ========================================================= +# Bridge setup / scheduler / discovery +# ========================================================= + async def setup_bridge(config): port, udp = config['serial_port'], config['udp_port'] + try: ser = serial.Serial(port, SERIAL_BAUDRATE) ser.close() @@ -538,17 +696,55 @@ async def setup_bridge(config): loop = asyncio.get_running_loop() udp_handler = UDPHandler(udp) - await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', 0)) + + local_port = udp if UDP_LISTEN_FIXED_PORT else 0 + await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', local_port)) serial_proto = SerialToUDP(udp_handler, port) await serial_asyncio.create_serial_connection(loop, lambda: serial_proto, port, baudrate=SERIAL_BAUDRATE) return serial_proto +async def auto_discovery_task(serial_protocols): + """ + 程式啟動前 AUTO_DISCOVERY_DURATION_SEC 秒自動送 DISC。 + 時間到後停止自動 DISC，避免 UAV 一直回 HELO。 + 後續新增 UAV 請用 GUI 的 Scan UAV 按鈕送 DSCF。 + """ + if not AUTO_DISCOVERY_ENABLED: + return + + print(f"[DISCOVERY] Auto DISC started for {AUTO_DISCOVERY_DURATION_SEC:.1f} seconds") + + start_t = time.time() + while time.time() - start_t < AUTO_DISCOVERY_DURATION_SEC: + for sp in serial_protocols: + if hasattr(sp, 'send_discovery'): + sp.send_discovery(force=False) + 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): + """ + GUI Scan UAV：送 DSCF burst。 + UAV 收到 DSCF 後會強制回 HELO。 + """ + print("[DISCOVERY] Force scan started") + for _ in range(FORCE_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") + + async def tdma_scheduler(serial_protocols): print(f"Packet-size TDMA Scheduler Started... 找到 {len(serial_protocols)} 個可用端口") - for sysid in ACTIVE_SYSIDS: + for sysid in list(ACTIVE_SYSIDS): + ensure_active_sysid(sysid) tdma_done_events[sysid] = asyncio.Event() while True: @@ -556,13 +752,21 @@ async def tdma_scheduler(serial_protocols): 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 ACTIVE_SYSIDS: - state = uav_states[sysid] + for sysid in list(ACTIVE_SYSIDS): + ensure_active_sysid(sysid) - if state['mode'] == 'INITIALIZING': + state = uav_states.get(sysid, {"mode": "NORMAL"}) + + if state.get('mode') == 'INITIALIZING': for _ in range(4): await grant_one_uav(serial_protocols, sysid, INIT_GRANT_BYTES) else: @@ -570,28 +774,57 @@ async def tdma_scheduler(serial_protocols): async def async_main(): + global ASYNC_LOOP + global SERIAL_PROTOCOLS + + ASYNC_LOOP = asyncio.get_running_loop() + protocols = await asyncio.gather(*(setup_bridge(cfg) for cfg in CONFIGS)) valid_protocols = [p for p in protocols if p is not None] + SERIAL_PROTOCOLS = valid_protocols + if valid_protocols: + asyncio.create_task(auto_discovery_task(valid_protocols)) asyncio.create_task(tdma_scheduler(valid_protocols)) else: print('No valid serial ports found.') + await asyncio.Future() -# === GUI 介面區 === +# ========================================================= +# GUI +# ========================================================= def start_gui(): root = tk.Tk() - root.title('UAV Packet-size TDMA Control Station - RSSI avg5') - root.geometry('1300x880') + root.title('UAV Packet-size TDMA Control Station - Auto Discovery / Force Scan') + root.geometry('1350x900') # --- 左側控制面板 --- - control_frame = tk.Frame(root, width=350, bg='#f0f0f0', padx=20, pady=20) + 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 強制掃描') + else: + scan_label.config(text='尚未建立 serial / asyncio loop') + + scan_btn = tk.Button(control_frame, text='Scan UAV / 發送 DSCF', + font=('Arial', 12, 'bold'), bg='#2196F3', fg='white', + command=on_scan_uav) + scan_btn.pack(pady=(5, 5), fill=tk.X) + + scan_label = tk.Label(control_frame, text=f'開機前 {AUTO_DISCOVERY_DURATION_SEC:.0f}s 自動 DISC，之後可手動掃描', + 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): @@ -599,11 +832,13 @@ def start_gui(): current_grant_bytes = int(float(val)) grant_label.config(text=f'每台授權: {current_grant_bytes} bytes') - grant_slider = ttk.Scale(control_frame, from_=100, to_=1800, orient='horizontal', variable=grant_var, command=on_grant_change) + grant_slider = ttk.Scale(control_frame, from_=100, to_=1800, orient='horizontal', + variable=grant_var, command=on_grant_change) grant_slider.pack(fill=tk.X, pady=10) 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): @@ -612,34 +847,46 @@ def start_gui(): guard_label.config(text=f'切換保護時間: {current_guard_ms} ms') tk.Label(control_frame, text='Guard Time', font=('Arial', 11, 'bold'), bg='#f0f0f0').pack(pady=(20, 0)) - guard_slider = ttk.Scale(control_frame, from_=0, to_=100, orient='horizontal', variable=guard_var, command=on_guard_change) + guard_slider = ttk.Scale(control_frame, from_=0, to_=100, orient='horizontal', + variable=guard_var, command=on_guard_change) guard_slider.pack(fill=tk.X, pady=10) 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) radios = {} status_labels = {} + radio_container = tk.Frame(control_frame, bg='#f0f0f0') + radio_container.pack(fill=tk.X) + def on_radio_change(): selected = init_var.get() - for sysid in ACTIVE_SYSIDS: + for sysid in list(ACTIVE_SYSIDS): + ensure_active_sysid(sysid) if sysid == selected: uav_states[sysid]['mode'] = 'INITIALIZING' else: uav_states[sysid]['mode'] = 'NORMAL' - tk.Radiobutton(control_frame, text='全體 NORMAL', variable=init_var, value=0, - command=on_radio_change, bg='#f0f0f0', font=('Arial', 11, 'bold'), fg='blue').pack(anchor=tk.W, pady=(0, 10)) + tk.Radiobutton(radio_container, text='全體 NORMAL', variable=init_var, value=0, + command=on_radio_change, bg='#f0f0f0', + font=('Arial', 11, 'bold'), fg='blue').pack(anchor=tk.W, pady=(0, 10)) + + def ensure_radio_row(sysid): + if sysid in radios: + return - for sysid in ACTIVE_SYSIDS: - frame = tk.Frame(control_frame, bg='#f0f0f0') - frame.pack(fill=tk.X, pady=5) + frame = tk.Frame(radio_container, bg='#f0f0f0') + frame.pack(fill=tk.X, pady=3) - rb = tk.Radiobutton(frame, text=f'SYSID {sysid} 專屬載入', variable=init_var, value=sysid, - command=on_radio_change, bg='#f0f0f0', font=('Arial', 11)) + rb = tk.Radiobutton(frame, text=f'SYSID {sysid} 專屬載入', + variable=init_var, value=sysid, + command=on_radio_change, + bg='#f0f0f0', font=('Arial', 11)) rb.pack(side=tk.LEFT) radios[sysid] = rb @@ -647,6 +894,9 @@ def start_gui(): lbl.pack(side=tk.RIGHT, padx=5) status_labels[sysid] = lbl + for sid in ACTIVE_SYSIDS: + ensure_radio_row(sid) + is_locked = False def toggle_tdma_mode(): @@ -669,38 +919,81 @@ def start_gui(): lock_btn = tk.Button(control_frame, text='參數載入完畢，鎖定進入 TDMA', font=('Arial', 12, 'bold'), bg='#4CAF50', fg='white', command=toggle_tdma_mode) - lock_btn.pack(pady=25, fill=tk.X) + lock_btn.pack(pady=20, fill=tk.X) - report_title = tk.Label(control_frame, text='最近 DONE 回報', font=('Arial', 12, 'bold'), bg='#f0f0f0') + # --- Reports --- + report_title = tk.Label(control_frame, text='最近 HELO / DONE 回報', font=('Arial', 12, 'bold'), bg='#f0f0f0') report_title.pack(pady=(10, 5)) + + report_container = tk.Frame(control_frame, bg='#f0f0f0') + report_container.pack(fill=tk.X) + report_labels = {} - for sysid in ACTIVE_SYSIDS: - lbl = tk.Label(control_frame, text=f'SYSID {sysid}: sent=0, remain=0', font=('Arial', 10), bg='#f0f0f0') - lbl.pack(anchor=tk.W) - report_labels[sysid] = lbl + hello_labels = {} + + def ensure_report_row(sysid): + if sysid not in hello_labels: + hl = tk.Label(report_container, text=f'SYSID {sysid}: HELO no report', + font=('Arial', 9), bg='#f0f0f0', justify=tk.LEFT) + hl.pack(anchor=tk.W) + hello_labels[sysid] = hl + + if sysid not in report_labels: + dl = tk.Label(report_container, text=f'SYSID {sysid}: DONE sent=0, remain=0', + font=('Arial', 9), bg='#f0f0f0', justify=tk.LEFT) + dl.pack(anchor=tk.W) + report_labels[sysid] = dl + + for sid in ACTIVE_SYSIDS: + ensure_report_row(sid) rssi_title = tk.Label(control_frame, text=f'RSSI 最近 {RSSI_AVG_WINDOW} 次平均', font=('Arial', 12, 'bold'), bg='#f0f0f0') rssi_title.pack(pady=(18, 5)) + + rssi_container = tk.Frame(control_frame, bg='#f0f0f0') + rssi_container.pack(fill=tk.X) + rssi_labels = {} - for sysid in ACTIVE_SYSIDS: - lbl = tk.Label(control_frame, text=f'SYSID {sysid}: RSSI avg -- dBm', font=('Arial', 10), bg='#f0f0f0', justify=tk.LEFT) + + def ensure_rssi_row(sysid): + if sysid in rssi_labels: + return + lbl = tk.Label(rssi_container, text=f'SYSID {sysid}: RSSI avg -- dBm', + font=('Arial', 9), bg='#f0f0f0', justify=tk.LEFT) lbl.pack(anchor=tk.W) rssi_labels[sysid] = lbl + for sid in ACTIVE_SYSIDS: + ensure_rssi_row(sid) + + # --- status updater --- def update_status_gui(): now = time.time() + + for sysid in list(ACTIVE_SYSIDS): + ensure_radio_row(sysid) + ensure_report_row(sysid) + ensure_rssi_row(sysid) + for sysid, lbl in status_labels.items(): - mode = uav_states[sysid]['mode'] + mode = uav_states.get(sysid, {"mode": "NORMAL"}).get("mode", "NORMAL") if mode == 'INITIALIZING': lbl.config(text='INIT', fg='orange') else: lbl.config(text='NORMAL', fg='green') + for sysid, lbl in hello_labels.items(): + rep = hello_last_reports[sysid] + age = now - rep['time'] if rep['time'] else 999.0 + age_text = f'{age:.1f}s ago' if age < 99 else 'no report' + addr_text = format_addr64(rep.get('src64')) + lbl.config(text=f"SYSID {sysid}: HELO ({age_text}) src64={addr_text}") + for sysid, lbl in report_labels.items(): rep = tdma_last_reports[sysid] age = now - rep['time'] if rep['time'] else 999.0 age_text = f'{age:.1f}s ago' if age < 99 else 'no report' - lbl.config(text=f"SYSID {sysid}: sent={rep['sent_len']}, remain={rep['remain_len']} ({age_text})") + lbl.config(text=f"SYSID {sysid}: DONE sent={rep['sent_len']}, remain={rep['remain_len']} ({age_text})") for sysid, lbl in rssi_labels.items(): st = rssi_latest_stats[sysid] @@ -722,14 +1015,15 @@ def start_gui(): plot_frame = tk.Frame(root) plot_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True) - fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(9.5, 8.3), dpi=100) + fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(9.5, 8.5), dpi=100) canvas = FigureCanvasTkAgg(fig, master=plot_frame) canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True) def update_plot(frame): ax1.clear() ax2.clear() - ax1.set_title(f'RSSI avg{RSSI_AVG_WINDOW} from XBee ATDB after DONE', fontsize=12) + + ax1.set_title(f'RSSI avg{RSSI_AVG_WINDOW} from XBee ATDB after HELO / DONE', fontsize=12) ax1.set_xlim(10, 0) ax1.set_ylim(-100, -10) ax1.grid(True, alpha=0.3) @@ -740,7 +1034,7 @@ def start_gui(): ax2.grid(True, alpha=0.3) now = time.time() - colors = ['blue', 'red', 'green', 'orange', 'purple', 'brown'] + colors = ['blue', 'red', 'green', 'orange', 'purple', 'brown', 'cyan', 'magenta'] try: sysids = sorted(list(set(list(rssi_history.keys()) + list(packet_loss_history.keys()) + ACTIVE_SYSIDS))) @@ -806,8 +1100,12 @@ def start_gui(): if abs(real_y - text_y) > 1.0: ax2.plot([lbl['x_real'], 0.5], [real_y, text_y], color=color, linestyle=':', alpha=0.6) - ax1.legend(loc='upper left') - ax2.legend(loc='upper left') + if ax1.lines: + ax1.legend(loc='upper left') + if ax2.lines: + ax2.legend(loc='upper left') + + canvas.draw_idle() ani = animation.FuncAnimation(fig, update_plot, interval=1000) @@ -819,6 +1117,10 @@ def start_gui(): root.mainloop() +# ========================================================= +# Entry point +# ========================================================= + if __name__ == '__main__': threading.Thread(target=lambda: asyncio.run(async_main()), daemon=True).start() start_gui()