處理一開始disc找不到飛機的問題

src/unitdev04/esp32.py

@@ -7,25 +7,27 @@ import gc
 # ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge
 # Packet-size TDMA + DONE + SYSID / GCS address 自動學習版
 #
-# 對應新版 GCS discovery 設計：
+# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
-# 1. GCS 開機前 30 秒自動送 DISC。
+# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
-# 2. GCS 之後停止自動 DISC。
-# 3. 若後續新增 UAV，可在 GCS GUI 按 Scan UAV，送 DSCF 強制掃描。
 #
-# ESP32 端核心行為：
+# 重要：
-# 1. FC -> ESP32：持續讀 MAVLink bytes，存進 tx_buf。
+# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。
-# 2. ESP32 從 FC MAVLink frame header 自動學自己的 MY_SYSID。
+# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。
-# 3. ESP32 收到 GCS 的 DISC / DSCF / POLL 時，從 XBee 0x90 src64 自動學 DEST_64。
+# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING；
-# 4. 收到 DISC：
+#   之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。
-#       同一個 GCS 只回一次 HELO，避免 HELO 一直洗版。
+#
-# 5. 收到 DSCF：
+# 封包格式：
-#       強制回 HELO，給 GCS 手動重新掃描用。
+# GCS -> UAV:
-# 6. 收到 POLL + SYSID + quota_bytes：
+#   DISC
-#       若 target_sysid == MY_SYSID，才根據 quota_bytes 傳完整 MAVLink frames。
+#   POLL + target_sysid(1) + grant_bytes(2)
-# 7. 傳完本輪資料後，獨立送 DONE report。
+#
+# UAV -> GCS:
+#   HELO + sysid(1)
+#   DONE + sysid(1) + sent_len(2) + remain_len(2)
 #
 # 注意：
-# - DONE / HELO 都是獨立 RF payload，不會接在 MAVLink stream 後面。
+# - HELO 不帶 remain。
+# - DONE 保留 remain。
 # - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
 # - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。
 # =========================================================
@@ -35,58 +37,40 @@ import gc
 FC_BAUDRATE = 115200
 XB_BAUDRATE = 115200
 
-# 不寫死地面站 XBee address。
-# 收到 GCS 的 DISC / DSCF / POLL / 下行 MAVLink 時，
-# 從 XBee 0x90 frame 的 src64 自動學到。
 DEST_64 = None
-
-# 不寫死每台 UAV 的 SYSID。
-# 會從 FC -> ESP32 的 MAVLink frame header 自動學到。
 MY_SYSID = None
 
-# 為了避免雜訊誤判 SYSID，要求連續確認幾次相同 SYSID 才正式採用。
 SYSID_LEARN_CONFIRM_COUNT = 3
 _sysid_candidate = None
 _sysid_candidate_count = 0
 
-# XBee API Transmit Request 內每次 RF payload 最大切片。
+# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。
-# 900HP / DigiMesh 實測常用 80~100 bytes 較穩。
+# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。
+DISC_PENDING = False
+
 XBEE_MAX_PAYLOAD = 100
 
-# GCS -> UAV discovery：
+# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。
-# 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 會 force=True，所以不會被 HELLO_SENT 擋掉。
-# 收到 DISC 時，如果 HELLO_SENT=True，就不再回 HELO。
-# 收到 DSCF 時，不管 HELLO_SENT，都強制回 HELO。
 HELLO_SENT = False
 
-# GCS -> UAV poll 格式：
-# 舊版：b'POLL' + sysid(1)
-# 新版：b'POLL' + sysid(1) + quota_bytes(2, big-endian)
 POLL_MAGIC = b'POLL'
 DEFAULT_GRANT_BYTES = 600
 
-# UAV -> GCS done 格式：
-# b'DONE' + sysid(1) + sent_len(2) + remain_len(2)
 DONE_MAGIC = b'DONE'
 
-# ESP32 RAM 有限，不要讓 FC 資料無限累積
 MAX_BUF_SIZE = 6144
 READ_FC_BYTES = 250
 
-# UART 腳位。若你的接線不同，只要改這裡。
 FC_UART_ID = 1
 FC_TX_PIN = 32
 FC_RX_PIN = 33
@@ -126,16 +110,10 @@ def learn_dest64_from_src64(src64):
     """
     UAV 收到 GCS 的 XBee 0x90 frame 時，
     0x90 裡面的 src64 就是 GCS / coordinator XBee 的 64-bit address。
-
-    若第一次學到 GCS address：
-        DEST_64 = src64
-        HELLO_SENT = False
-
-    若後來收到不同 src64：
-        視為地面站 XBee 改變，更新 DEST_64，並允許重新回 HELO。
     """
     global DEST_64
     global HELLO_SENT
+    global DISC_PENDING
 
     if not is_valid_addr64(src64):
         return False
@@ -150,6 +128,7 @@ def learn_dest64_from_src64(src64):
     if DEST_64 != src64:
         DEST_64 = src64
         HELLO_SENT = False
+        DISC_PENDING = False
         return True
 
     return False
@@ -160,18 +139,16 @@ def build_api_tx_frame(payload):
     建立 XBee API frame type 0x10，把 payload 送到 DEST_64。
     若 DEST_64 尚未學到，回傳 None。
     """
-    global DEST_64
-
     if DEST_64 is None:
         return None
 
     frame_content = bytearray()
-    frame_content.append(0x10)       # Transmit Request
+    frame_content.append(0x10)
-    frame_content.append(0x00)       # Frame ID = 0，不要求 XBee ACK，降低延遲
+    frame_content.append(0x00)
     frame_content.extend(DEST_64)
     frame_content.extend(b'\xFF\xFE')
-    frame_content.append(0x00)       # broadcast radius
+    frame_content.append(0x00)
-    frame_content.append(0x00)       # options
+    frame_content.append(0x00)
     frame_content.extend(payload)
 
     length = len(frame_content)
@@ -189,7 +166,6 @@ def build_api_tx_frame(payload):
 def send_to_xbee_chunked(payload):
     """
     把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。
-    若 DEST_64 尚未學到，直接不送，回傳 False。
     """
     if DEST_64 is None:
         return False
@@ -207,8 +183,6 @@ def send_to_xbee_chunked(payload):
             return False
 
         uart_xb.write(pkt)
-
-        # 很短的讓步，避免 ESP32 UART/XBee 本地 buffer 瞬間塞爆
         time.sleep_ms(1)
 
     return True
@@ -223,12 +197,7 @@ def send_hello_report(force=False):
     UAV 回報自己存在：
         HELO + MY_SYSID
 
-    force=False：
+    本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。
-        若 HELLO_SENT=True，代表同一個 GCS 已經註冊過，不再回 HELO。
-
-    force=True：
-        不管 HELLO_SENT，都重新回 HELO。
-        用於 GCS GUI 按 Scan UAV 送 DSCF 時。
     """
     global _last_hello_ms
     global HELLO_SENT
@@ -259,11 +228,22 @@ def send_hello_report(force=False):
     return True
 
 
+def try_send_pending_hello():
+    """
+    若先收到 DISC、後學到 MY_SYSID，這裡會自動補送 HELO。
+    """
+    global DISC_PENDING
+
+    if DISC_PENDING and MY_SYSID is not None and DEST_64 is not None:
+        delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
+        time.sleep_ms(delay_ms)
+
+        if send_hello_report(force=True):
+            DISC_PENDING = False
+
+
 def send_done_report(sent_len, remain_len):
     """
-    送出 TDMA DONE 標籤。
-    此封包不會轉給飛控，只給 GCS scheduler 使用。
-
     DONE payload:
         b'DONE' + sysid(1) + sent_len(2) + remain_len(2)
     """
@@ -292,7 +272,6 @@ def send_done_report(sent_len, remain_len):
 # =========================================================
 
 def find_first_mavlink_magic(buf):
-    """找 MAVLink v1/v2 magic：0xFE 或 0xFD。"""
     pos_fe = buf.find(b'\xFE')
     pos_fd = buf.find(b'\xFD')
 
@@ -304,17 +283,6 @@ def find_first_mavlink_magic(buf):
 
 
 def mavlink_frame_length(buf, start_idx):
-    """
-    回傳從 start_idx 開始的一包 MAVLink frame 長度。
-    若 header 還不完整或整包還沒收完，回傳 None。
-
-    MAVLink v1:
-        magic 0xFE, payload_len at byte 1, total = payload_len + 8
-
-    MAVLink v2:
-        magic 0xFD, payload_len at byte 1, incompat_flags at byte 2,
-        total = payload_len + 12，若 signed 則 +13
-    """
     if start_idx >= len(buf):
         return None
 
@@ -344,23 +312,15 @@ def mavlink_frame_length(buf, start_idx):
 
 
 def get_mavlink_sysid(buf, start_idx):
-    """
-    從 MAVLink frame header 讀出 SYSID。
-    不做 CRC 驗證，但會搭配多次確認降低誤判。
-    """
     if start_idx >= len(buf):
         return None
 
     magic = buf[start_idx]
 
-    # MAVLink v1:
-    # 0xFE | len | seq | sysid | compid | msgid | payload | checksum
     if magic == 0xFE:
         if len(buf) - start_idx >= 6:
             return buf[start_idx + 3]
 
-    # MAVLink v2:
-    # 0xFD | len | incompat | compat | seq | sysid | compid | msgid(3) | payload | checksum
     if magic == 0xFD:
         if len(buf) - start_idx >= 10:
             return buf[start_idx + 5]
@@ -373,7 +333,7 @@ def learn_my_sysid_from_tx_buf():
     從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame，
     並自動學習飛控的 MAVLink SYSID。
 
-    為避免雜訊造成誤判，要求連續多次看到相同 SYSID 才正式設定 MY_SYSID。
+    若更改飛控 SYSID，建議重新上電 ESP32，讓 MY_SYSID 重新學習。
     """
     global MY_SYSID
     global _sysid_candidate
@@ -402,8 +362,6 @@ def learn_my_sysid_from_tx_buf():
 
         if sysid is not None and sysid > 0:
             if MY_SYSID is not None:
-                # 正常情況不任意改變已學到的 SYSID。
-                # 如果你真的會在飛行中改 FC SYSID，再另行設計重學條件。
                 return
 
             if _sysid_candidate == sysid:
@@ -414,8 +372,6 @@ def learn_my_sysid_from_tx_buf():
 
             if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT:
                 MY_SYSID = _sysid_candidate
-
-                # 新學到 SYSID 後，允許對已知 GCS 回一次 HELO
                 HELLO_SENT = False
                 return
 
@@ -428,18 +384,11 @@ def learn_my_sysid_from_tx_buf():
 # =========================================================
 
 def pop_mavlink_frames_by_quota(quota_bytes):
-    """
-    從 tx_buf 取出不超過 quota_bytes 的完整 MAVLink frames。
-    不會使用 rfind(0xFD) 亂切，避免 payload 中剛好有 0xFD 造成誤判。
-
-    若第一個完整 MAVLink frame 比 quota 還大，仍會送出第一包，避免永遠卡住。
-    """
     global tx_buf
 
     if quota_bytes <= 0 or len(tx_buf) == 0:
         return b''
 
-    # 丟掉 MAVLink magic 前面的雜訊
     start = find_first_mavlink_magic(tx_buf)
     if start == -1:
         tx_buf = bytearray()
@@ -451,7 +400,6 @@ def pop_mavlink_frames_by_quota(quota_bytes):
     out = bytearray()
 
     while len(tx_buf) > 0:
-        # 再次對齊 magic，避免中間出現雜訊
         if tx_buf[0] not in (0xFE, 0xFD):
             start = find_first_mavlink_magic(tx_buf)
             if start == -1:
@@ -461,14 +409,11 @@ def pop_mavlink_frames_by_quota(quota_bytes):
 
         frame_len = mavlink_frame_length(tx_buf, 0)
         if frame_len is None:
-            # 第一包尚未完整，留在 buffer 等下一輪 FC bytes
             break
 
-        # 正常情況：加了這包會超過 quota，就先停止
         if len(out) > 0 and (len(out) + frame_len) > quota_bytes:
             break
 
-        # 第一包就比 quota 大：仍送出，避免 quota 設太小導致永遠送不出去
         if len(out) == 0 and frame_len > quota_bytes:
             out.extend(tx_buf[:frame_len])
             tx_buf = tx_buf[frame_len:]
@@ -484,9 +429,6 @@ def pop_mavlink_frames_by_quota(quota_bytes):
 
 
 def trim_tx_buffer_if_needed():
-    """
-    tx_buf 過大時，丟掉較舊的完整 MAVLink frames，保留較新的資料。
-    """
     global tx_buf
 
     if len(tx_buf) <= MAX_BUF_SIZE:
@@ -506,25 +448,16 @@ def trim_tx_buffer_if_needed():
 
         frame_len = mavlink_frame_length(tx_buf, 0)
         if frame_len is None:
-            # 若剩下的是超大半包資料，直接保留最後 target_size bytes
             if len(tx_buf) > target_size:
                 tx_buf = tx_buf[-target_size:]
             return
 
-        # 丟掉最舊的一包完整 MAVLink frame
         tx_buf = tx_buf[frame_len:]
 
 
 def flush_tx_buffer(grant_bytes):
-    """
-    收到自己 SYSID 的 POLL 後執行：
-    1. 根據 grant_bytes 取出完整 MAVLink frames。
-    2. 用 XBee API frame 送出去。
-    3. 獨立送 DONE report，讓 GCS 立即進下一台。
-    """
     global tx_buf
 
-    # 尚未學到 SYSID 或地面站地址，不進行上行傳送。
     if MY_SYSID is None:
         return
 
@@ -549,13 +482,6 @@ def flush_tx_buffer(grant_bytes):
 # =========================================================
 
 def parse_poll_payload(real_data):
-    """
-    支援兩種 poll：
-    舊版：POLL + sysid，共 5 bytes
-    新版：POLL + sysid + quota，共 7 bytes
-
-    回傳 (target_sysid, grant_bytes)，不是 poll 則回傳 (None, None)。
-    """
     if not real_data.startswith(POLL_MAGIC):
         return None, None
 
@@ -571,28 +497,16 @@ def parse_poll_payload(real_data):
 
 
 def is_discovery_payload(real_data):
-    """
-    一般 discovery：
-        b'DISC'
-    """
     return real_data.startswith(DISC_MAGIC)
 
 
-def is_force_discovery_payload(real_data):
-    """
-    強制 discovery：
-        b'DSCF'
-    用於 GCS GUI Scan UAV 按鍵。
-    """
-    return real_data.startswith(DISC_FORCE_MAGIC)
-
-
 # =========================================================
 # XBee API RX parser
 # =========================================================
 
 def process_xbee_buffer():
     global rx_buf
+    global DISC_PENDING
 
     while True:
         start_pos = rx_buf.find(b'\x7E')
@@ -609,7 +523,6 @@ def process_xbee_buffer():
         pkt_len = (rx_buf[1] << 8) | rx_buf[2]
         total_len = pkt_len + 4
 
-        # 避免亂資料讓 buffer 爆掉；XBee RX frame 通常不會太大
         if pkt_len > 300:
             rx_buf = rx_buf[1:]
             continue
@@ -629,33 +542,20 @@ def process_xbee_buffer():
                     real_data = packet[12:]
 
                     # -------------------------------------------------
-                    # GCS 一般 DISC：
+                    # DISC：本版唯一 discovery 封包。
-                    # 同一個 GCS 只回一次 HELO，避免一直 HELO。
+                    # 功能等同原本 DSCF：收到後強制回 HELO。
                     # -------------------------------------------------
                     if is_discovery_payload(real_data):
                         learn_dest64_from_src64(src64)
                         learn_my_sysid_from_tx_buf()
 
-                        if MY_SYSID is not None and DEST_64 is not None:
-                            # 加一點由 SYSID / 時間產生的延遲，降低多機同時 HELO 撞包機率。
-                            # 若 HELLO_SENT=True，send_hello_report() 會自己擋掉。
-                            delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
-                            time.sleep_ms(delay_ms)
-                            send_hello_report(force=False)
-
-                    # -------------------------------------------------
-                    # GCS 強制 DSCF：
-                    # 不管 HELLO_SENT，重新回 HELO。
-                    # 用於 GCS GUI Scan UAV 按鍵。
-                    # -------------------------------------------------
-                    elif is_force_discovery_payload(real_data):
-                        learn_dest64_from_src64(src64)
-                        learn_my_sysid_from_tx_buf()
-
                         if MY_SYSID is not None and DEST_64 is not None:
                             delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
                             time.sleep_ms(delay_ms)
                             send_hello_report(force=True)
+                            DISC_PENDING = False
+                        else:
+                            DISC_PENDING = True
 
                     else:
                         # -------------------------------------------------
@@ -664,21 +564,15 @@ def process_xbee_buffer():
                         target_sysid, grant_bytes = parse_poll_payload(real_data)
 
                         if target_sysid is not None:
-                            # 收到 GCS 的 POLL，也可以從 src64 學地面站地址。
                             learn_dest64_from_src64(src64)
-
-                            # 嘗試從 FC buffer 學 MY_SYSID。
                             learn_my_sysid_from_tx_buf()
+                            try_send_pending_hello()
 
-                            # 只有 target_sysid 等於自己飛控的 SYSID，才回傳。
                             if MY_SYSID is not None and target_sysid == MY_SYSID:
                                 flush_tx_buffer(grant_bytes)
 
                         else:
-                            # -------------------------------------------------
                             # 一般 GCS -> FC MAVLink 下行資料
-                            # 只接受來自已知 GCS address 的資料，避免其他節點污染飛控。
-                            # -------------------------------------------------
                             if DEST_64 is None:
                                 learn_dest64_from_src64(src64)
 
@@ -688,7 +582,6 @@ def process_xbee_buffer():
             rx_buf = rx_buf[total_len:]
 
         else:
-            # checksum 錯誤，丟掉第一個 byte，重新對齊
             rx_buf = rx_buf[1:]
 
 
@@ -716,10 +609,8 @@ while True:
             if data:
                 tx_buf.extend(data)
 
-                # 從飛控 MAVLink stream 自動學 MY_SYSID
                 learn_my_sysid_from_tx_buf()
-
+                try_send_pending_hello()
-                # 避免 tx_buf 無限長大
                 trim_tx_buffer_if_needed()
 
         # -------------------------------------------------
@@ -735,7 +626,6 @@ while True:
         time.sleep_ms(1)
 
     except MemoryError:
-        # OOM 時丟掉暫存資料，避免整顆 ESP32 死掉
         tx_buf = bytearray()
         rx_buf = bytearray()
 
@@ -747,5 +637,4 @@ while True:
         time.sleep_ms(10)
 
     except Exception:
-        # 現場飛行時避免因單次解析錯誤中止主迴圈
         time.sleep_ms(2)

src/unitdev04/udptest8.py

@@ -14,28 +14,28 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
 
 # =========================================================
 # GCS 端：XBee Serial <-> UDP Bridge
-# Packet-size TDMA + DONE + RSSI + Auto Discovery + Force Scan
+# Packet-size TDMA + DONE + RSSI + Auto Discovery
 #
-# 對應 ESP32 端：
+# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
-# - DISC：一般 discovery。UAV 對同一個 GCS 只回一次 HELO。
+# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
-# - DSCF：force discovery。GUI 按 Scan UAV 時送出，UAV 強制回 HELO。
-# - HELO：UAV -> GCS，回報 SYSID。
-# - POLL：GCS -> UAV，授權某台 SYSID 傳送 quota_bytes。
-# - DONE：UAV -> GCS，表示本輪資料已送完，GCS 可立即換下一台。
 #
-# 核心行為：
+# 重要：
-# 1. 程式啟動前 AUTO_DISCOVERY_DURATION_SEC 秒自動 broadcast DISC。
+# - DISC 的功能等同於原本的 DSCF，也就是強制 discovery。
-# 2. 收到 HELO 後，GCS 從 XBee 0x90 src64 學 UAV XBee address，
+# - GCS 開機自動 discovery 與 GUI Scan UAV 都送 DISC。
-#    從 HELO payload 學 MAVLink SYSID，建立 SYSID <-> src64 對應。
+# - UAV 收到 DISC 後會重新回 HELO。
-# 3. TDMA scheduler 依 ACTIVE_SYSIDS 輪詢。
+#
-# 4. send_poll() 若已知 SYSID 對應 src64，則 unicast；未知則 fallback broadcast。
+# 封包格式：
-# 5. 收到 DONE 後立即查 ATDB，將 RSSI 歸屬到該 SYSID。
+# GCS -> UAV:
-# 6. GUI 的 Scan UAV 按鈕會送 DSCF burst，讓新加入或已存在的 UAV 強制回 HELO。
+#   DISC
+#   POLL + target_sysid(1) + grant_bytes(2)
+#
+# UAV -> GCS:
+#   HELO + sysid(1)
+#   DONE + sysid(1) + sent_len(2) + remain_len(2)
 # =========================================================
 
 
 # ================= 多組設備設定 =================
-# Windows 測試時建議先只留你的實際 COM port，例如 COM15。
 CONFIGS = [
     {"serial_port": "/dev/ttyUSB0", "udp_port": 14551},
     {"serial_port": "COM15", "udp_port": 14590},
@@ -46,52 +46,40 @@ CONFIGS = [
 SERIAL_BAUDRATE = 115200
 UDP_REMOTE_IP = '127.0.0.1'
 
-# 若你要讓外部程式固定送 UDP 到本 bridge，可改成 True。
-# False：本程式只負責 sendto 到 udp_port；本地接收 port 由系統隨機配置。
-# True ：本程式綁定 local UDP port = config['udp_port']。
 UDP_LISTEN_FIXED_PORT = False
 
-# XBee broadcast address
 TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
 
-# 起始已知 SYSID。完全自動 discovery 可設空 list。
-# 若你想保留舊版行為，可改成 [3, 10, 15]。
 INITIAL_ACTIVE_SYSIDS = []
 ACTIVE_SYSIDS = list(INITIAL_ACTIVE_SYSIDS)
 
-# 若你知道每台 UAV XBee 的 64-bit address，可以預先填入。
-# 即使不填，本程式也會從 HELO / DONE / MAVLink data 自動學習。
 XBEE_ADDR64_TO_SYSID = {}
 AUTO_LEARN_XBEE_ADDR = True
 
-# 控制封包 magic
+# 只保留 DISC；功能等同原本 DSCF。
-DISC_MAGIC = b'DISC'      # GCS -> UAV 一般 discovery
+DISC_MAGIC = b'DISC'
-DISC_FORCE_MAGIC = b'DSCF'  # GCS -> UAV 強制 discovery，GUI Scan UAV 用
+HELLO_MAGIC = b'HELO'
-HELLO_MAGIC = b'HELO'     # UAV -> GCS: HELO + sysid(1)
+POLL_MAGIC = b'POLL'
-POLL_MAGIC = b'POLL'      # GCS -> UAV: POLL + sysid(1) + quota_bytes(2)
+DONE_MAGIC = b'DONE'
-DONE_MAGIC = b'DONE'      # UAV -> GCS: DONE + sysid(1) + sent_len(2) + remain_len(2)
 
 # ================= Discovery 參數 =================
 AUTO_DISCOVERY_ENABLED = True
 AUTO_DISCOVERY_DURATION_SEC = 30.0
 DISCOVERY_INTERVAL_SEC = 1.5
 
-# GUI 按 Scan UAV 時送出 DSCF burst
+# GUI 按 Scan UAV 時送出 DISC burst
-FORCE_DISCOVERY_BURST_COUNT = 5
+DISCOVERY_BURST_COUNT = 5
-FORCE_DISCOVERY_BURST_GAP_SEC = 0.20
+DISCOVERY_BURST_GAP_SEC = 0.20
 
 # ================= TDMA 參數 =================
 current_grant_bytes = 600
 current_guard_ms = 20
 INIT_GRANT_BYTES = 1200
 
-# 丟包率計算時間視窗
 LOSS_TIME_WINDOW_SEC = 5.0
 
-# RSSI 最近 N 次平均
 RSSI_AVG_WINDOW = 5
 
-# 是否也在資料封包時查 DB。預設 False：只在 DONE / HELO 後查 DB，較不干擾資料流。
 RSSI_QUERY_ON_DATA_FRAME = False
 RSSI_DATA_QUERY_MIN_INTERVAL_SEC = 0.5
 
@@ -101,7 +89,7 @@ uav_states = {}
 for sid in ACTIVE_SYSIDS:
     uav_states[sid] = {"mode": "NORMAL"}
 
-rssi_history = defaultdict(lambda: deque(maxlen=5000))          # 畫圖用：RSSI avgN
+rssi_history = defaultdict(lambda: deque(maxlen=5000))
 rssi_raw_windows = defaultdict(lambda: deque(maxlen=RSSI_AVG_WINDOW))
 rssi_time_history = defaultdict(lambda: deque(maxlen=5000))
 rssi_latest_stats = defaultdict(lambda: {
@@ -117,18 +105,14 @@ packet_loss_time_history = defaultdict(lambda: deque(maxlen=1000))
 mavlink_sequence_tracker = defaultdict(dict)
 packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0})
 
-# 自動學習到的 XBee address 對應
 learned_addr64_to_sysid = {}
 learned_sysid_to_addr64 = {}
 
-# 每個 SYSID 一個 asyncio.Event；收到 DONE 後 set，scheduler 立即換下一台。
 tdma_done_events = {}
 tdma_last_reports = defaultdict(lambda: {'time': 0.0, 'sent_len': 0, 'remain_len': 0})
 
-# HELO 回報紀錄
 hello_last_reports = defaultdict(lambda: {'time': 0.0, 'src64': None})
 
-# asyncio loop / serial protocols 給 GUI button 使用
 ASYNC_LOOP = None
 SERIAL_PROTOCOLS = []
 
@@ -144,7 +128,6 @@ def format_addr64(addr):
 
 
 def ensure_active_sysid(sysid):
-    """若發現新的 SYSID，加入 ACTIVE_SYSIDS 並建立狀態。"""
     if sysid is None:
         return
     if sysid <= 0:
@@ -162,7 +145,6 @@ def ensure_active_sysid(sysid):
         try:
             tdma_done_events[sysid] = asyncio.Event()
         except RuntimeError:
-            # 若從 GUI thread 呼叫時沒有 running loop，scheduler 之後會補建。
             pass
 
 
@@ -177,10 +159,6 @@ def infer_sysid_from_addr64(src64):
 
 
 def learn_xbee_source(sysid, src64):
-    """
-    建立 UAV XBee src64 <-> MAVLink SYSID 對應。
-    GCS 端收到 UAV 的 0x90 frame 時，src64 是 UAV 的 XBee address。
-    """
     if not AUTO_LEARN_XBEE_ADDR:
         return
     if sysid is None or src64 is None:
@@ -189,20 +167,29 @@ def learn_xbee_source(sysid, src64):
         return
 
     src64 = bytes(src64)
+
+    old_sysid = learned_addr64_to_sysid.get(src64)
+    if old_sysid is not None and old_sysid != sysid:
+        print(f"[DISCOVERY] src64 {format_addr64(src64)} changed SYSID {old_sysid} -> {sysid}")
+        learned_sysid_to_addr64.pop(old_sysid, None)
+        if old_sysid in ACTIVE_SYSIDS:
+            ACTIVE_SYSIDS.remove(old_sysid)
+        uav_states.pop(old_sysid, None)
+        tdma_done_events.pop(old_sysid, None)
+
+    old_src64 = learned_sysid_to_addr64.get(sysid)
+    if old_src64 is not None and old_src64 != src64:
+        learned_addr64_to_sysid.pop(old_src64, None)
+
     learned_addr64_to_sysid[src64] = sysid
     learned_sysid_to_addr64[sysid] = src64
     ensure_active_sysid(sysid)
 
 
 def get_poll_dest_addr64(sysid):
-    """
-    已知 SYSID -> src64 時，優先 unicast。
-    未知時 fallback broadcast。
-    """
     if sysid in learned_sysid_to_addr64:
         return learned_sysid_to_addr64[sysid]
 
-    # 使用者預填的 XBEE_ADDR64_TO_SYSID 反查
     for addr, sid in XBEE_ADDR64_TO_SYSID.items():
         if sid == sysid:
             return addr
@@ -211,10 +198,6 @@ def get_poll_dest_addr64(sysid):
 
 
 def record_rssi(sysid, rssi_positive_db, src64=None):
-    """
-    XBee ATDB 回傳通常是正值，例如 55 表示 -55 dBm。
-    這裡統一轉成負 dBm，並做最近 RSSI_AVG_WINDOW 次平均。
-    """
     if sysid is None:
         return
 
@@ -238,8 +221,6 @@ def record_rssi(sysid, rssi_positive_db, src64=None):
 
 
 def calculate_packet_loss(sysid, compid, current_seq):
-    global mavlink_sequence_tracker, packet_loss_stats
-
     ensure_active_sysid(sysid)
 
     tracker = mavlink_sequence_tracker[sysid]
@@ -292,18 +273,11 @@ def calculate_packet_loss(sysid, compid, current_seq):
 
 
 def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x00) -> bytes:
-    """
-    GCS XBee API TX frame type 0x10。
-    """
     frame = b'\x10' + struct.pack('>B', frame_id) + dest_addr64 + b'\xFF\xFE\x00\x00' + data
     return b'\x7E' + struct.pack('>H', len(frame)) + frame + struct.pack('B', 0xFF - (sum(frame) & 0xFF))
 
 
 def estimate_tdma_timeout(grant_bytes):
-    """
-    DONE 正常收到時不會等到 timeout。
-    timeout 只在 DONE 掉包或該台沒回應時保護 scheduler。
-    """
     grant_bytes = max(0, int(grant_bytes))
     max_payload = 100
     chunks = max(1, (grant_bytes + max_payload - 1) // max_payload)
@@ -314,7 +288,6 @@ def estimate_tdma_timeout(grant_bytes):
 
 
 async def grant_one_uav(serial_protocols, sysid, grant_bytes):
-    """授權一台 UAV；收到 DONE 立即結束，否則 timeout 後換下一台。"""
     ensure_active_sysid(sysid)
 
     ev = tdma_done_events.get(sysid)
@@ -351,7 +324,7 @@ class SerialToUDP(asyncio.Protocol):
         self.current_poll_time = 0.0
 
         self.at_frame_id = 0x20
-        self.pending_db = {}  # frame_id -> {'sysid', 'src64', 'time', 'reason'}
+        self.pending_db = {}
         self.last_data_db_query_time = 0.0
 
     def connection_made(self, transport):
@@ -360,16 +333,12 @@ class SerialToUDP(asyncio.Protocol):
             self.udp_protocol.set_serial_transport(self)
         print(f"[{self.serial_port}] Serial connection established.")
 
-    # -------------------- GCS -> UAV control --------------------
+    def send_discovery(self):
-
-    def send_discovery(self, force=False):
         """
-        發 discovery：
+        發 DISC。
-        - force=False：DISC，一般 discovery；UAV 對同一 GCS 只回一次 HELO。
+        本版 DISC 功能等同原本 DSCF，UAV 收到後會強制回 HELO。
-        - force=True ：DSCF，強制 discovery；UAV 會重新回 HELO。
         """
-        payload = DISC_FORCE_MAGIC if force else DISC_MAGIC
+        api_frame = build_api_tx_frame(DISC_MAGIC, TARGET_ADDR64, 0x00)
-        api_frame = build_api_tx_frame(payload, TARGET_ADDR64, 0x00)
         self.transport.write(api_frame)
 
     def send_poll(self, target_sysid, grant_bytes=None):
@@ -387,8 +356,6 @@ class SerialToUDP(asyncio.Protocol):
 
         self.transport.write(api_frame)
 
-    # -------------------- Serial RX parser --------------------
-
     def data_received(self, data):
         self.buffer.extend(data)
 
@@ -407,7 +374,6 @@ class SerialToUDP(asyncio.Protocol):
             length = (self.buffer[1] << 8) | self.buffer[2]
             full_length = 3 + length + 1
 
-            # 本程式使用 XBee RF payload 約 80~100 bytes，正常 length 不會很大。
             if length > 700:
                 self.buffer.pop(0)
                 continue
@@ -429,8 +395,6 @@ class SerialToUDP(asyncio.Protocol):
         frame_type = frame[3]
 
         if frame_type == 0x90:
-            # XBee Receive Packet:
-            # 7E | len(2) | 90 | src64(8) | src16(2) | options(1) | RF data | checksum
             src64 = frame[4:12]
             rf_data = frame[15:-1]
             self.handle_rx_packet(src64, rf_data)
@@ -440,13 +404,9 @@ class SerialToUDP(asyncio.Protocol):
             self.handle_at_response(frame)
             return
 
-        # 其他如 0x8B Transmit Status 可視需求處理；目前忽略。
-
     def handle_rx_packet(self, src64, rf_data):
-        # 先用 src64 查。若尚未學習，HELO / DONE payload 會帶 sysid，可建立對應。
         sysid_hint = infer_sysid_from_addr64(src64)
 
-        # 若在最近 poll 時槽內收到資料，也可作為備援歸屬。
         if sysid_hint is None and self.current_poll_sysid is not None:
             if time.time() - self.current_poll_time <= 2.0:
                 sysid_hint = self.current_poll_sysid
@@ -454,7 +414,6 @@ class SerialToUDP(asyncio.Protocol):
         result = self.udp_protocol.process_rf_data(rf_data, src64=src64, sysid_hint=sysid_hint)
 
         if result is not None:
-            # result: ('DONE' or 'HELO', sysid)
             reason, sysid = result
             learn_xbee_source(sysid, src64)
             self.send_at_command_db(sysid, src64=src64, reason=reason)
@@ -466,8 +425,6 @@ class SerialToUDP(asyncio.Protocol):
                 self.send_at_command_db(sysid_hint, src64=src64, reason='DATA')
                 self.last_data_db_query_time = now
 
-    # -------------------- ATDB RSSI --------------------
-
     def next_at_frame_id(self):
         self.at_frame_id += 1
         if self.at_frame_id > 0xFE:
@@ -475,10 +432,6 @@ class SerialToUDP(asyncio.Protocol):
         return self.at_frame_id
 
     def send_at_command_db(self, sysid, src64=None, reason=''):
-        """
-        查本地 XBee 的 DB 值。DB 代表本地 XBee 最近收到 RF packet 的 RSSI。
-        這裡在 DONE / HELO 後立即查，所以 RSSI 會歸屬到剛剛回報的 sysid。
-        """
         try:
             frame_id = self.next_at_frame_id()
             frame_type = 0x08
@@ -495,7 +448,6 @@ class SerialToUDP(asyncio.Protocol):
                 'reason': reason,
             }
 
-            # 清掉太舊 pending，避免長時間累積
             now = time.time()
             stale = [fid for fid, info in self.pending_db.items() if now - info['time'] > 2.0]
             for fid in stale:
@@ -506,7 +458,6 @@ class SerialToUDP(asyncio.Protocol):
             print(f"[{self.serial_port}] send ATDB failed: {e}")
 
     def handle_at_response(self, frame):
-        # AT Command Response: 0x88 | frame_id | AT(2) | status | value...
         if len(frame) < 9:
             return
 
@@ -523,7 +474,6 @@ class SerialToUDP(asyncio.Protocol):
         info = self.pending_db.pop(frame_id, None)
 
         if info is None:
-            # 理論上不該發生；做保底：歸屬到目前 TDMA slot。
             sysid = self.current_poll_sysid
             src64 = learned_sysid_to_addr64.get(sysid)
         else:
@@ -533,17 +483,13 @@ class SerialToUDP(asyncio.Protocol):
         if sysid is not None:
             record_rssi(sysid, rssi_value, src64=src64)
 
-    # -------------------- UDP -> UAV downlink --------------------
-
     def write_to_serial(self, data):
         self.gcs_tx_queue.extend(data)
 
     def flush_gcs_queue(self):
-        """GCS -> UAV 下行資料，小批量送出，避免阻塞上行 TDMA。"""
         if not self.gcs_tx_queue:
             return
 
-        # 若還沒任何 UAV address，仍 broadcast。ESP32 端只接受已知 GCS 來源資料。
         send_limit = min(len(self.gcs_tx_queue), 150)
         data_to_send = self.gcs_tx_queue[:send_limit]
         self.gcs_tx_queue = self.gcs_tx_queue[send_limit:]
@@ -558,8 +504,6 @@ class SerialToUDP(asyncio.Protocol):
                 chunk = data[sent_len:end_len]
                 sent_len = end_len
 
-                # 下行命令目前 broadcast，所有 ESP32 收到後會寫給 FC。
-                # 若你要精準下行到某台 UAV，需在 MAVLink router 層或 UI 選定 sysid 後 unicast。
                 api_frame = build_api_tx_frame(chunk, TARGET_ADDR64, 0x00)
                 self.transport.write(api_frame)
                 await asyncio.sleep(0.01)
@@ -591,7 +535,6 @@ class UDPHandler(asyncio.DatagramProtocol):
     def handle_hello_report(self, rf_data, src64=None):
         """
         HELO + sysid(1)
-        回傳 sysid 或 None。
         """
         if len(rf_data) < 5 or not rf_data.startswith(HELLO_MAGIC):
             return None
@@ -612,10 +555,6 @@ class UDPHandler(asyncio.DatagramProtocol):
             return None
 
     def handle_done_report(self, rf_data, src64=None):
-        """
-        DONE + sysid(1) + sent_len(2) + remain_len(2)
-        回傳 sysid 或 None。
-        """
         if len(rf_data) < 9 or not rf_data.startswith(DONE_MAGIC):
             return None
 
@@ -640,13 +579,6 @@ class UDPHandler(asyncio.DatagramProtocol):
             return None
 
     def process_rf_data(self, rf_data, src64=None, sysid_hint=None):
-        """
-        處理 XBee 0x90 的 RF data。
-        回傳：
-            ('HELO', sysid) 若是 HELO
-            ('DONE', sysid) 若是 DONE
-            None 若是 MAVLink data
-        """
         hello_sysid = self.handle_hello_report(rf_data, src64=src64)
         if hello_sysid is not None:
             return ('HELO', hello_sysid)
@@ -655,7 +587,6 @@ class UDPHandler(asyncio.DatagramProtocol):
         if done_sysid is not None:
             return ('DONE', done_sysid)
 
-        # MAVLink data
         try:
             for byte in rf_data:
                 msg = self.mav_decoder.parse_char(bytes([byte]))
@@ -708,8 +639,7 @@ async def setup_bridge(config):
 async def auto_discovery_task(serial_protocols):
     """
     程式啟動前 AUTO_DISCOVERY_DURATION_SEC 秒自動送 DISC。
-    時間到後停止自動 DISC，避免 UAV 一直回 HELO。
+    本版 DISC 是強制 discovery，UAV 收到後會重新回 HELO。
-    後續新增 UAV 請用 GUI 的 Scan UAV 按鈕送 DSCF。
     """
     if not AUTO_DISCOVERY_ENABLED:
         return
@@ -720,24 +650,23 @@ async def auto_discovery_task(serial_protocols):
     while time.time() - start_t < AUTO_DISCOVERY_DURATION_SEC:
         for sp in serial_protocols:
             if hasattr(sp, 'send_discovery'):
-                sp.send_discovery(force=False)
+                sp.send_discovery()
         await asyncio.sleep(DISCOVERY_INTERVAL_SEC)
 
     print("[DISCOVERY] Auto DISC stopped. Use GUI Scan UAV for new aircraft.")
 
 
-async def force_discovery_burst(serial_protocols):
+async def discovery_burst(serial_protocols):
     """
-    GUI Scan UAV：送 DSCF burst。
+    GUI Scan UAV：送 DISC burst。
-    UAV 收到 DSCF 後會強制回 HELO。
     """
-    print("[DISCOVERY] Force scan started")
+    print("[DISCOVERY] Manual DISC scan started")
-    for _ in range(FORCE_DISCOVERY_BURST_COUNT):
+    for _ in range(DISCOVERY_BURST_COUNT):
         for sp in serial_protocols:
             if hasattr(sp, 'send_discovery'):
-                sp.send_discovery(force=True)
+                sp.send_discovery()
-        await asyncio.sleep(FORCE_DISCOVERY_BURST_GAP_SEC)
+        await asyncio.sleep(DISCOVERY_BURST_GAP_SEC)
-    print("[DISCOVERY] Force scan finished")
+    print("[DISCOVERY] Manual DISC scan finished")
 
 
 async def tdma_scheduler(serial_protocols):
@@ -748,19 +677,16 @@ async def tdma_scheduler(serial_protocols):
         tdma_done_events[sysid] = asyncio.Event()
 
     while True:
-        # GCS 下行：每輪小批量送出，不再固定空等 100 ms
         for sp in serial_protocols:
             if hasattr(sp, 'flush_gcs_queue'):
                 sp.flush_gcs_queue()
 
         await asyncio.sleep(current_guard_ms / 1000.0)
 
-        # 若尚未發現任何 UAV，避免空轉太快
         if not ACTIVE_SYSIDS:
             await asyncio.sleep(0.2)
             continue
 
-        # UAV 上行：以 byte quota 授權，不以固定 time slot 等待
         for sysid in list(ACTIVE_SYSIDS):
             ensure_active_sysid(sysid)
 
@@ -798,24 +724,22 @@ async def async_main():
 
 def start_gui():
     root = tk.Tk()
-    root.title('UAV Packet-size TDMA Control Station - Auto Discovery / Force Scan')
+    root.title('UAV Packet-size TDMA Control Station - Auto Discovery')
     root.geometry('1350x900')
 
-    # --- 左側控制面板 ---
     control_frame = tk.Frame(root, width=390, bg='#f0f0f0', padx=20, pady=20)
     control_frame.pack(side=tk.LEFT, fill=tk.Y)
 
     tk.Label(control_frame, text='Packet-size TDMA 控制', font=('Arial', 14, 'bold'), bg='#f0f0f0').pack(pady=10)
 
-    # --- Discovery buttons ---
     def on_scan_uav():
         if ASYNC_LOOP is not None and SERIAL_PROTOCOLS:
-            asyncio.run_coroutine_threadsafe(force_discovery_burst(SERIAL_PROTOCOLS), ASYNC_LOOP)
+            asyncio.run_coroutine_threadsafe(discovery_burst(SERIAL_PROTOCOLS), ASYNC_LOOP)
-            scan_label.config(text='已送出 DSCF 強制掃描')
+            scan_label.config(text='已送出 DISC 掃描')
         else:
             scan_label.config(text='尚未建立 serial / asyncio loop')
 
-    scan_btn = tk.Button(control_frame, text='Scan UAV / 發送 DSCF',
+    scan_btn = tk.Button(control_frame, text='Scan UAV / 發送 DISC',
                          font=('Arial', 12, 'bold'), bg='#2196F3', fg='white',
                          command=on_scan_uav)
     scan_btn.pack(pady=(5, 5), fill=tk.X)
@@ -824,7 +748,6 @@ def start_gui():
                           bg='#f0f0f0', font=('Arial', 9), wraplength=330, justify=tk.LEFT)
     scan_label.pack(pady=(0, 15), anchor=tk.W)
 
-    # --- grant bytes slider ---
     grant_var = tk.IntVar(value=current_grant_bytes)
 
     def on_grant_change(val):
@@ -838,7 +761,6 @@ def start_gui():
     grant_label = tk.Label(control_frame, text=f'每台授權: {current_grant_bytes} bytes', bg='#f0f0f0')
     grant_label.pack()
 
-    # --- guard time slider ---
     guard_var = tk.IntVar(value=current_guard_ms)
 
     def on_guard_change(val):
@@ -853,7 +775,6 @@ def start_gui():
     guard_label = tk.Label(control_frame, text=f'切換保護時間: {current_guard_ms} ms', bg='#f0f0f0')
     guard_label.pack()
 
-    # --- UAV INIT mode ---
     tk.Label(control_frame, text='群機初始化控制 (INIT)', font=('Arial', 14, 'bold'), bg='#f0f0f0').pack(pady=(30, 10))
 
     init_var = tk.IntVar(value=0)
@@ -921,7 +842,6 @@ def start_gui():
                          command=toggle_tdma_mode)
     lock_btn.pack(pady=20, fill=tk.X)
 
-    # --- Reports ---
     report_title = tk.Label(control_frame, text='最近 HELO / DONE 回報', font=('Arial', 12, 'bold'), bg='#f0f0f0')
     report_title.pack(pady=(10, 5))
 
@@ -966,7 +886,6 @@ def start_gui():
     for sid in ACTIVE_SYSIDS:
         ensure_rssi_row(sid)
 
-    # --- status updater ---
     def update_status_gui():
         now = time.time()
 
@@ -1011,7 +930,6 @@ def start_gui():
 
     update_status_gui()
 
-    # --- 右側圖表區 ---
     plot_frame = tk.Frame(root)
     plot_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True)