esp32 不寫死sysid

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,22 +201,95 @@ 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。"""
@@ -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:] 開始
+            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:
-                    if target_sysid == MY_SYSID:
+                            # 收到 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()
+
+        try:
             gc.collect()
+        except Exception:
+            pass
+
         time.sleep_ms(10)
 
     except Exception:

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)
+
+            state = uav_states.get(sysid, {"mode": "NORMAL"})
 
-            if state['mode'] == 'INITIALIZING':
+            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,9 +1100,13 @@ 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)
 
+        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)
 
     def on_closing():
@@ -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()