學長建議修改內容

.gitignore

@@ -32,8 +32,3 @@ CLAUDE.md
 my_env_notes.md
 notes/
 **/eeprom.bin
-
-# MAVProxy / SITL 執行產生的 log 與參數 dump
-*.tlog
-*.tlog.raw
-mav.parm

README.md

@@ -2,15 +2,9 @@
 
 ===
 ## 功能簡介
-1. mavlink 多對多串流與交換
-2. 支援 xbee 與 telemetry
-3. 支援 udp 管理
-4. ROS2 介面監控載具
-
-===
-使用限制
-1. 不允許進到 ros 系統有相同 sysid
-2. 假設一台載具上所有 component 共用同一 socket
+1. mavlink 多對多支援平台
+2. 不允許進到 ros 系統有相同 sysid
+3. 假設一台載具上所有 component 共用同一 socket
 
 ===
 ## 運行環境
@@ -67,8 +61,6 @@ cd ~/AirTrapMine/src/ # 這是範例!!!
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 python -m fc_network_adapter.tests.demo_integration
 python -m someotherpkg.src.example_wholeMoving
-
-python fc_network_apps/vehicleDiagShow.py
 ```
 
 2. 
@@ -81,29 +73,25 @@ python gui.py
 
 ===
 ## 資料夾說明
-1. fc_network_adapter (核心)
-    建立、維護與飛控韌體的連接 
-    構築 mavlink 封包
-    處理無線模組的通訊格式 (XBee)
-2. fc_interfaces (必要)
-    自定義的 ROS2 介面檔 沒有這個核心會運作不了
-3. fc_network_module (重要)
-    非核心 但是支援載具的重要附屬功能 
-    需要該功能時 作為一個 ros2 節點打開
-    例如 : ntrip rtk 訊號轉接
-4. fc_network_apps
-    是 fc_network_adapter 高階包裝API
-    利於開發GUI或其他應用 使用者的外層包裝
-    可以不使用 或者當作一個範例程式來看
-5. someotherpkg
-    如何使用 fc_network_apps 的範例檔案
-6. GUI 
-    由 PyQt6 開發的互動式介面
-7. unitdev 為各自協作者做開發時的測試區
+1. unitdev 為各自協作者做開發時的測試區
     01 -> 晉凱(ken)
     02 -> 其宇(chiyu)
     03 -> 文鈞
     04 -> 倫渝
-8. logs 是執行時期的記錄檔
+2. fc_network_adapter (核心)
+    建立、維護與飛控韌體的連接 
+    構築 mavlink 封包
+    處理無線模組的通訊格式 (XBee)
+    --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
+3. fc_interfaces (重要)
+    自定義的 ROS2 介面檔
+4. fc_network_apps
+    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
+    使用者的外層包裝
+5. someotherpkg
+    如何使用 fc_network_apps 的範例檔案
+6. GUI 
+    由 PyQt6 開發的互動式介面
+N. logs 是執行時期的記錄檔
 
 ===

serial_udp_update.py

@@ -0,0 +1,193 @@
+import asyncio
+import serial_asyncio
+import struct
+import os
+import sys
+import serial
+import signal
+import traceback
+from pymavlink import mavutil
+
+# === 設定區 ===
+SERIAL_PORT = 'COM15'  # 手動指定
+SERIAL_BAUDRATE = 57600
+UDP_REMOTE_IP = '127.0.0.1'
+UDP_REMOTE_PORT = 14550
+DEBUG_MODE = False
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # 廣播
+
+# === 工具函數 ===
+def check_serial_port():
+    try:
+        ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
+        ser.close()
+        return True
+    except serial.SerialException as e:
+        print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
+        return False
+    except Exception as e:
+        print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
+        return False
+
+def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
+    frame_type = 0x10
+    dest_addr16 = b'\xFF\xFE'
+    broadcast_radius = 0x00
+    options = 0x00
+
+    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+    frame += dest_addr64 + dest_addr16
+    frame += struct.pack(">BB", broadcast_radius, options) + data
+    checksum = 0xFF - (sum(frame) & 0xFF)
+    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+# === Serial Protocol 實作 ===
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol):
+        self.udp_protocol = udp_protocol
+        self.buffer = bytearray()
+
+    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"Serial connection established on {SERIAL_PORT}")
+
+    def data_received(self, data):
+        self.buffer.extend(data)
+        while True:
+            if len(self.buffer) < 3:
+                return
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+            if len(self.buffer) < full_length:
+                return
+            frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+            if hasattr(self.udp_protocol, 'send_udp'):
+                self.udp_protocol.send_udp(bytes(frame))
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            pass
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[TX Error] 無法封裝或傳送資料: {e}")
+
+# === UDP Protocol 實作 ===
+class UDPHandler(asyncio.DatagramProtocol):
+    def __init__(self):
+        self.serial_transport = None
+        self.transport = None
+        self.mav_decoder = mavutil.mavlink.MAVLink(None)
+
+    def connection_made(self, transport):
+        self.transport = transport
+        print("UDP transport ready.")
+
+    def set_serial_transport(self, serial_transport):
+        self.serial_transport = serial_transport
+
+    def datagram_received(self, data, addr):
+        if self.serial_transport:
+            self.serial_transport.write_to_serial(data)
+
+    def send_udp(self, data):
+        decoded_data = self.decapsulate_data(data)
+        if decoded_data is None:
+            pass
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def decapsulate_data(self, data):
+        try:
+            if not data or data[0] != 0x7E:
+                return None
+            length = (data[1] << 8) | data[2]
+            if len(data) < length + 4:
+                return None
+            frame_type = data[3]
+            if frame_type == 0x90:
+                rf_data_start = 3 + 12
+                return data[rf_data_start:3 + length]
+            else:
+                return None
+        except Exception as e:
+            print(f"[XBee 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            msg = self.mav_decoder.parse_char(data)
+            if msg:
+                if msg.get_type() == "HEARTBEAT":
+                    pass  # 不輸出任何訊息
+                else:
+                    pass
+        except Exception as e:
+            print(f"[MAVLink Decode Error] {e}")
+
+# === 主流程 ===
+async def main():
+    if not check_serial_port():
+        print("程式終止：串口檢查失敗")
+        return
+
+    loop = asyncio.get_running_loop()
+    if os.name != 'nt':  # Windows 不支援 add_signal_handler
+        for sig in (signal.SIGINT, signal.SIGTERM):
+            loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
+
+    udp_handler = UDPHandler()
+
+    try:
+        udp_transport, _ = await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"無法創建 UDP 端點：{str(e)}")
+        return
+
+    sock = udp_transport.get_extra_info('socket')
+    print(f"UDP listening on {sock.getsockname()}")
+
+    try:
+        serial_proto = SerialToUDP(udp_handler)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"無法建立串口連接：{str(e)}")
+        traceback.print_exc()
+        udp_transport.close()
+        return
+
+    print("等待串口資料...")
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+async def shutdown(loop):
+    print("Shutting down...")
+    tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
+    for task in tasks:
+        task.cancel()
+    await asyncio.gather(*tasks, return_exceptions=True)
+    loop.stop()
+
+if __name__ == '__main__':
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print("程式被使用者中斷")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()

src/GUI/comm_panel.py

@@ -1,78 +1,10 @@
 #!/usr/bin/env python3
-from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
-                            QPushButton, QLineEdit, QComboBox, QApplication)
-from PyQt6.QtGui import QFont
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+                            QPushButton, QLineEdit, QComboBox)
 from PyQt6.QtCore import pyqtSignal
 import glob
 import os
 
-
-def _get_font_scale():
-    app = QApplication.instance()
-    if app is None:
-        return 1.0
-    scale = app.property("font_scale")
-    try:
-        return float(scale) if scale is not None else 1.0
-    except (TypeError, ValueError):
-        return 1.0
-
-
-def _scale_stylesheet_font_sizes(stylesheet, scale):
-    if not stylesheet or 'font-size' not in stylesheet:
-        return stylesheet
-
-    import re
-
-    def repl(match):
-        size = float(match.group(1))
-        unit = match.group(2)
-        scaled = max(1.0, size * scale)
-        text = f"{scaled:.2f}".rstrip('0').rstrip('.')
-        return f"font-size: {text}{unit}"
-
-    return re.sub(r'font-size\s*:\s*([0-9]+(?:\.[0-9]+)?)\s*(px|pt)', repl, stylesheet)
-
-
-def _set_scaled_stylesheet(widget, stylesheet):
-    widget._base_stylesheet = stylesheet
-    scaled = _scale_stylesheet_font_sizes(stylesheet, _get_font_scale())
-    widget._applied_stylesheet = scaled
-    widget.setStyleSheet(scaled)
-
-
-def _reapply_scaled_stylesheet(widget):
-    current_stylesheet = widget.styleSheet()
-    base_stylesheet = getattr(widget, '_base_stylesheet', None)
-    applied_stylesheet = getattr(widget, '_applied_stylesheet', None)
-
-    if current_stylesheet != applied_stylesheet:
-        base_stylesheet = current_stylesheet
-        widget._base_stylesheet = base_stylesheet
-
-    if base_stylesheet is not None:
-        scaled = _scale_stylesheet_font_sizes(base_stylesheet, _get_font_scale())
-        widget._applied_stylesheet = scaled
-        if current_stylesheet != scaled:
-            widget.setStyleSheet(scaled)
-
-
-def _apply_scaled_font(widget):
-    base_font = getattr(widget, '_base_font_for_scale', None)
-    if base_font is None:
-        app = QApplication.instance()
-        app_base_font = app.property("base_app_font") if app else None
-        base_font = QFont(app_base_font) if app_base_font is not None else QFont(widget.font())
-        widget._base_font_for_scale = QFont(base_font)
-
-    scaled_font = QFont(base_font)
-    scale = _get_font_scale()
-    if base_font.pointSizeF() > 0:
-        scaled_font.setPointSizeF(max(1.0, base_font.pointSizeF() * scale))
-    elif base_font.pointSize() > 0:
-        scaled_font.setPointSize(max(1, int(round(base_font.pointSize() * scale))))
-    widget.setFont(scaled_font)
-
 class CommPanel(QWidget):
     """通讯设置面板类"""
     
@@ -94,7 +26,6 @@ class CommPanel(QWidget):
         self.ws_connections = []
         self.serial_connections = []
         self._init_ui()
-        self.apply_font_scale()
     
     def _init_ui(self):
         """初始化UI"""
@@ -104,7 +35,7 @@ class CommPanel(QWidget):
         
         # ========== UDP MAVLink 區域 ==========
         udp_title = QLabel("UDP")
-        _set_scaled_stylesheet(udp_title, """
+        udp_title.setStyleSheet("""
             color: #DDD;
             font-size: 14px;
             font-weight: bold;
@@ -129,7 +60,7 @@ class CommPanel(QWidget):
         self.udp_ip_input = QLineEdit()
         self.udp_ip_input.setText("127.0.0.1")
         self.udp_ip_input.setPlaceholderText("IP")
-        _set_scaled_stylesheet(self.udp_ip_input, """
+        self.udp_ip_input.setStyleSheet("""
             QLineEdit {
                 background-color: #333;
                 color: #DDD;
@@ -143,7 +74,7 @@ class CommPanel(QWidget):
         self.udp_port_input.setText("14550")
         self.udp_port_input.setPlaceholderText("Port")
         self.udp_port_input.setFixedWidth(80)
-        _set_scaled_stylesheet(self.udp_port_input, """
+        self.udp_port_input.setStyleSheet("""
             QLineEdit {
                 background-color: #333;
                 color: #DDD;
@@ -155,7 +86,7 @@ class CommPanel(QWidget):
         
         add_udp_btn = QPushButton("添加")
         add_udp_btn.clicked.connect(self._handle_add_udp)
-        _set_scaled_stylesheet(add_udp_btn, """
+        add_udp_btn.setStyleSheet("""
             QPushButton {
                 background-color: #4CAF50;
                 color: white;
@@ -167,13 +98,9 @@ class CommPanel(QWidget):
             QPushButton:hover { background-color: #45a049; }
         """)
         
-        ip_label = QLabel("IP:")
-        _set_scaled_stylesheet(ip_label, "color: #DDD;")
-        add_udp_layout.addWidget(ip_label)
+        add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
         add_udp_layout.addWidget(self.udp_ip_input)
-        port_label = QLabel("Port:")
-        _set_scaled_stylesheet(port_label, "color: #DDD;")
-        add_udp_layout.addWidget(port_label)
+        add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
         add_udp_layout.addWidget(self.udp_port_input)
         add_udp_layout.addWidget(add_udp_btn)
         
@@ -186,7 +113,7 @@ class CommPanel(QWidget):
         
         # ========== Serial 區域 ==========
         serial_title = QLabel("Serial")
-        _set_scaled_stylesheet(serial_title, """
+        serial_title.setStyleSheet("""
             color: #DDD;
             font-size: 14px;
             font-weight: bold;
@@ -209,7 +136,7 @@ class CommPanel(QWidget):
         add_serial_layout.setContentsMargins(0, 0, 0, 0)
         
         self.serial_port_combo = QComboBox()
-        _set_scaled_stylesheet(self.serial_port_combo, """
+        self.serial_port_combo.setStyleSheet("""
             QComboBox {
                 background-color: #333;
                 color: #DDD;
@@ -233,7 +160,7 @@ class CommPanel(QWidget):
         refresh_ports_btn.setFixedWidth(35)
         refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
         refresh_ports_btn.setToolTip("重新掃描串口")
-        _set_scaled_stylesheet(refresh_ports_btn, """
+        refresh_ports_btn.setStyleSheet("""
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -247,9 +174,9 @@ class CommPanel(QWidget):
         
         self.serial_baudrate_combo = QComboBox()
         self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
-        self.serial_baudrate_combo.setCurrentText('115200')
+        self.serial_baudrate_combo.setCurrentText('57600')
         self.serial_baudrate_combo.setFixedWidth(100)
-        _set_scaled_stylesheet(self.serial_baudrate_combo, """
+        self.serial_baudrate_combo.setStyleSheet("""
             QComboBox {
                 background-color: #333;
                 color: #DDD;
@@ -270,7 +197,7 @@ class CommPanel(QWidget):
         
         add_serial_btn = QPushButton("添加")
         add_serial_btn.clicked.connect(self._handle_add_serial)
-        _set_scaled_stylesheet(add_serial_btn, """
+        add_serial_btn.setStyleSheet("""
             QPushButton {
                 background-color: #4CAF50;
                 color: white;
@@ -282,14 +209,10 @@ class CommPanel(QWidget):
             QPushButton:hover { background-color: #45a049; }
         """)
         
-        serial_port_label = QLabel("Port:")
-        _set_scaled_stylesheet(serial_port_label, "color: #DDD;")
-        add_serial_layout.addWidget(serial_port_label)
+        add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
         add_serial_layout.addWidget(self.serial_port_combo)
         add_serial_layout.addWidget(refresh_ports_btn)
-        baud_label = QLabel("Baud:")
-        _set_scaled_stylesheet(baud_label, "color: #DDD;")
-        add_serial_layout.addWidget(baud_label)
+        add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;"))
         add_serial_layout.addWidget(self.serial_baudrate_combo)
         add_serial_layout.addWidget(add_serial_btn)
         
@@ -302,7 +225,7 @@ class CommPanel(QWidget):
         
         # ========== WebSocket 區域 ==========
         ws_title = QLabel("WebSocket")
-        _set_scaled_stylesheet(ws_title, """
+        ws_title.setStyleSheet("""
             color: #DDD;
             font-size: 14px;
             font-weight: bold;
@@ -326,7 +249,7 @@ class CommPanel(QWidget):
         
         self.ws_url_input = QLineEdit()
         self.ws_url_input.setPlaceholderText("host")
-        _set_scaled_stylesheet(self.ws_url_input, """
+        self.ws_url_input.setStyleSheet("""
             QLineEdit {
                 background-color: #333;
                 color: #DDD;
@@ -338,7 +261,7 @@ class CommPanel(QWidget):
         
         add_ws_btn = QPushButton("添加")
         add_ws_btn.clicked.connect(self._handle_add_ws)
-        _set_scaled_stylesheet(add_ws_btn, """
+        add_ws_btn.setStyleSheet("""
             QPushButton {
                 background-color: #4CAF50;
                 color: white;
@@ -350,9 +273,7 @@ class CommPanel(QWidget):
             QPushButton:hover { background-color: #45a049; }
         """)
         
-        url_label = QLabel("URL:")
-        _set_scaled_stylesheet(url_label, "color: #DDD;")
-        add_ws_layout.addWidget(url_label)
+        add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
         add_ws_layout.addWidget(self.ws_url_input)
         add_ws_layout.addWidget(add_ws_btn)
         
@@ -516,7 +437,7 @@ class CommPanel(QWidget):
     def create_udp_connection_panel(self, conn):
         """創建 UDP 連接面板"""
         panel = QWidget()
-        _set_scaled_stylesheet(panel, """
+        panel.setStyleSheet("""
             QWidget {
                 background-color: #2A2A2A;
                 border-radius: 6px;
@@ -530,22 +451,22 @@ class CommPanel(QWidget):
         
         # 連接資訊
         info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
-        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
         
         # 狀態指示器
         status_label = QLabel("●")
         if conn.get('enabled', False):
-            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
             status_label.setToolTip("運行中")
         else:
-            _set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
+            status_label.setStyleSheet("color: #888; font-size: 16px;")
             status_label.setToolTip("已停止")
         
         # 控制按鈕
         toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
         toggle_btn.setFixedWidth(60)
         toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label))
-        _set_scaled_stylesheet(toggle_btn, """
+        toggle_btn.setStyleSheet("""
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -560,7 +481,7 @@ class CommPanel(QWidget):
         remove_btn = QPushButton("移除")
         remove_btn.setFixedWidth(60)
         remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
-        _set_scaled_stylesheet(remove_btn, """
+        remove_btn.setStyleSheet("""
             QPushButton {
                 background-color: #d32f2f;
                 color: white;
@@ -588,7 +509,7 @@ class CommPanel(QWidget):
     def create_ws_connection_panel(self, conn):
         """創建 WebSocket 連接面板"""
         panel = QWidget()
-        _set_scaled_stylesheet(panel, """
+        panel.setStyleSheet("""
             QWidget {
                 background-color: #2A2A2A;
                 border-radius: 6px;
@@ -602,22 +523,22 @@ class CommPanel(QWidget):
         
         # 連接資訊
         info_label = QLabel(f"{conn['name']} - {conn['url']}")
-        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
         
         # 狀態指示器
         status_label = QLabel("●")
         if conn.get('enabled', False):
-            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
             status_label.setToolTip("運行中")
         else:
-            _set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
+            status_label.setStyleSheet("color: #888; font-size: 16px;")
             status_label.setToolTip("已停止")
         
         # 控制按鈕
         toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
         toggle_btn.setFixedWidth(60)
         toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label))
-        _set_scaled_stylesheet(toggle_btn, """
+        toggle_btn.setStyleSheet("""
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -632,7 +553,7 @@ class CommPanel(QWidget):
         remove_btn = QPushButton("移除")
         remove_btn.setFixedWidth(60)
         remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
-        _set_scaled_stylesheet(remove_btn, """
+        remove_btn.setStyleSheet("""
             QPushButton {
                 background-color: #d32f2f;
                 color: white;
@@ -684,7 +605,7 @@ class CommPanel(QWidget):
     def create_serial_connection_panel(self, conn):
         """創建 Serial 連接面板"""
         panel = QWidget()
-        _set_scaled_stylesheet(panel, """
+        panel.setStyleSheet("""
             QWidget {
                 background-color: #2A2A2A;
                 border-radius: 6px;
@@ -698,22 +619,22 @@ class CommPanel(QWidget):
         
         # 連接資訊
         info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
-        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
         
         # 狀態指示器
         status_label = QLabel("●")
         if conn.get('enabled', False):
-            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
             status_label.setToolTip("運行中")
         else:
-            _set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
+            status_label.setStyleSheet("color: #888; font-size: 16px;")
             status_label.setToolTip("已停止")
         
         # 控制按鈕
         toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
         toggle_btn.setFixedWidth(60)
         toggle_btn.clicked.connect(lambda: self.serial_connection_toggled.emit(conn, toggle_btn, status_label))
-        _set_scaled_stylesheet(toggle_btn, """
+        toggle_btn.setStyleSheet("""
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -728,7 +649,7 @@ class CommPanel(QWidget):
         remove_btn = QPushButton("移除")
         remove_btn.setFixedWidth(60)
         remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
-        _set_scaled_stylesheet(remove_btn, """
+        remove_btn.setStyleSheet("""
             QPushButton {
                 background-color: #d32f2f;
                 color: white;
@@ -763,12 +684,4 @@ class CommPanel(QWidget):
     def remove_serial_connection_from_list(self, conn):
         """從列表中移除 Serial 連接"""
         if conn in self.serial_connections:
-            self.serial_connections.remove(conn)
-
-    def apply_font_scale(self):
-        """重新套用目前字體倍率到通訊面板。"""
-        _apply_scaled_font(self)
-        _reapply_scaled_stylesheet(self)
-        for child in self.findChildren(QWidget):
-            _apply_scaled_font(child)
-            _reapply_scaled_stylesheet(child)
+            self.serial_connections.remove(conn)

src/GUI/command_sender.py

@@ -16,10 +16,6 @@ from PyQt6.QtCore import QObject, pyqtSignal
 from pymavlink import mavutil
 
 
-def _log(level, message):
-    print(f"[{level}] {message}")
-
-
 class CommandSender(ABC):
     """指令發送抽象介面"""
 
@@ -67,7 +63,7 @@ class MavlinkSender(CommandSender):
         """
         self.connection_string = connection_string
         self.mav = mavutil.mavlink_connection(connection_string)
-        _log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
+        print(f"MavlinkSender 已建立連線: {connection_string}")
 
     def send_position_global(self, drone_id, sysid, lat, lon, alt):
         """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
@@ -90,7 +86,7 @@ class MavlinkSender(CommandSender):
         if self.mav:
             self.mav.close()
             self.mav = None
-            _log("INFO", "MavlinkSender 已關閉")
+            print("MavlinkSender 已關閉")
 
 
 class Ros2CommandSender(QObject):
@@ -106,11 +102,7 @@ class Ros2CommandSender(QObject):
     # (drone_id, sysid, success, message)
     send_result = pyqtSignal(str, int, bool, str)
 
-    # 閉環設計：goto setpoint 由 mission_executor 週期性重送、換點靠 GPS 到達判定，
-    # 所以這裡的 timeout 只影響「echo 驗證要等多久」，不影響指令是否送達飛控
-    # （adapter 是先送 setpoint 再等 echo）。故意設短，讓 adapter 序列化的 service
-    # 在 echo miss 時快速放棄、換下一台，避免多機時排隊塞爆 client poll。
-    DEFAULT_TIMEOUT_SEC = 0.5
+    DEFAULT_TIMEOUT_SEC = 2.0
 
     def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
         super().__init__()
@@ -170,4 +162,4 @@ class Ros2CommandSender(QObject):
             if not task.done():
                 task.cancel()
         self._pending.clear()
-        _log("INFO", "Ros2CommandSender 已關閉")
+        print("Ros2CommandSender 已關閉")

src/GUI/drone_panel.py

@@ -1,77 +1,10 @@
 #!/usr/bin/env python3
-from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox, QApplication)
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
 from PyQt6.QtCore import pyqtSignal
 from PyQt6.QtGui import QPainter, QPen, QColor, QFont, QPolygonF
 from PyQt6.QtCore import QPointF, Qt
 import math
 
-
-def _get_font_scale():
-    app = QApplication.instance()
-    if app is None:
-        return 1.0
-    scale = app.property("font_scale")
-    try:
-        return float(scale) if scale is not None else 1.0
-    except (TypeError, ValueError):
-        return 1.0
-
-
-def _scale_stylesheet_font_sizes(stylesheet, scale):
-    if not stylesheet or 'font-size' not in stylesheet:
-        return stylesheet
-
-    import re
-
-    def repl(match):
-        size = float(match.group(1))
-        unit = match.group(2)
-        scaled = max(1.0, size * scale)
-        text = f"{scaled:.2f}".rstrip('0').rstrip('.')
-        return f"font-size: {text}{unit}"
-
-    return re.sub(r'font-size\s*:\s*([0-9]+(?:\.[0-9]+)?)\s*(px|pt)', repl, stylesheet)
-
-
-def _set_scaled_stylesheet(widget, stylesheet):
-    widget._base_stylesheet = stylesheet
-    scaled = _scale_stylesheet_font_sizes(stylesheet, _get_font_scale())
-    widget._applied_stylesheet = scaled
-    widget.setStyleSheet(scaled)
-
-
-def _reapply_scaled_stylesheet(widget):
-    current_stylesheet = widget.styleSheet()
-    base_stylesheet = getattr(widget, '_base_stylesheet', None)
-    applied_stylesheet = getattr(widget, '_applied_stylesheet', None)
-
-    if current_stylesheet != applied_stylesheet:
-        base_stylesheet = current_stylesheet
-        widget._base_stylesheet = base_stylesheet
-
-    if base_stylesheet is not None:
-        scaled = _scale_stylesheet_font_sizes(base_stylesheet, _get_font_scale())
-        widget._applied_stylesheet = scaled
-        if current_stylesheet != scaled:
-            widget.setStyleSheet(scaled)
-
-
-def _apply_scaled_font(widget):
-    base_font = getattr(widget, '_base_font_for_scale', None)
-    if base_font is None:
-        app = QApplication.instance()
-        app_base_font = app.property("base_app_font") if app else None
-        base_font = QFont(app_base_font) if app_base_font is not None else QFont(widget.font())
-        widget._base_font_for_scale = QFont(base_font)
-
-    scaled_font = QFont(base_font)
-    scale = _get_font_scale()
-    if base_font.pointSizeF() > 0:
-        scaled_font.setPointSizeF(max(1.0, base_font.pointSizeF() * scale))
-    elif base_font.pointSize() > 0:
-        scaled_font.setPointSize(max(1, int(round(base_font.pointSize() * scale))))
-    widget.setFont(scaled_font)
-
 class DronePanel(QWidget):
     """單個無人機面板類別"""
     
@@ -99,13 +32,12 @@ class DronePanel(QWidget):
         
         self.attitude_indicator = None
         self._init_ui()
-        self.apply_font_scale()
     
     def _init_ui(self):
         """初始化UI"""
         self.setObjectName(f"panel_{self.drone_id}")
-        self.setFixedHeight(160)
-        _set_scaled_stylesheet(self, """
+        self.setFixedHeight(140)
+        self.setStyleSheet("""
             background-color: #2A2A2A;
             border-radius: 8px;
         """)
@@ -117,7 +49,7 @@ class DronePanel(QWidget):
         
         # 創建內容容器（包含 info 和 control）
         content_widget = QWidget()
-        _set_scaled_stylesheet(content_widget, "background-color: #333; border-radius: 6px;")
+        content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
         content_layout = QHBoxLayout(content_widget)
         content_layout.setContentsMargins(8, 8, 8, 8)
         content_layout.setSpacing(8)
@@ -150,7 +82,7 @@ class DronePanel(QWidget):
         # 勾選框
         self.checkbox = QCheckBox()
         self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
-        _set_scaled_stylesheet(self.checkbox, """
+        self.checkbox.setStyleSheet("""
             QCheckBox { 
                 color: #DDD;
             }
@@ -171,8 +103,8 @@ class DronePanel(QWidget):
         )
         
         # ID 顯示
-        self.id_label = QLabel(self.display_id)
-        _set_scaled_stylesheet(self.id_label, """
+        id_label = QLabel(self.display_id)
+        id_label.setStyleSheet("""
             font-weight: bold; 
             font-size: 14px; 
             color: #7FFFD4;
@@ -180,7 +112,7 @@ class DronePanel(QWidget):
         """)
         
         header_layout.addWidget(self.checkbox)
-        header_layout.addWidget(self.id_label)
+        header_layout.addWidget(id_label)
         header_layout.addStretch()
         
         info_layout.addWidget(header)
@@ -196,12 +128,8 @@ class DronePanel(QWidget):
         # 第三行：高度
         altitude_row = self._create_altitude_row()
         info_layout.addWidget(altitude_row)
-
-        # 第四行：GNSS 定位狀態
-        gnss_row = self._create_gnss_row()
-        info_layout.addWidget(gnss_row)
         
-        # 第五行：航向 + 速度
+        # 第四行：航向 + 速度
         nav_row = self._create_nav_row()
         info_layout.addWidget(nav_row)
         
@@ -220,15 +148,15 @@ class DronePanel(QWidget):
         status_layout.setContentsMargins(0, 0, 0, 0)
         
         status_title = QLabel("狀態：")
-        _set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;")
+        status_title.setStyleSheet("color: #888; min-width: 50px;")
         
         self.mode_label = QLabel("--")
         self.mode_label.setObjectName(f"{self.drone_id}_mode")
-        _set_scaled_stylesheet(self.mode_label, "color: #DDD;")
+        self.mode_label.setStyleSheet("color: #DDD;")
         
         self.armed_label = QLabel("--")
         self.armed_label.setObjectName(f"{self.drone_id}_armed")
-        _set_scaled_stylesheet(self.armed_label, "color: #DDD;")
+        self.armed_label.setStyleSheet("color: #DDD;")
         
         status_layout.addWidget(status_title)
         status_layout.addWidget(self.mode_label)
@@ -244,15 +172,15 @@ class DronePanel(QWidget):
         connection_layout.setContentsMargins(0, 0, 0, 0)
         
         connection_title = QLabel("Socket：")
-        _set_scaled_stylesheet(connection_title, "color: #888; min-width: 50px;")
+        connection_title.setStyleSheet("color: #888; min-width: 50px;")
         
         # 根據解析的 drone_id 資訊設定初始值
         self.socket_seq_label = QLabel(self.socket_seq)
         self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq")
-        _set_scaled_stylesheet(self.socket_seq_label, "color: #DDD;")
+        self.socket_seq_label.setStyleSheet("color: #DDD;")
         
         connection_sep = QLabel(" - ")
-        _set_scaled_stylesheet(connection_sep, "color: #DDD;")
+        connection_sep.setStyleSheet("color: #DDD;")
         
         # 設定連接方式顯示
         connection_type_map = {
@@ -265,7 +193,7 @@ class DronePanel(QWidget):
         
         self.connection_type_label = QLabel(connection_type)
         self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
-        _set_scaled_stylesheet(self.connection_type_label, "color: #DDD;")
+        self.connection_type_label.setStyleSheet("color: #DDD;")
         
         connection_layout.addWidget(connection_title)
         connection_layout.addWidget(self.socket_seq_label)
@@ -282,31 +210,29 @@ class DronePanel(QWidget):
         battery_layout.setContentsMargins(0, 0, 0, 0)
         # 顯示百分比
         battery_title = QLabel("電池：")
-        _set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;")
+        battery_title.setStyleSheet("color: #888; min-width: 50px;")
 
         self.battery_pct_label = QLabel("--")
         self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
-        _set_scaled_stylesheet(self.battery_pct_label, "color: #DDD;")
+        self.battery_pct_label.setStyleSheet("color: #DDD;")
 
         # 分隔符
         separator1 = QLabel(" - ")
-        separator1.setObjectName(f"{self.drone_id}_battery_sep1")
-        _set_scaled_stylesheet(separator1, "color: #DDD;")
+        separator1.setStyleSheet("color: #DDD;")
 
         # 顯示電壓
         self.battery_vol_label = QLabel("--")
         self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
-        _set_scaled_stylesheet(self.battery_vol_label, "color: #DDD;")
+        self.battery_vol_label.setStyleSheet("color: #DDD;")
 
         # 分隔符
         separator2 = QLabel(" - ")
-        separator2.setObjectName(f"{self.drone_id}_battery_sep2")
-        _set_scaled_stylesheet(separator2, "color: #DDD;")
+        separator2.setStyleSheet("color: #DDD;")
 
         # 顯示電池節數 (S count)
         self.battery_cells_label = QLabel("--")
         self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
-        _set_scaled_stylesheet(self.battery_cells_label, "color: #DDD;")
+        self.battery_cells_label.setStyleSheet("color: #DDD;")
 
         battery_layout.addWidget(battery_title)
         battery_layout.addWidget(self.battery_pct_label)
@@ -325,18 +251,18 @@ class DronePanel(QWidget):
         altitude_layout.setContentsMargins(0, 0, 0, 0)
         
         altitude_title = QLabel("高度：")
-        _set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;")
+        altitude_title.setStyleSheet("color: #888; min-width: 50px;")
         
         self.altitude_label = QLabel("--")
         self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
-        _set_scaled_stylesheet(self.altitude_label, "color: #DDD;")
+        self.altitude_label.setStyleSheet("color: #DDD;")
         
         speed_title = QLabel("速度：")
-        _set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;")
+        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
         
         self.speed_label = QLabel("--")
         self.speed_label.setObjectName(f"{self.drone_id}_speed")
-        _set_scaled_stylesheet(self.speed_label, "color: #DDD;")
+        self.speed_label.setStyleSheet("color: #DDD;")
         
         altitude_layout.addWidget(altitude_title)
         altitude_layout.addWidget(self.altitude_label)
@@ -345,34 +271,6 @@ class DronePanel(QWidget):
         altitude_layout.addStretch()
         
         return altitude_row
-
-    def _create_gnss_row(self):
-        """創建 GNSS 定位狀態行。"""
-        gnss_row = QWidget()
-        gnss_layout = QHBoxLayout(gnss_row)
-        gnss_layout.setContentsMargins(0, 0, 0, 0)
-
-        fix_title = QLabel("定位：")
-        _set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;")
-
-        self.fix_type_label = QLabel("--")
-        self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label")
-        _set_scaled_stylesheet(self.fix_type_label, "color: #DDD;")
-
-        error_title = QLabel("誤差(H/V)：")
-        _set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;")
-
-        self.gnss_error_label = QLabel("--")
-        self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error")
-        _set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;")
-
-        gnss_layout.addWidget(fix_title)
-        gnss_layout.addWidget(self.fix_type_label)
-        gnss_layout.addWidget(error_title)
-        gnss_layout.addWidget(self.gnss_error_label)
-        gnss_layout.addStretch()
-
-        return gnss_row
     
     def _create_position_row(self):
         """位置行已移除（位置座標不再顯示於面板）。"""
@@ -422,16 +320,6 @@ class DronePanel(QWidget):
     def is_checked(self):
         """獲取勾選狀態"""
         return self.checkbox.isChecked()
-
-    def apply_font_scale(self):
-        """重新套用目前字體倍率到 panel 內所有元件。"""
-        _apply_scaled_font(self)
-        _reapply_scaled_stylesheet(self)
-        for child in self.findChildren(QWidget):
-            _apply_scaled_font(child)
-            _reapply_scaled_stylesheet(child)
-        if self.attitude_indicator:
-            self.attitude_indicator.update()
     
 class SocketGroupPanel(QWidget):
     # 定義信號
@@ -443,12 +331,11 @@ class SocketGroupPanel(QWidget):
         self.color = color
         self.socket_type = socket_type
         self._init_ui()
-        self.apply_font_scale()
     
     def _init_ui(self):
         """初始化UI"""
         self.setObjectName(f"socket_group_{self.socket_id}")
-        _set_scaled_stylesheet(self, """
+        self.setStyleSheet("""
             background-color: #1E1E1E;
             border-radius: 12px;
         """)
@@ -465,7 +352,7 @@ class SocketGroupPanel(QWidget):
         # 分組勾選框
         self.group_checkbox = QCheckBox()
         self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
-        _set_scaled_stylesheet(self.group_checkbox, f"""
+        self.group_checkbox.setStyleSheet(f"""
             QCheckBox {{ color: #DDD; }}
             QCheckBox::indicator {{
                 width: 14px;
@@ -493,7 +380,7 @@ class SocketGroupPanel(QWidget):
         else:
             title_text = f"Socket {self.socket_id}"
         self.title_label = QLabel(title_text)
-        _set_scaled_stylesheet(self.title_label, f"""
+        self.title_label.setStyleSheet(f"""
             font-weight: bold; 
             font-size: 16px; 
             color: {self.color};
@@ -543,14 +430,6 @@ class SocketGroupPanel(QWidget):
         """設置分組勾選狀態（支持半選）"""
         self.group_checkbox.setCheckState(state)
 
-    def apply_font_scale(self):
-        """重新套用目前字體倍率到 socket 分組面板。"""
-        _apply_scaled_font(self)
-        _reapply_scaled_stylesheet(self)
-        for child in self.findChildren(QWidget):
-            _apply_scaled_font(child)
-            _reapply_scaled_stylesheet(child)
-
 
 class AttitudeIndicator(QWidget):
     """
@@ -624,9 +503,7 @@ class AttitudeIndicator(QWidget):
 
         # pitch ladder (every 10°, ±30°)
         p.setPen(QPen(QColor(255, 255, 255, 180), 1))
-        ladder_font = QFont("Arial")
-        ladder_font.setPointSizeF(max(1.0, 6 * _get_font_scale()))
-        p.setFont(ladder_font)
+        p.setFont(QFont("Arial", 6))
         for deg in range(-30, 31, 10):
             if deg == 0:
                 continue
@@ -704,8 +581,6 @@ class AttitudeIndicator(QWidget):
 
         # heading text centred (bigger)
         p.setPen(QPen(QColor("#FFFFFF")))
-        heading_font = QFont("Arial", weight=QFont.Weight.Bold)
-        heading_font.setPointSizeF(max(1.0, 10 * _get_font_scale()))
-        p.setFont(heading_font)
+        p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
         hdg_str = f"{int(self.heading)}°"
-        p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)
+        p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)

src/GUI/map_layout.py

@@ -1,15 +1,8 @@
 #!/usr/bin/env python3
-import json
-
 from PyQt6.QtWebEngineWidgets import QWebEngineView
 from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
 from PyQt6.QtWebChannel import QWebChannel
 
-
-def _log(level, message):
-    print(f"[{level}] {message}")
-
-
 class DroneMap:
     """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
     
@@ -18,9 +11,6 @@ class DroneMap:
         self.map_view = QWebEngineView()
         self.map_loaded = False
         self.pending_map_updates = {}
-        self.font_scale = 1.0
-        self.socket_colors = {}
-        self.drone_group_colors = {}
         
         # 創建橋接對象
         self.bridge = MapBridge()
@@ -41,7 +31,6 @@ class DroneMap:
             <script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
             <script src="qrc:///qtwebchannel/qwebchannel.js"></script>
             <style>
-                :root { --ui-font-scale: 1; }
                 html, body, #map { height: 100%; margin: 0; }
                 #map {
                     user-select: none;
@@ -76,7 +65,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: calc(13px * var(--ui-font-scale));
+                    font-size: 13px;
                     font-weight: bold;
                     box-shadow: 0 2px 5px rgba(0,0,0,0.2);
                 }
@@ -96,7 +85,7 @@ class DroneMap:
                 }
                 .mission-info-row {
                     margin-bottom: 8px;
-                    font-size: calc(12px * var(--ui-font-scale));
+                    font-size: 12px;
                     color: #333;
                 }
                 .mission-info-label {
@@ -115,7 +104,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: calc(13px * var(--ui-font-scale));
+                    font-size: 13px;
                     font-weight: bold;
                     margin-top: 8px;
                 }
@@ -142,7 +131,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: calc(13px * var(--ui-font-scale));
+                    font-size: 13px;
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -160,7 +149,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: calc(13px * var(--ui-font-scale));
+                    font-size: 13px;
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -178,7 +167,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: calc(13px * var(--ui-font-scale));
+                    font-size: 13px;
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -285,37 +274,38 @@ class DroneMap:
                     }, 100);
                 });
 
-                function createArrowIcon(color, groupColor) {
-                const outlineColor = groupColor || 'transparent';
+                function createArrowIcon(color) {
                 return L.divIcon({
                     className: 'drone-arrow',
                     html: `
                     <div style="
-                        width: 34px; height: 34px;
+                        width: 30px; height: 30px;
                         display: flex;
                         align-items: center;
                         justify-content: center;
                     ">
-                        <svg width="34" height="34" viewBox="0 0 34 34">
-                            <polygon
-                                points="17,3 31,31 17,26 3,31"
-                                fill="${color || '#FF0000'}"
-                                stroke="${outlineColor}"
-                                stroke-width="3"
-                                stroke-linejoin="round"
-                            />
+                        <svg width="30" height="30" viewBox="0 0 30 30">
+                            <polygon points="15,0 30,30  15,25  0,30" fill="${color || '#FF0000'}" />
                         </svg>
                     </div>
                     `,
-                    iconSize: [34, 34],
-                    iconAnchor: [17, 17]
+                    iconSize: [30, 30],
+                    iconAnchor: [15, 15]
                 });
                 }
 
                 function getColorBySocketId(id) {
-                    const match = /^s([^_]+)_/.exec(id || '');
-                    const socketId = match ? match[1] : 'default';
-                    return socketColors[socketId] || socketColors.default || "#E0E0E0";
+                    if (id.startsWith("s0_")) return "#00BFFF";
+                    if (id.startsWith("s1_")) return "#FFD700";
+                    if (id.startsWith("s2_")) return "#FF6969";
+                    if (id.startsWith("s3_")) return "#FF69B4";
+                    if (id.startsWith("s4_")) return "#00FA9A";
+                    if (id.startsWith("s5_")) return "#9370DB";
+                    if (id.startsWith("s6_")) return "#FFA500";
+                    if (id.startsWith("s7_")) return "#20B2AA";
+                    if (id.startsWith("s8_")) return "#7CFC00";
+                    if (id.startsWith("s9_")) return "#FF8C00";
+                    return "#AAAAAA";
                 }
 
                 function createIdIcon(id) {
@@ -332,7 +322,7 @@ class DroneMap:
                     align-items: center;
                     justify-content: center;
                     font-weight: bold;
-                    font-size: calc(12px * var(--ui-font-scale));
+                    font-size: 12px;
                     text-shadow: 1px 1px 2px rgba(255,255,255,0.8), -1px -1px 2px rgba(255,255,255,0.8);
                     ">${sysid}</div>`,
                     iconSize: [16, 16],
@@ -343,11 +333,7 @@ class DroneMap:
                 var markers = {};
                 var idLabels = {};
                 var trajectories = {};
-                const maxTrajectoryPoints = 2000;
-                const trajectoryMinDistanceMeters = 0.05;
                 var trajectoryLines = {};
-                var socketColors = {};
-                var droneGroupColors = {};
                 var focusedId = null;
                 var initialized = false;
                 var trajectoryGroup = L.layerGroup().addTo(map);
@@ -375,54 +361,6 @@ class DroneMap:
                 var drawStartPoint = null;
                 var mapDragStarted = false;
                 var clickStartPos = null;
-                var lastManualMapMoveAt = 0;
-                var autoCenteringMap = false;
-                const autoFollowPanThresholdRatio = 0.25;
-                var lastAutoFollowPanAt = 0;
-
-                function markManualMapMove() {
-                    if (!autoCenteringMap) {
-                        lastManualMapMoveAt = Date.now();
-                    }
-                }
-
-                map.on('dragstart drag dragend zoomstart zoomend', markManualMapMove);
-
-                function isOutsideAutoFollowWindow(latlng) {
-                    if (!latlng) {
-                        return false;
-                    }
-
-                    var size = map.getSize();
-                    if (!size || size.x <= 0 || size.y <= 0) {
-                        return false;
-                    }
-
-                    var centerPoint = map.latLngToContainerPoint(map.getCenter());
-                    var targetPoint = map.latLngToContainerPoint(latlng);
-                    var dx = Math.abs(targetPoint.x - centerPoint.x);
-                    var dy = Math.abs(targetPoint.y - centerPoint.y);
-
-                    return dx >= size.x * autoFollowPanThresholdRatio ||
-                           dy >= size.y * autoFollowPanThresholdRatio;
-                }
-
-                function autoFollowPanTo(latlng) {
-                    if (!isOutsideAutoFollowWindow(latlng)) {
-                        return;
-                    }
-                    if (Date.now() - lastManualMapMoveAt < 1000) {
-                        return;
-                    }
-                    if (Date.now() - lastAutoFollowPanAt < 250) {
-                        return;
-                    }
-
-                    lastAutoFollowPanAt = Date.now();
-                    autoCenteringMap = true;
-                    map.panTo(latlng, {animate: true});
-                    setTimeout(() => { autoCenteringMap = false; }, 300);
-                }
                 
                 // 路徑標記變量 (跟隨模式用)
                 var routePoints = [];
@@ -626,7 +564,7 @@ class DroneMap:
                             'align-items: center;' +
                             'justify-content: center;' +
                             'font-weight: bold;' +
-                            'font-size: calc(11px * var(--ui-font-scale));' +
+                            'font-size: 11px;' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">' + idx + '</div>',
@@ -679,10 +617,6 @@ class DroneMap:
                     }
                 }
                 // ================================================================================
-
-                function setFontScale(scale) {
-                    document.documentElement.style.setProperty('--ui-font-scale', scale);
-                }
                 
                 // 開始任務
                 function startMission() {
@@ -747,7 +681,7 @@ class DroneMap:
                             color: color,
                             weight: 3,
                             opacity: 0.7,
-                            smoothFactor: 0.1
+                            smoothFactor: 1
                         }).addTo(trajectoryGroup);
                     }
                 }
@@ -757,27 +691,24 @@ class DroneMap:
                     const point = [lat, lon];
                     trajectories[id].push(point);
                     
-                    if (trajectories[id].length > maxTrajectoryPoints) {
+                    if (trajectories[id].length > 1000) {
                         trajectories[id].shift();
-                        trajectoryLines[id].setLatLngs(trajectories[id]);
-                    } else {
-                        trajectoryLines[id].addLatLng(point);
                     }
+                    
+                    trajectoryLines[id].setLatLngs([...trajectories[id]]);
                 }
 
                 function focusOn(id) {
                     if (!markers[id]) return;
                     focusedId = id;
                     var latlng = markers[id].getLatLng();
-                    autoCenteringMap = true;
                     map.flyTo(latlng, map.getZoom());
-                    setTimeout(() => { autoCenteringMap = false; }, 500);
                 }
 
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
                         var latlng = markers[focusedId].getLatLng();
-                        autoFollowPanTo(latlng);
+                        map.panTo(latlng);
                     }
                 }, 1000);
 
@@ -785,7 +716,7 @@ class DroneMap:
                     if (markers[id]) {
                         const lastPos = markers[id].getLatLng();
                         const distance = lastPos.distanceTo([lat, lon]);
-                        if (distance >= trajectoryMinDistanceMeters) {
+                        if (distance > 1) {
                             addTrajectoryPoint(id, lat, lon);
                         }
                         
@@ -793,16 +724,13 @@ class DroneMap:
                             .setLatLng([lat, lon])
                             .setRotationAngle(heading);
                         idLabels[id].setLatLng([lat, lon]);
-                        if (id === focusedId) {
-                            autoFollowPanTo(markers[id].getLatLng());
-                        }
                     } else {
                         initTrajectory(id);
                         addTrajectoryPoint(id, lat, lon);
                         
                         const color = getColorBySocketId(id);
                         markers[id] = L.marker([lat, lon], {
-                            icon: createArrowIcon(color, droneGroupColors[id]),
+                            icon: createArrowIcon(color),
                             rotationAngle: heading,
                             rotationOrigin: 'center'
                         })
@@ -840,33 +768,6 @@ class DroneMap:
                     }
                 }
 
-                function setDroneGroupColors(groupColors) {
-                    droneGroupColors = groupColors || {};
-                    Object.keys(markers).forEach(id => {
-                        markers[id].setIcon(
-                            createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
-                        );
-                        if (typeof markers[id].setRotationAngle === 'function') {
-                            markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
-                        }
-                    });
-                }
-
-                function setSocketColors(colors) {
-                    socketColors = colors || {};
-                    Object.keys(markers).forEach(id => {
-                        markers[id].setIcon(
-                            createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
-                        );
-                        if (typeof markers[id].setRotationAngle === 'function') {
-                            markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
-                        }
-                    });
-                    Object.keys(trajectoryLines).forEach(id => {
-                        trajectoryLines[id].setStyle({ color: getColorBySocketId(id) });
-                    });
-                }
-
                 function clearAllTrajectories() {
                     trajectories = {};
                     Object.values(trajectoryLines).forEach(line => {
@@ -901,7 +802,7 @@ class DroneMap:
                             'width: 22px; height: 22px;' +
                             'border-radius: 50%;' +
                             'display: flex; align-items: center; justify-content: center;' +
-                            'font-weight: bold; font-size: calc(10px * var(--ui-font-scale));' +
+                            'font-weight: bold; font-size: 10px;' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">' + groupId + '</div>',
@@ -921,7 +822,7 @@ class DroneMap:
                             'width: 22px; height: 22px;' +
                             'border-radius: 50%;' +
                             'display: flex; align-items: center; justify-content: center;' +
-                            'font-weight: bold; font-size: calc(14px * var(--ui-font-scale));' +
+                            'font-weight: bold; font-size: 14px;' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">★</div>',
@@ -966,17 +867,14 @@ class DroneMap:
         # 設置地圖更新計時器
         self.map_update_timer = QTimer()
         self.map_update_timer.timeout.connect(self.update_map_positions)
-        self.map_update_timer.start(100)  # 每 100ms 更新一次（10Hz）
+        self.map_update_timer.start(200)  # 每 200ms 更新一次
     
     def _on_map_loaded(self, ok: bool):
         """地圖加載完成回調"""
         if ok:
             self.map_loaded = True
-            self.set_font_scale(self.font_scale)
-            self.set_socket_colors(self.socket_colors)
-            self.set_drone_group_colors(self.drone_group_colors)
         else:
-            _log("ERROR", "地圖載入失敗")
+            print("⚠️ 地圖加載失敗")
     
     def update_drone_position(self, drone_id, lat, lon, heading):
         """更新無人機位置（加入待處理隊列）"""
@@ -989,7 +887,7 @@ class DroneMap:
         
         js_commands = []
         for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
-            js_commands.append(f"updateDrone({lat:.8f}, {lon:.8f}, '{drone_id}', {heading:.1f});")
+            js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
         
         if js_commands:
             combined_js = "\n".join(js_commands)
@@ -1001,31 +899,11 @@ class DroneMap:
         """清除所有軌跡"""
         if self.map_loaded:
             self.map_view.page().runJavaScript("clearAllTrajectories();")
-
-    def set_drone_group_colors(self, drone_group_colors):
-        """設定每台無人機的 group 外框顏色；未分組者不傳入即為透明。"""
-        self.drone_group_colors = dict(drone_group_colors or {})
-        if self.map_loaded:
-            payload = json.dumps(self.drone_group_colors)
-            self.map_view.page().runJavaScript(f"setDroneGroupColors({payload});")
-
-    def set_socket_colors(self, socket_colors):
-        """設定 socket 顏色，供地圖箭頭本體與軌跡線使用。"""
-        self.socket_colors = dict(socket_colors or {})
-        if self.map_loaded:
-            payload = json.dumps(self.socket_colors)
-            self.map_view.page().runJavaScript(f"setSocketColors({payload});")
     
     def focus_on_drone(self, drone_id):
         """聚焦到指定無人機"""
         if self.map_loaded:
             self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
-
-    def set_font_scale(self, scale):
-        """設定地圖 HTML 控制項的字體倍率。"""
-        self.font_scale = scale
-        if self.map_loaded:
-            self.map_view.page().runJavaScript(f"setFontScale({scale:.3f});")
     
     # ================================================================================
     # 任務規劃視覺化方法
@@ -1046,24 +924,20 @@ class DroneMap:
                 f"{target_lat:.6f}, {target_lon:.6f});"
             )
             self.map_view.page().runJavaScript(js_code)
-            _log(
-                "INFO",
-                f"地圖已繪製 Group {group_id} 任務規劃: "
-                f"C({center_lat:.6f}, {center_lon:.6f}) -> "
-                f"T({target_lat:.6f}, {target_lon:.6f})",
-            )
+            print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
+                  f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
 
     def clear_mission_plan(self):
         """清除地圖上所有任務規劃標記"""
         if self.map_loaded:
             self.map_view.page().runJavaScript("clearAllMissionPlans();")
-            _log("INFO", "地圖已清除所有任務規劃")
+            print("🗑️ 地圖已清除所有任務規劃")
 
     def clear_mission_plan_for_group(self, group_id):
         """清除指定群組的任務規劃標記"""
         if self.map_loaded:
             self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
-            _log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
+            print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
 
     def set_mission_mode(self, mode):
         """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
@@ -1159,56 +1033,52 @@ class MapBridge(QObject):
     def clearAllDroneSelection(self):
         """供 JavaScript 調用的方法 - 清除所有無人機選擇"""
         self.clear_all_drone_selection.emit()
-        _log("INFO", "已清除所有無人機選擇")
+        print("🗑️ 清除所有無人機選擇")
     
     @pyqtSlot()
     def toggleSelectAllDrones(self):
         """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
         self.select_all_drones.emit()
-        _log("INFO", "已切換全選無人機")
+        print("🔄 切換全選無人機")
     
     @pyqtSlot(float, float, float, float)
     def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
         """供 JavaScript 調用的方法 - 開始任務"""
         self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon)
-        _log(
-            "INFO",
-            f"已發出開始任務信號: "
-            f"C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})",
-        )
+        print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
     
     @pyqtSlot()
     def pauseMissionSignal(self):
         """供 JavaScript 調用的方法 - 暫停任務"""
         self.pause_mission_signal.emit()
-        _log("INFO", "已發出暫停任務信號")
+        print("⏸️ 暫停任務信號已發出")
     
     @pyqtSlot(str)
     def rectangleSelected(self, points_json):
         """供 JavaScript 調用的方法 - 矩形選擇完成"""
         self.rectangle_selected.emit(points_json)
-        _log("INFO", f"矩形區域已選擇: {points_json}")
+        print(f"📦 矩形區域已選擇: {points_json}")
     
     @pyqtSlot(str)
     def polygonSelected(self, points_json):
         """供 JavaScript 調用的方法 - 多邊形選擇完成"""
         self.polygon_selected.emit(points_json)
-        _log("INFO", f"多邊形區域已選擇: {points_json}")
+        print(f"🔷 多邊形區域已選擇: {points_json}")
     
     @pyqtSlot(str)
     def missionModeChanged(self, mode):
         """供 JavaScript 調用的方法 - 任務模式切換"""
         self.mission_mode_changed.emit(mode)
-        _log("INFO", f"任務模式已切換: {mode}")
+        print(f"🔄 任務模式切換: {mode}")
     
     @pyqtSlot(str)
     def routeConfirmed(self, points_json):
         """供 JavaScript 調用的方法 - 路徑確認"""
         self.route_confirmed.emit(points_json)
-        _log("INFO", f"路徑已確認: {points_json}")
+        print(f"📍 路徑已確認: {points_json}")
 
     @pyqtSlot(str)
     def droneBoxSelected(self, drone_ids_json):
         """供 JavaScript 調用的方法 - 框選無人機完成"""
         self.drone_box_selected.emit(drone_ids_json)
-        _log("INFO", f"框選無人機完成: {drone_ids_json}")
+        print(f"📦 框選無人機: {drone_ids_json}")

src/GUI/mission_executor.py

@@ -3,19 +3,14 @@
 任務執行模組
 管理多架無人機的 GUIDED 模式飛行控制迴圈
 
-設計 (閉環 / closed-loop):
+設計:
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等），
-      掉包會在下個週期自癒；換點完全靠 GPS 到達判定驅動，不依賴通訊 ACK
-    - send_result（echo 驗證）僅作觀測/UI 顯示，不影響飛行控制：
-      驗證失敗不會 fallback LOITER，指令其實已送達飛控，交給 GPS 續飛
+    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
+    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
     - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
     - 用 QTimer 驅動到達判定，在 Qt 主線程執行
     - 暫停 = 停止到達判定 + 停止新指令
-
-背景：adapter 的 pos_global_int service 被序列化 + 阻塞等 msg87 echo，實飛慢速鏈路下
-多機會只有一台在 client timeout 內拿到回應。閉環設計繞過此瓶頸（見專案記憶）。
 """
 import asyncio
 import math
@@ -24,10 +19,6 @@ from enum import Enum
 from PyQt6.QtCore import QObject, QTimer, pyqtSignal
 
 
-def _log(level, message):
-    print(f"[{level}] {message}")
-
-
 class MissionState(Enum):
     """整體任務狀態"""
     IDLE = "idle"
@@ -47,8 +38,7 @@ class DroneTask:
     """單架無人機的任務資料"""
     __slots__ = (
         'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
-        'last_sent_at', 'fail_count', 'status', 'waiting_since',
-        'entered_radius_at', 'last_log_at',
+        'sent_current_wp', 'fail_count', 'status', 'waiting_since',
     )
 
     def __init__(self, drone_id, sysid, waypoints):
@@ -57,13 +47,10 @@ class DroneTask:
         self.waypoints = waypoints
         self.wp_index = 0
         self.done = len(waypoints) == 0
-        # 上次送出當前目標的 monotonic 時間；0.0 = 尚未送過（下個 tick 立即送）
-        self.last_sent_at = 0.0
+        self.sent_current_wp = False
         self.fail_count = 0
         self.status = TaskStatus.NORMAL
         self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
-        self.entered_radius_at = 0.0  # 進入 arrival_radius 的時間，0 = 尚未進入
-        self.last_log_at = 0.0  # 上次印 distance log 的時間
 
     @property
     def current_target(self):
@@ -91,32 +78,21 @@ class MissionExecutor(QObject):
         }
     """
 
-    MAX_RETRY = 3  # 已停用：閉環下 echo 驗證失敗不再累計切 LOITER（保留常數以防外部引用）
+    MAX_RETRY = 3
 
     drone_waypoint_reached = pyqtSignal(str, int, int)   # (drone_id, wp_index, total)
     task_status_changed = pyqtSignal(str, str, str)       # (drone_id, status, message)
     mission_completed = pyqtSignal()
 
     def __init__(self, sender, drone_gps, monitor=None,
-                 arrival_radius=4.0, tick_rate_hz=2.0,
-                 barrier_timeout_sec=20.0,
-                 hover_stable_sec=2.0,
-                 progress_log_interval_sec=3.0,
-                 resend_interval_sec=2.5):
+                 arrival_radius=2.0, tick_rate_hz=2.0,
+                 barrier_timeout_sec=20.0):
         super().__init__()
         self.sender = sender
         self.drone_gps = drone_gps
-        self.monitor = monitor  # 保留：僅供外部手動 fallback 使用，閉環下不自動切 LOITER
+        self.monitor = monitor  # 用於失敗 fallback 到 LOITER
         self.arrival_radius = arrival_radius
-        # 閉環重送週期：每隔這麼久重送一次當前目標 setpoint（掉包自癒）。
-        # 注意：需 > N架 × client_timeout，否則多機序列化會塞不完（見 command_sender）。
-        self.resend_interval_sec = resend_interval_sec
         self.barrier_timeout_sec = barrier_timeout_sec
-        # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
-        # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
-        self.hover_stable_sec = hover_stable_sec
-        self.hysteresis_factor = 1.5
-        self.progress_log_interval_sec = progress_log_interval_sec
         self.state = MissionState.IDLE
         self.tasks = {}
         self.rendezvous_indices = set()
@@ -133,7 +109,7 @@ class MissionExecutor(QObject):
 
     def start(self, planned_waypoints):
         if self.state == MissionState.RUNNING:
-            _log("WARN", "任務已在執行中")
+            print("任務已在執行中")
             return
 
         self.tasks.clear()
@@ -156,35 +132,31 @@ class MissionExecutor(QObject):
             f"rendezvous WP={sorted(self.rendezvous_indices)}"
             if self.rendezvous_indices else "無 rendezvous (各自跑)"
         )
-        _log(
-            "INFO",
-            f"任務已啟動: {len(self.tasks)} 架無人機, "
-            f"共 {total_wps} 個航點, "
-            f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), "
-            f"tick 週期={self._interval_ms}ms, "
-            f"重送週期={self.resend_interval_sec}s (閉環，GPS 驅動換點), "
-            f"barrier timeout={self.barrier_timeout_sec}s, "
-            f"{rv_info}",
-        )
+        print(f"任務啟動: {len(self.tasks)} 架無人機, "
+              f"共 {total_wps} 個航點, "
+              f"到達半徑={self.arrival_radius}m, "
+              f"tick 週期={self._interval_ms}ms, "
+              f"barrier timeout={self.barrier_timeout_sec}s, "
+              f"{rv_info}")
 
     def pause(self):
         if self.state == MissionState.RUNNING:
             self._timer.stop()
             self.state = MissionState.PAUSED
-            _log("INFO", "任務已暫停")
+            print("任務暫停")
 
     def resume(self):
         if self.state == MissionState.PAUSED:
             self._timer.start(self._interval_ms)
             self.state = MissionState.RUNNING
-            _log("INFO", "任務已恢復")
+            print("任務恢復")
 
     def stop(self):
         self._timer.stop()
         self.tasks.clear()
         self.rendezvous_indices = set()
         self.state = MissionState.IDLE
-        _log("INFO", "任務已停止")
+        print("任務停止")
 
     # ------------------------------------------------------------------ 控制迴圈
 
@@ -198,7 +170,7 @@ class MissionExecutor(QObject):
         """
         now = time.monotonic()
 
-        # ---- Phase 1: 到達判定（hover-stable） ----
+        # ---- Phase 1: 到達判定 ----
         for task in self.tasks.values():
             if task.done or task.status == TaskStatus.FALLBACK_LOITER:
                 continue
@@ -216,38 +188,16 @@ class MissionExecutor(QObject):
 
             tgt_lat, tgt_lon, _ = target
             distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
-
-            # hover-stable 判定：
-            #   - 首次進入 radius → 記錄時間
-            #   - 在 radius * hysteresis 內持續 → 保留計時
-            #   - 離開 hysteresis → 重置
-            #   - 在 radius 內穩定 hover_stable_sec 秒 → 視為到達
-            hysteresis_radius = self.arrival_radius * self.hysteresis_factor
-            if distance < self.arrival_radius:
-                if task.entered_radius_at == 0.0:
-                    task.entered_radius_at = now
-                stable_for = now - task.entered_radius_at
-                if stable_for < self.hover_stable_sec:
-                    self._maybe_log_progress(task, now, distance, stable_for)
-                    continue
-            elif distance < hysteresis_radius and task.entered_radius_at > 0.0:
-                # 還在 hysteresis 內，計時不重置（容忍 GPS 抖動跨越邊界）
-                self._maybe_log_progress(task, now, distance, now - task.entered_radius_at)
-                continue
-            else:
-                # 離開 hysteresis：重置計時，繼續等待到達
-                task.entered_radius_at = 0.0
-                self._maybe_log_progress(task, now, distance, 0.0)
+            if distance >= self.arrival_radius:
                 continue
 
             # 到達當前 wp_index
-            task.entered_radius_at = 0.0  # 重置給下個 wp 用
             if (task.wp_index in self.rendezvous_indices
                     and task.wp_index < task.total_waypoints - 1):
                 # rendezvous 點 → 不推進，進入 barrier 等待
                 task.status = TaskStatus.WAITING_AT_BARRIER
                 task.waiting_since = now
-                _log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index}，等待同伴")
+                print(f"  {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
                 self.task_status_changed.emit(
                     task.drone_id, task.status.value,
                     f"waiting at WP {task.wp_index}"
@@ -260,21 +210,19 @@ class MissionExecutor(QObject):
         # ---- Phase 2: barrier 釋放檢查 ----
         self._check_barriers(now)
 
-        # ---- Phase 3: 週期性重送當前目標（閉環）----
-        # 每 resend_interval_sec 重送一次目前 wp 的 setpoint，不靠 send_result 決定前進。
-        # GUIDED 位置目標冪等，重送安全；掉包會在下個週期自癒。first send: last_sent_at=0 → 立即送。
+        # ---- Phase 3: 發送未送過的目標 ----
         for task in self.tasks.values():
             if task.done or task.status in (
                 TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
             ):
                 continue
+            if task.sent_current_wp:
+                continue
             target = task.current_target
             if target is None:
                 continue
-            if now - task.last_sent_at < self.resend_interval_sec:
-                continue
             tgt_lat, tgt_lon, tgt_alt = target
-            task.last_sent_at = now
+            task.sent_current_wp = True
             self.sender.send_position_global(
                 task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
             )
@@ -288,51 +236,26 @@ class MissionExecutor(QObject):
             self._timer.stop()
             self.state = MissionState.IDLE
             self.mission_completed.emit()
-            _log("INFO", "任務全部完成")
-
-    def _maybe_log_progress(self, task, now, distance, stable_for):
-        """週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
-        if now - task.last_log_at < self.progress_log_interval_sec:
-            return
-        task.last_log_at = now
-        if stable_for > 0:
-            stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
-        else:
-            stable_info = ""
-        _log(
-            "INFO",
-            f"{task.drone_id} → WP {task.wp_index}: "
-            f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
-        )
+            print("===== 任務全部完成 =====")
 
     def _advance_waypoint(self, task, arrived_distance):
-        """把 task 推進一個航點，重置發送計時。不處理 barrier 邏輯。"""
+        """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
         task.wp_index += 1
-        task.last_sent_at = 0.0  # 新 wp 下個 tick 立即送
+        task.sent_current_wp = False
         task.fail_count = 0
-        task.entered_radius_at = 0.0
-        task.last_log_at = 0.0
-        if task.status == TaskStatus.RETRYING:
-            task.status = TaskStatus.NORMAL  # 清掉上個 wp 殘留的「送出未驗證」狀態
         if task.wp_index >= task.total_waypoints:
             task.done = True
             self.drone_waypoint_reached.emit(
                 task.drone_id, task.wp_index, task.total_waypoints
             )
-            _log(
-                "INFO",
-                f"{task.drone_id} 已完成所有航點 "
-                f"({task.total_waypoints}/{task.total_waypoints})",
-            )
+            print(f"  {task.drone_id} → DONE "
+                  f"({task.total_waypoints}/{task.total_waypoints})")
             return
         self.drone_waypoint_reached.emit(
             task.drone_id, task.wp_index, task.total_waypoints
         )
-        _log(
-            "INFO",
-            f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
-            f"(到達距離: {arrived_distance:.1f}m)",
-        )
+        print(f"  {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
+              f"(到達距離: {arrived_distance:.1f}m)")
 
     def _check_barriers(self, now):
         """檢查每個有人在等的 barrier 能不能釋放。"""
@@ -366,13 +289,14 @@ class MissionExecutor(QObject):
                 oldest_wait = min(t.waiting_since for t in waiting_tasks)
                 if now - oldest_wait >= self.barrier_timeout_sec:
                     force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
-                    _log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
+                    print(f"⚠️  barrier WP {barrier_idx} {force_reason}，強制放行")
                 else:
                     continue  # 還沒到齊、也還沒 timeout → 繼續等
 
             # 釋放：把所有在此 barrier 等待的機一起推進
             tag = "全員到齊" if force_reason is None else force_reason
-            _log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
+            print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
+                  f"推進 {len(waiting_tasks)} 架")
             for task in waiting_tasks:
                 task.status = TaskStatus.NORMAL
                 msg = f"barrier WP {barrier_idx} released ({tag})"
@@ -384,48 +308,44 @@ class MissionExecutor(QObject):
     # ------------------------------------------------------------------ 結果回呼
 
     def _on_send_result(self, drone_id, sysid, success, message):
-        """
-        Ros2CommandSender.send_result 的 slot（閉環：僅觀測，不影響飛行控制）。
-
-        指令在 adapter 端等 echo 前就已送達飛控，echo 驗證失敗不代表沒送到；
-        因此這裡不再累計失敗切 LOITER，只更新 UI 狀態。真正的前進由 GPS 到達判定驅動，
-        重送由 Phase 3 的週期性重送負責。
-        """
+        """Ros2CommandSender.send_result 的 slot"""
         task = self.tasks.get(drone_id)
         if task is None or task.done:
             return
-        # 若已在 barrier 等待，舊指令的遲到回應不要再干擾狀態
+        # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
         if task.status == TaskStatus.WAITING_AT_BARRIER:
             return
 
         if success:
             if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
                 task.status = TaskStatus.NORMAL
-                self.task_status_changed.emit(drone_id, task.status.value, "send verified")
+                self.task_status_changed.emit(drone_id, task.status.value, "recovered")
             task.fail_count = 0
             return
 
-        # echo 驗證失敗：僅記錄 + UI 提示，不改變飛行（交給 GPS 續飛、下個週期重送）
         task.fail_count += 1
-        task.status = TaskStatus.RETRYING  # 純觀測用途，Phase 1/3 不會因此停飛
-        _log(
-            "WARN",
-            f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}",
-        )
-        self.task_status_changed.emit(
-            drone_id, task.status.value,
-            f"send unverified x{task.fail_count} (GPS 續飛): {message}"
-        )
+        print(f"⚠️  {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
+
+        if task.fail_count < self.MAX_RETRY:
+            task.status = TaskStatus.RETRYING
+            task.sent_current_wp = False  # 下個 tick 會重送
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+            )
+        else:
+            task.status = TaskStatus.FALLBACK_LOITER
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
+            )
+            print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
+            self._fallback_to_loiter(drone_id)
 
     def _fallback_to_loiter(self, drone_id):
-        """
-        用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。
-
-        閉環改版後不再由 _on_send_result 自動呼叫（送指令失敗不代表沒送到）；
-        保留此方法供外部（例如 GUI 手動安全按鈕）需要時使用。
-        """
+        """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
         if self.monitor is None:
-            _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
+            print(f"⚠️  無 monitor，無法將 {drone_id} 切到 LOITER")
             return
         try:
             loop = asyncio.get_event_loop()

src/GUI/mission_group.py

@@ -13,34 +13,16 @@ from mission_executor import MissionExecutor, MissionState
 
 # 群組顏色（循環使用）
 GROUP_COLORS = [
-    "#E53935",  # Deep Red
-    "#FB8C00",  # Deep Orange
-    "#FDD835",  # Deep Yellow
-    "#43A047",  # Deep Green
-    "#1E88E5",  # Deep Blue
-    "#8E24AA",  # Deep Purple
-    "#D81B60",  # Deep Pink
+    '#4A9EFF',  # 藍
+    '#FF8C42',  # 橘
+    '#56C87A',  # 綠
+    '#E85D75',  # 紅
+    '#B07CED',  # 紫
+    '#F5C542',  # 黃
+    '#42C9C9',  # 青
+    '#FF6B9D',  # 粉
 ]
 
-DEFAULT_MISSION_PARAM_VALUES = {
-    'spacing': '5.0',
-    'base_altitude': '10.0',
-    'altitude_diff': '2.0',
-    'radius': '10.0',
-    'altitude': '10.0',
-    'start_angle': '0',
-    'lateral_offset': '3.0',
-    'longitudinal_spacing': '5.0',
-    'line_spacing': '5.0',
-    # 執行參數（所有任務類型共用）
-    'arrival_radius': '4.0',     # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2
-    'hover_stable_sec': '2.0',   # 在 radius 內穩定停留多久才視為到達 (s)
-}
-
-
-# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換）
-EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
-
 
 class MissionGroup:
     """單一任務群組的資料"""
@@ -162,7 +144,6 @@ class GroupPanel(QWidget):
     mode_change_requested = pyqtSignal(str, str)       # group_id, mode
     arm_requested = pyqtSignal(str)                    # group_id
     takeoff_requested = pyqtSignal(str, float)         # group_id, altitude
-    reboot_requested = pyqtSignal(str)                 # group_id
     box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
     select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
     clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
@@ -176,13 +157,10 @@ class GroupPanel(QWidget):
         QPushButton:disabled {{ background-color: #444; color: #666; }}
     """
 
-    def __init__(self, group: MissionGroup, parent=None, default_params=None):
+    def __init__(self, group: MissionGroup, parent=None):
         super().__init__(parent)
         self.group = group
         self.all_btn_ref = None  # 保存全選按鈕的參考
-        self.default_params = dict(DEFAULT_MISSION_PARAM_VALUES)
-        if default_params:
-            self.default_params.update(default_params)
         self._build_ui()
 
     def _make_sep(self):
@@ -377,23 +355,6 @@ class GroupPanel(QWidget):
 
         right.addStretch()
 
-        # ============================
-        # 飛控操作
-        # ============================
-        reboot_col = QVBoxLayout()
-        reboot_col.setSpacing(3)
-
-        reboot_title = QLabel("飛控操作")
-        reboot_title.setStyleSheet(TITLE)
-        reboot_col.addWidget(reboot_title)
-
-        reboot_btn = QPushButton("重啟飛控")
-        reboot_btn.setStyleSheet(BTN.format(bg='#8A3A3A', fg='#FFF', hover='#A84646'))
-        reboot_btn.clicked.connect(
-            lambda: self.reboot_requested.emit(self.group.group_id))
-        reboot_col.addWidget(reboot_btn)
-        reboot_col.addStretch()
-
         # ============================
         # 第四欄：任務參數
         # ============================
@@ -411,32 +372,26 @@ class GroupPanel(QWidget):
         # 每種任務類型的參數定義: (key, label, default_value)
         self._param_defs = {
             'M_FORMATION': [
-                ('spacing', '間距 (m)', self.default_params['spacing']),
-                ('base_altitude', '基準高度 (m)', self.default_params['base_altitude']),
-                ('altitude_diff', '高低差 (m)', self.default_params['altitude_diff']),
+                ('spacing', '間距 (m)', '5.0'),
+                ('base_altitude', '基準高度 (m)', '10.0'),
+                ('altitude_diff', '高低差 (m)', '2.0'),
             ],
             'CIRCLE_FORMATION': [
-                ('radius', '半徑 (m)', self.default_params['radius']),
-                ('altitude', '高度 (m)', self.default_params['altitude']),
-                ('start_angle', '起始角 (°)', self.default_params['start_angle']),
+                ('radius', '半徑 (m)', '10.0'),
+                ('altitude', '高度 (m)', '10.0'),
+                ('start_angle', '起始角 (°)', '0'),
             ],
             'LEADER_FOLLOWER': [
-                ('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
-                ('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
-                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('lateral_offset', '橫向偏移 (m)', '3.0'),
+                ('longitudinal_spacing', '縱向間距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
             ],
             'GRID_SWEEP': [
-                ('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
-                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('line_spacing', '掃描線距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
             ],
         }
 
-        # 所有 mission type 共用的執行參數（不隨類型切換隱藏）
-        self._exec_param_defs = [
-            ('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
-            ('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
-        ]
-
         # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
         self._param_widgets = {}  # key → (label_widget, input_widget)
         self._param_rows = []     # 所有 row layout 對應的 container widget
@@ -460,23 +415,6 @@ class GroupPanel(QWidget):
                 self._param_widgets[key] = (row_w, inp)
                 self._param_rows.append(row_w)
 
-        # 執行參數列（所有 mission type 共用，永遠顯示）
-        for key, label_text, default in self._exec_param_defs:
-            row_w = QWidget()
-            row_l = QHBoxLayout(row_w)
-            row_l.setContentsMargins(0, 0, 0, 0)
-            row_l.setSpacing(3)
-            lbl = QLabel(label_text)
-            lbl.setStyleSheet(LBL)
-            inp = QLineEdit(default)
-            inp.setStyleSheet(INPUT)
-            inp.setFixedWidth(50)
-            row_l.addWidget(lbl, 1)
-            row_l.addWidget(inp)
-            param_col.addWidget(row_w)
-            self._param_widgets[key] = (row_w, inp)
-            self._param_rows.append(row_w)
-
         # LEADER_FOLLOWER 專用：領隊下拉選單
         self._leader_row = QWidget()
         leader_layout = QHBoxLayout(self._leader_row)
@@ -500,7 +438,7 @@ class GroupPanel(QWidget):
         # 當任務類型切換時更新參數顯示
         self.type_combo.currentTextChanged.connect(self._update_param_visibility)
 
-        # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ──
+        # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
         # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
         cols.addLayout(left, 0)
         cols.addWidget(self._make_sep())
@@ -509,9 +447,7 @@ class GroupPanel(QWidget):
         cols.addLayout(param_col, 0)
         cols.addWidget(self._make_sep())
         cols.addLayout(right, 0)
-        cols.addWidget(self._make_sep())
-        cols.addLayout(reboot_col, 0)
-        cols.addStretch()  # 填充剩餘空間，使欄位置左
+        cols.addStretch()  # 填充剩餘空間，使四列置左
 
         layout.addLayout(cols)
 
@@ -576,17 +512,15 @@ class GroupPanel(QWidget):
         self.delete_group_btn.setEnabled(enabled)
 
     def _update_param_visibility(self, _=None):
-        """根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。"""
+        """根據當前任務類型，顯示/隱藏對應的參數列"""
         mission_type = self.type_combo.currentText()
-        mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
-        exec_keys = {d[0] for d in self._exec_param_defs}
-        visible_keys = mission_keys | exec_keys
+        visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
         for key, (row_w, _inp) in self._param_widgets.items():
             row_w.setVisible(key in visible_keys)
         self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
 
     def get_mission_params(self):
-        """讀取當前任務類型的規劃參數，回傳 dict（不含執行參數）"""
+        """讀取當前顯示的參數值，回傳 dict"""
         mission_type = self.type_combo.currentText()
         params = {}
         for key, _label, default in self._param_defs.get(mission_type, []):
@@ -598,25 +532,6 @@ class GroupPanel(QWidget):
                     params[key] = float(default)
         return params
 
-    def get_execution_params(self):
-        """讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict"""
-        params = {}
-        for key, _label, default in self._exec_param_defs:
-            if key in self._param_widgets:
-                _row_w, inp = self._param_widgets[key]
-                try:
-                    params[key] = float(inp.text())
-                except ValueError:
-                    params[key] = float(default)
-        return params
-
-    def set_param_value(self, key, value):
-        """更新指定參數欄位的文字值。"""
-        if key not in self._param_widgets:
-            return
-        _row_w, inp = self._param_widgets[key]
-        inp.setText(str(value))
-
     def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
         """更新中心點 / 目標點顯示"""
         info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
@@ -638,3 +553,4 @@ class GroupPanel(QWidget):
         except ValueError:
             alt = 10.0
         self.takeoff_requested.emit(self.group.group_id, alt)
+

src/GUI/mission_planner.py

@@ -83,11 +83,10 @@ class FormationPlanner:
 
         center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
         center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
+        center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
 
-        # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
-        # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
-        self.current_origin = (center_lat, center_lon, 0.0)
-        self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
+        self.current_origin = (center_lat, center_lon, center_alt)
+        self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
 
         drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
         target_local = self.converter.gps_to_local(*target_gps)
@@ -193,13 +192,11 @@ class FormationPlanner:
             final_y = center_y + radius * math.sin(angle_rad)
             final_z = altitude
 
-            current_x, current_y, _current_z = drone_positions[i]
-            # stage1 高度直接用 final_z：先爬升到目標高度再水平集合，
-            # 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高）
+            current_x, current_y, current_z = drone_positions[i]
             stage1_positions.append((
                 current_x + (final_x - current_x) * 0.5,
                 current_y + (final_y - current_y) * 0.5,
-                final_z,
+                current_z + (final_z - current_z) * 0.5
             ))
             stage2_positions.append((final_x, final_y, final_z))
 
@@ -209,17 +206,29 @@ class FormationPlanner:
 
     def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
         """
-        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化
+        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
 
-        center_path = 使用者畫的路徑點（直線段連接，不插弧），
-        每架 drone 沿弧長 s 參數化做隊形偏移：
-            s_follower = s_leader - lon_k
-            position = lookup(s_follower) + lat_k × perp(tangent)
-        s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
+        核心想法：
+            - center_path 建好後計算每個 sample 的累積弧長 s
+            - 每架 drone 給 (lat_k, lon_k) 的隊形偏移
+            - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
+            - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
+            - 超過 path 起點/終點以 clip 處理，避免倒退或衝出
 
-        Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
-        隊形 (俯視):
-            D0 (lat=-L, lon=0)   ← leader
+        解決前版「空間偏移」的三個 bug：
+            1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
+            2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
+            3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
+
+        Rank 定序規則：
+            **以 drone_positions 的輸入順序為準**（= GUI 選取順序）。
+            drone_positions[0] = rank 0 = leader (lon=0)，
+            drone_positions[1] = rank 1 (lon=5)，依此類推。
+            刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run
+            leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。
+
+        隊形 (俯視, 以路徑行進方向為前):
+            D0 (lat=-L, lon=0)   ← front-right (leader)
                 D1 (lat=+L, lon=5)
             D2 (lat=-L, lon=10)
                 D3 (lat=+L, lon=15)
@@ -228,16 +237,29 @@ class FormationPlanner:
         lateral_offset = params.get('lateral_offset', 3.0)
         longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
         altitude = params.get('altitude', self.base_altitude)
+        min_inner_radius = params.get('min_inner_radius', 3.0)
+        arc_resolution = params.get('arc_resolution', 12)
+        sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0)  # cos(90°)=0
 
-        # Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
-        center_path, barrier_indices = self._build_center_path(route_wps_local)
+        # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
+        center_path, barrier_indices = self._build_center_path(
+            route_wps_local,
+            formation_max_lateral=lateral_offset,
+            min_inner_radius=min_inner_radius,
+            arc_resolution=arc_resolution,
+            sharp_turn_cos_threshold=sharp_turn_cos,
+        )
 
         s_list = [pt['s'] for pt in center_path]
         s_max = s_list[-1]
         n_pts = len(center_path)
 
         def lookup(s_target):
-            """在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。"""
+            """
+            在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
+            s<0 → clip 到 start；s>s_max → clip 到 end。
+            tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
+            """
             if n_pts == 1:
                 pt = center_path[0]
                 return pt['x'], pt['y'], pt['dir']
@@ -248,8 +270,7 @@ class FormationPlanner:
                 sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
                 slen = math.hypot(sdx, sdy)
                 if slen > 1e-6:
-                    dx, dy = sdx / slen, sdy / slen
-                    return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
+                    return a['x'], a['y'], (sdx / slen, sdy / slen)
                 return a['x'], a['y'], a['dir']
 
             if s_target >= s_max:
@@ -258,9 +279,7 @@ class FormationPlanner:
                 sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
                 slen = math.hypot(sdx, sdy)
                 if slen > 1e-6:
-                    dx, dy = sdx / slen, sdy / slen
-                    overshoot = s_target - s_max
-                    return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
+                    return b['x'], b['y'], (sdx / slen, sdy / slen)
                 return b['x'], b['y'], b['dir']
 
             # Binary search：s_list[lo] <= s_target < s_list[hi]
@@ -289,6 +308,16 @@ class FormationPlanner:
         # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
         all_waypoints = [None] * N
 
+        # 預先算好路徑終點的切線（給收尾點用）
+        end = center_path[-1]
+        end_dx, end_dy = end['dir']
+        if n_pts >= 2:
+            a = center_path[-2]
+            sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
+            slen = math.hypot(sdx, sdy)
+            if slen > 1e-6:
+                end_dx, end_dy = sdx / slen, sdy / slen
+
         for rank in range(N):
             lat_sign = -1 if rank % 2 == 0 else 1
             lat = lat_sign * lateral_offset
@@ -305,16 +334,46 @@ class FormationPlanner:
                     altitude,
                 ))
 
+            # 收尾：全員到 path 終點的 rank 位置（lon 歸零）
+            waypoints.append((
+                end['x'] + lat * (-end_dy),
+                end['y'] + lat * end_dx,
+                altitude,
+            ))
+
             all_waypoints[rank] = waypoints
 
+        # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
+        # 不觸及末尾收尾點，直接沿用即可
         return all_waypoints, barrier_indices
 
-    def _build_center_path(self, waypoints):
+    def _build_center_path(self, waypoints,
+                           formation_max_lateral,
+                           min_inner_radius,
+                           arc_resolution,
+                           sharp_turn_cos_threshold):
         """
-        建立中心路徑 — 直接用路徑點連接，不插弧。
+        建立以「圓弧」連接直線段的中心路徑。
 
-        每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
-        飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。
+        每個中間 waypoint 的處理：
+            1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
+            2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
+               barrier 放在該點，讓隊伍整體停下再繼續
+            3. 否則 → 平滑弧：
+                  R_base = formation_max_lateral + min_inner_radius
+                  d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
+                  R_actual = d / tan(β/2)
+                  tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
+                  arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
+                  arc 由 theta_in 到 theta_out 以 arc_resolution 等分
+               path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
+               barrier 放在 T_out (轉完彎後的集合點)
+
+        path 是一個 list[dict]，每個 dict 至少含:
+            {'seg': 'straight' | 'arc',
+             'x': float, 'y': float,
+             'dir': (dx, dy)}
+        'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
 
         Returns:
             (path, barrier_indices)
@@ -326,35 +385,129 @@ class FormationPlanner:
                 'seg': 'straight',
                 'x': waypoints[0][0], 'y': waypoints[0][1],
                 'dir': (0.0, 1.0),
-                's': 0.0,
             }], []
 
+        segments = []
+        for i in range(num_wps - 1):
+            dx = waypoints[i + 1][0] - waypoints[i][0]
+            dy = waypoints[i + 1][1] - waypoints[i][1]
+            length = math.hypot(dx, dy)
+            if length < 0.01:
+                segments.append(((0.0, 1.0), length))
+            else:
+                segments.append(((dx / length, dy / length), length))
+
+        R_base = formation_max_lateral + min_inner_radius
         path = []
         barrier_indices = []
 
-        for i in range(num_wps):
-            if i < num_wps - 1:
-                dx = waypoints[i + 1][0] - waypoints[i][0]
-                dy = waypoints[i + 1][1] - waypoints[i][1]
-            else:
-                dx = waypoints[i][0] - waypoints[i - 1][0]
-                dy = waypoints[i][1] - waypoints[i - 1][1]
+        # 起點
+        path.append({
+            'seg': 'straight',
+            'x': waypoints[0][0], 'y': waypoints[0][1],
+            'dir': segments[0][0],
+        })
 
-            length = math.hypot(dx, dy)
-            if length < 0.01:
-                direction = (0.0, 1.0)
-            else:
-                direction = (dx / length, dy / length)
+        for i in range(1, num_wps - 1):
+            u_in, len_in = segments[i - 1]
+            u_out, len_out = segments[i]
+            wp = waypoints[i]
 
+            dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
+            dot = max(-1.0, min(1.0, dot))
+
+            # 銳角：hover + 原地轉向
+            if dot < sharp_turn_cos_threshold:
+                avg_dx = u_in[0] + u_out[0]
+                avg_dy = u_in[1] + u_out[1]
+                avg_len = math.hypot(avg_dx, avg_dy)
+                if avg_len > 0.01:
+                    avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
+                else:
+                    avg_dir = u_in
+                path.append({
+                    'seg': 'straight',
+                    'x': wp[0], 'y': wp[1],
+                    'dir': avg_dir,
+                })
+                barrier_indices.append(len(path) - 1)
+                continue
+
+            # 平滑弧
+            beta = math.acos(dot)
+            if beta < 1e-4:
+                # 幾乎直線，不插弧
+                path.append({
+                    'seg': 'straight',
+                    'x': wp[0], 'y': wp[1],
+                    'dir': u_in,
+                })
+                continue
+
+            half_tan = math.tan(beta / 2.0)
+            d_ideal = R_base * half_tan
+            d_max = 0.45 * min(len_in, len_out)
+            d = min(d_ideal, d_max)
+            R_actual = d / half_tan
+
+            T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
+            T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
+
+            # +1 CCW (左轉) / -1 CW (右轉)
+            cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
+            sign = 1 if cross > 0 else -1
+
+            # 入向左法線
+            n_left = (-u_in[1], u_in[0])
+            center = (
+                T_in[0] + R_actual * sign * n_left[0],
+                T_in[1] + R_actual * sign * n_left[1],
+            )
+
+            theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
+
+            # T_in (straight, 方向 u_in)
             path.append({
                 'seg': 'straight',
-                'x': waypoints[i][0], 'y': waypoints[i][1],
-                'dir': direction,
+                'x': T_in[0], 'y': T_in[1],
+                'dir': u_in,
             })
 
-            if 0 < i < num_wps - 1:
-                barrier_indices.append(len(path) - 1)
+            # 弧段採樣 k=1..arc_resolution-1（不含兩端）
+            for k in range(1, arc_resolution):
+                t = k / arc_resolution
+                theta = theta_in + sign * beta * t
+                px = center[0] + R_actual * math.cos(theta)
+                py = center[1] + R_actual * math.sin(theta)
+                # 切線: sign × (-sin θ, cos θ)
+                tx = sign * (-math.sin(theta))
+                ty = sign * math.cos(theta)
+                path.append({
+                    'seg': 'arc',
+                    'x': px, 'y': py,
+                    'dir': (tx, ty),
+                    'arc_center': center,
+                    'arc_R': R_actual,
+                    'arc_sign': sign,
+                    'theta': theta,
+                })
 
+            # T_out (straight, 方向 u_out)，barrier 放這
+            path.append({
+                'seg': 'straight',
+                'x': T_out[0], 'y': T_out[1],
+                'dir': u_out,
+            })
+            barrier_indices.append(len(path) - 1)
+
+        # 終點
+        path.append({
+            'seg': 'straight',
+            'x': waypoints[-1][0], 'y': waypoints[-1][1],
+            'dir': segments[-1][0],
+        })
+
+        # 後處理：為每個 sample 加上累積 polyline 弧長 s
         path[0]['s'] = 0.0
         for i in range(1, len(path)):
             prev = path[i - 1]

src/GUI/overview_table.py

@@ -6,38 +6,29 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    # 「任務狀態」放最上面：閉環跑點時一眼看每台的 WP 進度 + 通訊(送出)狀態。
-    # 此列不來自遙測 QLabel，由 MissionExecutor 訊號經 update_table(field='mission') 餵入。
-    DEFAULT_INFO_TYPES = ["任務狀態",
-                          "模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
-                          "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
-                          "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
-
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
+                          "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+    
     DEFAULT_INFO_TYPE_MAP = {
-        "mission": 0,
-        "mode": 1,
-        "armed": 2,
-        "battery": 3,
-        "longitude": 4,
-        "latitude": 5,
-        "fix_type": 6,
-        "satellites_visible": 7,
-        "eph": 8,
-        "epv": 9,
-        "altitude": 10,
-        "local": 11,
-        "velocity": 12,
-        "groundspeed": 13,
-        "heading": 14,
-        "airspeed": 15,
-        "throttle": 16,
-        "hud_alt": 17,
-        "climb": 18,
-        "roll": 19,
-        "pitch": 20,
-        "yaw": 21,
-        "loss_rate": 22,
-        "ping": 23
+        "mode": 0,
+        "armed": 1,
+        "battery": 2,
+        "longitude": 3,
+        "latitude": 4,
+        "altitude": 5,
+        "local": 6,
+        "velocity": 7,
+        "groundspeed": 8,
+        "heading": 9,
+        "airspeed": 10,
+        "throttle": 11,
+        "hud_alt": 12,
+        "climb": 13,
+        "roll": 14,
+        "pitch": 15,
+        "yaw": 16,
+        "loss_rate": 17,
+        "ping": 18
     }
     
     def __init__(self, info_types=None, info_type_map=None, parent=None):
@@ -47,8 +38,6 @@ class OverviewTable(QTableWidget):
         self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES
         self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP
         self.drones = {}  # 存儲無人機面板的引用
-        # 任務狀態不是遙測，不在 panel QLabel 內；獨立保存，避免 refresh_all 用遙測把它蓋掉
-        self.mission_status = {}  # drone_id -> 任務狀態文字
         
         # 初始化表格
         self.setColumnCount(1)
@@ -81,11 +70,7 @@ class OverviewTable(QTableWidget):
         if drone_id and field and value:
             if drone_id not in self.drones:
                 return
-
-            # 任務狀態另存一份，讓 refresh_all（用遙測重繪）不會把它清掉
-            if field == 'mission':
-                self.mission_status[drone_id] = value
-
+                
             col = 1 + list(self.drones.keys()).index(drone_id)
             row = self.info_type_map.get(field, -1)
 
@@ -114,12 +99,8 @@ class OverviewTable(QTableWidget):
         for col, did in enumerate(self.drones, start=1):
             panel = self.drones[did]
             for field, row in self.info_type_map.items():
-                if field == 'mission':
-                    # 任務狀態非遙測，取自獨立保存，避免被 "--" 蓋掉
-                    val = self.mission_status.get(did, "--")
-                else:
-                    lbl = panel.findChild(QLabel, f"{did}_{field}")
-                    val = lbl.text() if lbl else "--"
+                lbl = panel.findChild(QLabel, f"{did}_{field}")
+                val = lbl.text() if lbl else "--"
                 val_item = QTableWidgetItem(val)
                 val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
                 self.setItem(row, col, val_item)

src/fc_interfaces/CMakeLists.txt

@@ -8,19 +8,11 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(builtin_interfaces REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/AttitudeRaw.msg"
-  "msg/GnssRaw.msg"
-  "msg/SystemDiagnosticsRaw.msg"
-  "msg/FcNetworkLog.msg"
-  "msg/ServiceAckResult.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
   "srv/MavPositionTargetGlobalInt.srv"
-  DEPENDENCIES std_msgs builtin_interfaces
   )
 
 ament_package()

src/fc_interfaces/msg/AttitudeRaw.msg

@@ -1,9 +0,0 @@
-uint32 seq
-builtin_interfaces/Time stamp
-string frame_id
-float32 roll
-float32 pitch
-float32 yaw
-float32 rollspeed
-float32 pitchspeed
-float32 yawspeed

src/fc_interfaces/msg/FcNetworkLog.msg

@@ -1,12 +0,0 @@
-uint8 DEBUG=10
-uint8 INFO=20
-uint8 WARN=30
-uint8 ERROR=40
-uint8 FATAL=50
-
-builtin_interfaces/Time stamp
-uint8 level
-string source
-string event_code
-int32 sysid
-string message

src/fc_interfaces/msg/GnssRaw.msg

@@ -1,8 +0,0 @@
-builtin_interfaces/Time stamp
-float64 latitude
-float64 longitude
-float64 altitude
-uint8 fix_type
-uint8 satellites_visible
-float32 eph
-float32 epv

src/fc_interfaces/msg/ServiceAckResult.msg

@@ -1,43 +0,0 @@
-# ServiceAckResult.msg
-# ACK code definition for command-like service responses.
-# 這邊只是定義常數的檔案
-#
-# Design:
-# - 0~10   : MAVLink MAV_RESULT compatible range (autopilot/firmware ACK) 大多不會用到 直接吃飛控回傳值
-# - 50~89  : System-level ACK codes (local bridge/orchestrator status)
-# - 90~99  : Unknown/reserved fallback
-#
-# Rule:
-# - If FC ACK is received, return MAV_RESULT_* code.
-# - If FC ACK is not reachable / local pipeline failed, return SYS_* code.
-
-# ---- MAVLink MAV_RESULT (standard) ---- 詳細請自行去網路找 mavlink MAV_RESULT 
-uint8 MAV_RESULT_ACCEPTED=0             # 封包正常接受並執行
-uint8 MAV_RESULT_TEMPORARILY_REJECTED=1 # 封包正常接受 但是飛控很忙 不執行 需要等待後重試
-uint8 MAV_RESULT_DENIED=2               # 封包不正常 參數可能給錯了
-uint8 MAV_RESULT_UNSUPPORTED=3          # 封包不正常 根本認不出是啥
-uint8 MAV_RESULT_FAILED=4               # 封包正常接受 但是載具的狀態異常 不執行
-uint8 MAV_RESULT_IN_PROGRESS=5
-uint8 MAV_RESULT_CANCELLED=6
-uint8 MAV_RESULT_COMMAND_LONG_ONLY=7
-uint8 MAV_RESULT_COMMAND_INT_ONLY=8
-uint8 MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME=9
-uint8 MAV_RESULT_NOT_IN_CONTROL=10
-
-# ---- System-level result codes (custom) ----
-# 50 : 有接到預期的封包 但是訊息建議再確認
-# 51 : some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair 
-# 
-# 52 : mavlink communication timeout 可能是通訊丟包了
-# 53 : there already has a command being execution for this sysid, try later
-# 54 : this sysid is not found
-
-uint8 FCSYS_GET_EXPECTED_FEEDBACK=50
-uint8 FCSYS_NO_EXPECTED_FEEDBACK=51
-uint8 FCSYS_MAVLINK_TIMEOUT=52
-uint8 FCSYS_BUSY=53
-uint8 FCSYS_DEVICE_NOT_FOUND=54
-
-
-# ---- Fallback ----
-uint8 FCSYS_UNKNOWN=90

src/fc_interfaces/msg/SystemDiagnosticsRaw.msg

@@ -1,11 +0,0 @@
-builtin_interfaces/Time stamp
-uint32 sensors_install_mask
-uint32 sensors_enabled_mask
-uint32 sensors_health_mask
-uint16 mcu_load
-uint16 bus_error_rate
-uint16 bus_error_count
-uint16 errors_count1
-uint16 errors_count2
-uint16 errors_count3
-uint16 errors_count4

src/fc_interfaces/package.xml

@@ -15,11 +15,6 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
-  <build_depend>std_msgs</build_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <build_depend>builtin_interfaces</build_depend>
-  <exec_depend>builtin_interfaces</exec_depend>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/fc_network_adapter/fc_network_adapter/ackEnum.py

@@ -0,0 +1,31 @@
+
+'''
+單獨出來定義回傳碼的意義
+若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
+若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
+所以要避開重疊的號碼
+
+這邊備註 Mavlink 的 MAV_RESULT 參數意義
+0 : 封包正常接受並執行
+1 : 封包正常接受 但是正在忙不執行 需要等待後重試
+4 : 封包正常接受 但是載具的狀態異常 不執行
+
+2 : 封包不正常 參數可能給錯了
+3 : 封包不正常 根本認不出是啥
+
+'''
+
+
+from enum import IntEnum
+
+class serviceAckResult(IntEnum):
+    MAVLINK_TIMEOUT = 51 # mavlink communication timeout
+
+    MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair 
+
+    MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
+    MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
+
+    UNKNOWN = 90 # i did not capture this error, try read the logs
+
+

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -37,7 +37,6 @@
 - *self.bridge* 存放 mavlink_bridge 實例 
 - *self.plumber* 存放 serial_manager 實例
 - *self.vehicle_registry* 存放 vehicle_registry 實例
-- *self.fc_ros_manager* 存放 fc_ros_manager 實例 
 
 - *self.panel_thread* 面板的執行緒
 - *self.panelState* 暫存面板與調配者互動的資料流動區
@@ -177,15 +176,13 @@
     這裡支持了所有 ROS2 框架的對應功能 包涵 
     1. 將載具的串流更新的狀態更新到對應的 topic 
     2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
-    3. 訂閱外部 RTCM 資料並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
 
 ### **[Class]** fc_ros_manager
-    MAVLink ROS2 節點管理器 管理三個獨立的 Node
-    會開一個執行緒 讓三個 Node 都跑在裡面
+    MAVLink ROS2 節點管理器 管理兩個獨立的 Node
+    會開一個執行緒 讓兩個 Node 都跑在裡面
     然後用 spin_once 保持 Node 的活性
 - *self.status_publisher* 物件實例
 - *self.command_service* 物件實例
-- *self.rtcm_relay* 物件實例
 - *self.spin_thread* 執行緒
 ---
 - *start()* 啟動自己
@@ -213,20 +210,6 @@
 - *__init__()* 這邊登入要創建的 service 到 ros2 系統
 - *handle_XXX()* 這邊是實現 service 的具體方法
 
-### **[Class]** RtcmRelay
-    訂閱 /fc_network/rtcm_in (mavros_msgs/msg/RTCM) 並轉發為 MAVLink GPS_RTCM_DATA
-    topic 的 QoS：BEST_EFFORT / KEEP_LAST depth=1 / VOLATILE（低延遲優先，丟舊包不排隊）
-    處理順序：過期丟棄(1s) → blake2s hash 去重 → 最短轉發間隔節流 → 分片轉發
-    分片：GPS_RTCM_DATA 每片 180 bytes，超過自動分片，最多 4 片 (720 bytes)，超長直接丟棄
-    失敗語意：不做舊包重送，送失敗就放棄等下一包
-- *self.counters* 觀測用計數器 (received/forwarded/dropped_stale/dropped_duplicate/dropped_throttle/dropped_oversize/no_route/send_fail)
-- *self.clock_offset_sec* 與 RTCM 來源的時鐘偏移量（秒）
----
-- *_rtcm_callback()* 收到 RTCM 封包時的處理流程
-- *_send_rtcm()* 對單一載具編碼並送出 GPS_RTCM_DATA（含分片邏輯）
-- *set_clock_offset()* 設定時鐘偏移量
-- *get_counters()* 取得觀測計數器
-
 
 # 開發記錄
 
@@ -241,14 +224,7 @@
 8. 基礎載具流量觀測
 9. 載具狀態收集與彙整
 10. a. ros2 topic 應用開發介面 (基礎框架)
-        - GNSS (alt lan fix_type epv eph)
-        - attitude position_ned (pitch yaw row local_position)
-        - battery
     b. ros2 service 應用開發介面 (基礎框架)
-    c. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
-11. ros2 topic service rtcm Node 重啟功能
-12. 可以定義 ROS_DOMAIN_ID
-
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-PROJECT_VER = "v1.20"
+PROJECT_VER = "v0.70"
 
 class PanelState:
     def __init__(self):
@@ -177,18 +177,11 @@ class ControlPanel:
                     MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
                     MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
                 ]),
-                MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "SHOW_VEHICLES"),
+                MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
                 MenuNode("Engineer Mode", "工程模式", children=[
-                    MenuNode("Stop Manager", "停止 Mavlink_Object 管理", "STOP_MANAGER"),
+                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
                     MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
-                    MenuNode("Stop Serial M.", "停止 Serial 端口", "STOP_SERIAL_MANAGER"),
-                    MenuNode("Edit ROS Nodes", "管理 ROS NODE", children=[
-                        MenuNode("Reboot Topics", "重建 ROS Topics", "REBOOT_ROS_TOPICS"),
-                        MenuNode("Reboot Services", "重建 ROS Services", "REBOOT_ROS_SERVICES"),
-                        MenuNode("Reboot RTCM", "重建 RTK RTCM", "REBOOT_ROS_RTCM"),
-                        MenuNode("Stop ROS Manager", "停止 ROS 功能", "STOP_ROS_MANAGER"),
-                        MenuNode("Start ROS Manager", "啟動 ROS 功能", "START_ROS_MANAGER"),
-                    ]),
+                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
                 ]),
                 MenuNode("Shutdown", "關閉整個系統", children=[
                     MenuNode("Return", "繼續運行", "BACK"),
@@ -391,7 +384,6 @@ class ControlPanel:
                         state.intoTERMINATION()
                         pre_panel_shutdown()
 
-                    # related with -> UDP mavlink Object 
                     elif selected.action == "TEXT_UDP_IP":
                         result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
                         if result is not None:
@@ -420,8 +412,6 @@ class ControlPanel:
                             # menu_stack.pop()
                             # idx_stack.pop()
 
-
-                    # related with -> Serial Connection
                     elif selected.action == "TEXT_BAUD_SERIAL":
                         result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
                         if result is not None:
@@ -435,10 +425,6 @@ class ControlPanel:
                         state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
                         menu_stack.pop()
                         idx_stack.pop()
-                    elif selected.action == "SET_SERIAL_COMM_XBEE_ESP":
-                        state.serial_info_temp["CommunicationType"] = "XBee(API-API) espv1"
-                        menu_stack.pop()
-                        idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
                         state.serial_info_temp["CommunicationType"] = "TELEMETRY"
                         menu_stack.pop()
@@ -491,7 +477,12 @@ class ControlPanel:
                                 menu_stack.pop()
                                 idx_stack.pop()
 
-                    # related with -> 列表選單
+                    elif selected.action == "LIST_MAV_OBJECT":
+                        # 動態生成 mavlink_object 列表選單
+                        created_list_menu = self.create_object_list_menu(state, page=0)
+                        menu_stack.append(created_list_menu)
+                        idx_stack.append(0)
+
                     elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
                         if hasattr(selected, 'page'):
                             current_list_menu = menu_stack[-1]
@@ -516,13 +507,6 @@ class ControlPanel:
                             menu_stack.append(created_list_menu)
                             idx_stack.append(0)
 
-                    # related with -> mavlink object list
-                    elif selected.action == "LIST_MAV_OBJECT":
-                        # 動態生成 mavlink_object 列表選單
-                        created_list_menu = self.create_object_list_menu(state, page=0)
-                        menu_stack.append(created_list_menu)
-                        idx_stack.append(0)
-
                     elif selected.action == "INSPECT_MAV_OBJECT":
                         # 顯示物件詳細資訊
                         if hasattr(selected, 'socket_id'):
@@ -567,7 +551,6 @@ class ControlPanel:
                             if target_id is not None:
                                 cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
 
-                    # related with -> Engineer Mode
                     elif selected.action == "STOP_MANAGER":
                         if state.panel_status != "Engineer":
                             state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
@@ -585,40 +568,8 @@ class ControlPanel:
                             state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
                             continue  # 只有在工程模式下才能操作
                         cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
-                    
-                    elif selected.action == "STOP_ROS_MANAGER":
-                        if state.panel_status != "Engineer":
-                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
-                            continue  # 只有在工程模式下才能操作
-                        cmd_q.put("STOP_ROS_MANAGER")
 
-                    elif selected.action == "START_ROS_MANAGER":
-                        if state.panel_status != "Engineer":
-                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
-                            continue  # 只有在工程模式下才能操作
-                        cmd_q.put("START_ROS_MANAGER")
-                        
-                    
-                    elif selected.action == "REBOOT_ROS_TOPICS":
-                        if state.panel_status != "Engineer":
-                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
-                            continue  # 只有在工程模式下才能操作
-                        cmd_q.put(("RESTART_ROS_NODE", "status_publisher")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
-                    
-                    elif selected.action == "REBOOT_ROS_SERVICES":
-                        if state.panel_status != "Engineer":
-                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
-                            continue  # 只有在工程模式下才能操作
-                        cmd_q.put(("RESTART_ROS_NODE", "command_service")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
-
-                    elif selected.action == "REBOOT_ROS_RTCM":
-                        if state.panel_status != "Engineer":
-                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
-                            continue  # 只有在工程模式下才能操作
-                        cmd_q.put(("RESTART_ROS_NODE", "rtcm_relay")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
-
-                    # related with -> 進 bridge 的設備 列表
-                    elif selected.action == "SHOW_VEHICLES":
+                    elif selected.action == "INSPECT_VEHICLES":
                         # 進入載具檢視選單
                         cmd_q.put("UPDATE_VEHICLES_LIST")
                         created_list_menu = self.create_vehicles_list_menu(state, page=0)
@@ -816,7 +767,6 @@ class ControlPanel:
                 port_menu = MenuNode(f"{port}", children=[
                     MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[
                         MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"),
-                        MenuNode("XBee(API-API)", "XBee 模式(with ESP)", "SET_SERIAL_COMM_XBEE_ESP"),
                         MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                     ]),
                     MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
@@ -1111,7 +1061,7 @@ class ControlPanel:
                 0: "HB", 1: "S_STAT", 2: "S_TIME", 24: "GPS_RAW", 27: "RAW_IMU",
                 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
                 74: "VFR_HUD", 147: "BATT_ST", 136: "TERRAIN", 241: "VIBRAT",
-                125: "POW_STA", 253: "STAT_TXT",
+                125: "POW_STA",
             }
 
             # ardupilot mega 
@@ -1195,16 +1145,14 @@ class Orchestrator:
         self.fc_ros_manager = mros.ros2_manager
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
-            'position_gnss': 0.2,
-            'position_ned': 0.5,
-            'attitude': 0.1,
+            'position': 1.0,
+            'position_ned': 1.0,
+            'attitude': 1.0,
             'velocity': 0.0,
             'battery': 1.0,
             'vfr_hud': 1.0,
             'mode': 0.0,
             'summary': 1.0,
-            'sys_diags': 1.0,
-            'status_text': 1.0,
         }
 
     def engageWholeSystem(self):
@@ -1225,8 +1173,6 @@ class Orchestrator:
 
     def mainLoop(self):
         logger.info("Main orchestrator started <-")
-        self.ros2_retry_time = None
-        self.ros2_manual_stop = False
         try:
             # while not self.stop_evt.is_set():
             while self.panel_thread.is_alive():
@@ -1258,17 +1204,6 @@ class Orchestrator:
                 else:
                     self.panelState.ros2_manager_state = 'Stopped'
 
-                    # 這邊做一個簡易的重啟 掛掉要等待一秒再重啟 避免過度頻繁 # 算是高技術債的方式構建 但是未來有需要在還吧 (煙
-                    if not self.ros2_manual_stop:
-                        # logger.info(f"mark A {self.ros2_retry_time}")
-                        if self.ros2_retry_time:
-                            if (time.time() - self.ros2_retry_time) > 1:
-                                self.fc_ros_manager.start()
-                                self.fc_ros_manager.schedule_restart_node("command_service")
-                                self.ros2_retry_time = None
-                        else:
-                            self.ros2_retry_time = time.time()
-
                 # B. 更新載具列表（從 vehicle_registry 獲取）
                 self._update_vehicles_list()
 
@@ -1333,12 +1268,6 @@ class Orchestrator:
                         elif action == "UNREGISTER_VEHICLE":
                             sysid = cmd[1]
                             self.vehicle_registry.unregister(sysid)
-                        elif action == "RESTART_ROS_NODE":
-                            node_key = cmd[1]
-                            if not self.fc_ros_manager.schedule_restart_node(node_key):
-                                logger.warning(
-                                    f"RESTART_ROS_NODE failed: {node_key!r}"
-                                )
 
                     elif cmd == "CREATE_UDP_INBOUND":
                         self.panelState.udp_info_temp["direction"] = "inbound"
@@ -1348,19 +1277,12 @@ class Orchestrator:
                         self.create_udp_object()
                     elif cmd == "CREATE_SERIAL_PORT":
                         self.create_serial_port_object()
-
                     elif cmd == "SHUTDOWN_BRIDGE":
                         self.bridge.stop()
                     elif cmd == "SHUTDOWN_MANAGER":
                         self.manager.shutdown()
                     elif cmd == "SHUTDOWN_SERIAL_MANAGER":
                         self.plumber.shutdown()
-                    elif cmd == "STOP_ROS_MANAGER":
-                        self.fc_ros_manager.stop()
-                        self.ros2_manual_stop = True
-                    elif cmd == "START_ROS_MANAGER":
-                        self.fc_ros_manager.start()
-                        self.ros2_manual_stop = False
 
                 except queue.Empty:
                     pass
@@ -1600,7 +1522,6 @@ class Orchestrator:
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
             "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
-            "XBee(API-AT)": sm.SerialMode.XBEEAPI_espv1,
             "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
@@ -1644,7 +1565,25 @@ class Orchestrator:
 
 def main():
 
-    logger.info(f"Each Module Running Version at mavlinkObkect:{mo.MODULE_VER}, mavlinkROS2Nodes:{mros.MODULE_VER}, mavlinkVehicleView:{mvv.MODULE_VER}, serialManager:{sm.MODULE_VER}")
+    # =========== 各項模組的版本先驗 ===========
+    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
+    version_check = True
+    if mo.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkObkect")
+        version_check = False
+    if mros.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkROS2Nodes")
+        version_check = False
+    if mvv.MODULE_VER != "1.00":
+        print("Module Version Error! : mavlinkVehicleView")
+        version_check = False
+    if sm.MODULE_VER != "0.80":
+        print("Module Version Error! : serialManager")
+        version_check = False
+    if version_check == False:
+        print("Environment Obstacle! Check YOUR Execution System Path First!!")
+        return
+    # ========================================
 
     stop_evt = threading.Event()
     def signal_handler(signum, frame):
@@ -1683,7 +1622,4 @@ if __name__ == "__main__":
 2025.04.13
 1. 加入各項模組的版本先驗 檢驗失敗就直接離開
 
-2025.05.28
-1. 工程模式 新增 Node 重啟的功能
-
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -47,11 +47,11 @@ from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
-    vehicle_registry, # 儲存全部物件的地方
-    VehicleView, # 代表每台載具的最外層
+    vehicle_registry,
+    VehicleView,
     VehicleComponent,
     ComponentType,
-    StatusTextEntry,
+    ConnectionType
 )
 from .utils import RingBuffer, setup_logger
 
@@ -108,15 +108,12 @@ class mavlink_bridge:
     def _init_message_handlers(self):
         """初始化訊息處理器映射表，提高處理效率"""
         self.message_handlers = {
-            0: self._handle_heartbeat,          # HEARTBEAT
-            1: self._handle_vehicle_sys_status, # SYS_STATUS
-            24: self._handle_gps_raw_int,       # GPS_RAW_INT
-            30: self._handle_attitude,          # ATTITUDE
-            32: self._handle_local_position,    # LOCAL_POSITION_NED
-            33: self._handle_global_position,   # GLOBAL_POSITION_INT
-            74: self._handle_vfr_hud,           # VFR_HUD
-            147: self._handle_battery_status,   # BATTERY_STATUS
-            253: self._handle_status_text,      # STATUSTEXT
+            0: self._handle_heartbeat,        # HEARTBEAT
+            30: self._handle_attitude,        # ATTITUDE
+            32: self._handle_local_position,  # LOCAL_POSITION_NED
+            33: self._handle_global_position, # GLOBAL_POSITION_INT
+            74: self._handle_vfr_hud,         # VFR_HUD
+            147: self._handle_battery_status, # BATTERY_STATUS
         }
 
     def start(self):
@@ -233,29 +230,6 @@ class mavlink_bridge:
             'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
         }
 
-    def _handle_vehicle_sys_status(self, vehicle, component, msg, timestamp):
-        """處理 SYS_STATUS 訊息 (msg_id: 1)"""
-        diag = component.status.sys_diag
-        diag.sensors_install_mask = msg.onboard_control_sensors_present
-        diag.sensors_enabled_mask = msg.onboard_control_sensors_enabled
-        diag.sensors_health_mask = msg.onboard_control_sensors_health
-        diag.mcuLoad = msg.load
-        diag.busErrorRate = msg.drop_rate_comm
-        diag.busErrorCount = msg.errors_comm
-        diag.errors_count1 = msg.errors_count1
-        diag.errors_count2 = msg.errors_count2
-        diag.errors_count3 = msg.errors_count3
-        diag.errors_count4 = msg.errors_count4
-        diag.timestamp = timestamp
-
-    def _handle_status_text(self, vehicle, component, msg, timestamp):
-        """處理 STATUSTEXT 訊息 (msg_id: 253)"""
-        text = msg.text.rstrip('\x00') if msg.text else ''
-        if not text:
-            return
-        component.status.status_text_queue.append(
-            StatusTextEntry(text=text, severity=msg.severity, timestamp=timestamp)
-        )
 
     def _handle_global_position(self, vehicle, component, msg, timestamp):
         """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
@@ -265,14 +239,6 @@ class mavlink_bridge:
         component.status.position.relative_altitude = msg.relative_alt / 1000.0  # 轉換為公尺
         component.status.position.timestamp = timestamp
 
-    def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
-        """處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
-        component.status.gps.fix_type = msg.fix_type
-        component.status.gps.satellites_visible = msg.satellites_visible
-        component.status.gps.eph = None if msg.eph == 65535 else msg.eph
-        component.status.gps.epv = None if msg.epv == 65535 else msg.epv
-        component.status.gps.timestamp = timestamp
-
     def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
         """處理 VFR_HUD 訊息 (msg_id: 74)"""
         component.status.vfr.airspeed = msg.airspeed
@@ -420,9 +386,7 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        # 0 HEARTBEAT, 1 SYS_STATUS, 24 GPS_RAW_INT, 30 ATTITUDE, 
-        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 253 STATUSTEXT
-        self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147, 253])  
+        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表
@@ -859,8 +823,5 @@ if __name__ == '__main__':
 1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects
 2. async_io_manager 的 _stop_event 無效變數移除
 
-2026年 06月 10日
-1. 增加 SYS_STATUS 與 STATUSTEXT 訊息的處理機制
-
 '''
 

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,14 +1,11 @@
 """
 MAVLink ROS2 Nodes
-主要包含四個獨立的 ROS2 Node :
+主要包含兩個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
     從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
     以最基礎的 mavlink microservice 會實現目標 
     並不會包含額外的功能
-3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
-    過期丟棄、去重、節流、分片
-4. FcNetworkLogPublisher - 將 FC Network logging queue 發布到 /fc_network/logs
 
 與一個節點管理器
 - fc_ros_manager
@@ -18,8 +15,6 @@ MAVLink ROS2 Nodes
 import os
 import time
 import math
-import hashlib
-import queue
 import threading
 from typing import Dict, Optional
 
@@ -27,7 +22,6 @@ from typing import Dict, Optional
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import MultiThreadedExecutor
-from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 
 # ROS2 Message imports
 import std_msgs.msg
@@ -38,19 +32,15 @@ import mavros_msgs.msg
 
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
-import fc_interfaces.msg as fcmsg
-from fc_interfaces.msg import ServiceAckResult
+from .ackEnum import serviceAckResult
 
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
 from . import mavlinkObject as mo
-from .utils import get_ros_log_queue, setup_logger
+from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "2.50"
-
-FC_ROS_DOMAIN_ID = "0"
-NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay")
+MODULE_VER = "1.50"
 
 # ============================================================================
 #                       VehicleStatusPublisher Node
@@ -70,16 +60,14 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'summary': 0.0,         # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
-            'position_gnss': 0.0,   # GNSS位置 (海拔高度)
-            'position_ned': 0.0,    # LOCAL_POSITION_NED (位置+速度+相對高度)
-            'attitude': 0.0,        # 姿態 (pitch yaw row 與其加速狀態)
-            'battery': 0.0,         # 電池
-            'vfr_hud': 0.0,         # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
-            'sys_diags': 0.0,       # SYS_STATUS 系統診斷
-            'status_text': 0.0,     # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標）
-            'mode': 0.0,            # 飛行模式 (已經在 summary 裡 未來移除)
-            'velocity': 0.0,        # 速度 (已經包含在 vfr_hud 未來移除)
+            'position': 0.0,       # GNSS位置
+            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
+            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
+            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
+            'battery': 0.0,        # 電池
+            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -116,10 +104,6 @@ class PublishRateController:
         
         return False
     
-    def is_topic_enabled(self, topic: str) -> bool:
-        """檢查 topic 是否啟用（interval > 0）"""
-        return self.topic_intervals.get(topic, 0) > 0
-    
     def reset(self):
         """重置所有計時器"""
         self.last_publish_time.clear()
@@ -130,7 +114,7 @@ class VehicleStatusPublisher(Node):
     
     職責:
     - 定期從 vehicle_registry 讀取載具狀態
-    - 頻率控制 (位置/姿態 2Hz, 電池/摘要 1Hz)
+    - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
     - 發布標準 ROS2 消息類型
     - 檢測訂閱者，按需發布
     """
@@ -146,8 +130,8 @@ class VehicleStatusPublisher(Node):
         self.fc_publishers: Dict[tuple, any] = {}
         
         # 定時器：以較高頻率檢查 vehicle_registry 並發布
-        # 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
-        self.timer_period = 0.05  # 50ms
+        # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
+        self.timer_period = 0.1  # 100ms
         self.timer = self.create_timer(self.timer_period, self.timer_callback)
         
         # 狀態標誌
@@ -186,16 +170,14 @@ class VehicleStatusPublisher(Node):
         # 例如：self._publish_ekf_status(sysid, status)
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
-        self._publish_position_gnss(sysid, status)
+        self._publish_position(sysid, status)
         self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
+        self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
         self._publish_vfr_hud(sysid, status)
-        self._publish_summary(vehicle)
-        self._publish_system_diagnostics(sysid, status)
-        self._publish_status_text(sysid, status)
-        self._publish_velocity(sysid, status)
         self._publish_mode(sysid, status)
+        self._publish_summary(vehicle)
         # 在這裡新增更多 publish 方法調用...
     
     def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1):
@@ -216,12 +198,12 @@ class VehicleStatusPublisher(Node):
             topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
             publisher = self.create_publisher(msg_type, topic_name, qos)
             self.fc_publishers[key] = publisher
-            logger.debug(f"Created publisher: {topic_name}")
+            logger.info(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
-    def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
+    def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
         """發布 GPS 位置"""
-        if not self.rate_controller.should_publish(sysid, 'position_gnss'):
+        if not self.rate_controller.should_publish(sysid, 'position'):
             return
         
         pos = status.position
@@ -229,32 +211,27 @@ class VehicleStatusPublisher(Node):
             return
         
         publisher = self._get_or_create_publisher(
-            sysid, 'position_gnss', fcmsg.GnssRaw
+            sysid, 'position', sensor_msgs.msg.NavSatFix
             )
         
         # 檢查是否有訂閱者
         if publisher.get_subscription_count() == 0:
             return
         
-        msg = fcmsg.GnssRaw()
-        msg.stamp = self.get_clock().now().to_msg()
-        msg.latitude = float(pos.latitude)
-        msg.longitude = float(pos.longitude)
-        msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
+        msg = sensor_msgs.msg.NavSatFix()
+        msg.latitude = pos.latitude
+        msg.longitude = pos.longitude
+        msg.altitude = pos.altitude if pos.altitude is not None else 0.0
         
         # GPS 狀態資訊
         gps = status.gps
         if gps.fix_type is not None:
-            msg.fix_type = int(gps.fix_type)
-        if gps.satellites_visible is not None:
-            msg.satellites_visible = int(gps.satellites_visible)
-        msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
-        msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
+            msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
         
         publisher.publish(msg)
 
     def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
-        """發布 LOCAL_POSITION_NED (NED 座標，含位置與速度）"""
+        """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
         if not self.rate_controller.should_publish(sysid, 'position_ned'):
             return
 
@@ -290,28 +267,34 @@ class VehicleStatusPublisher(Node):
         publisher.publish(msg)
     
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
-        """發布原始姿態（roll/pitch/yaw 與角速度）"""
+        """發布姿態（IMU）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
             return
         
         att = status.attitude
-        if att.roll is None or att.pitch is None or att.yaw is None:
+        if att.roll is None:
             return
-        publisher = self._get_or_create_publisher(
-            sysid, 'attitude', fcmsg.AttitudeRaw
-        )
+        publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
         
         if publisher.get_subscription_count() == 0:
             return
         
-        msg = fcmsg.AttitudeRaw()
-        msg.stamp = self.get_clock().now().to_msg()
-        msg.roll = float(att.roll)
-        msg.pitch = float(att.pitch)
-        msg.yaw = float(att.yaw)
-        msg.rollspeed = float(att.rollspeed) if att.rollspeed is not None else 0.0
-        msg.pitchspeed = float(att.pitchspeed) if att.pitchspeed is not None else 0.0
-        msg.yawspeed = float(att.yawspeed) if att.yawspeed is not None else 0.0
+        msg = sensor_msgs.msg.Imu()
+        
+        # 歐拉角轉四元數
+        qx, qy, qz, qw = self._euler_to_quaternion(
+            att.roll, att.pitch, att.yaw
+        )
+        msg.orientation.x = qx
+        msg.orientation.y = qy
+        msg.orientation.z = qz
+        msg.orientation.w = qw
+        
+        # 角速度
+        if att.rollspeed is not None:
+            msg.angular_velocity.x = att.rollspeed
+            msg.angular_velocity.y = att.pitchspeed
+            msg.angular_velocity.z = att.yawspeed
         
         publisher.publish(msg)
     
@@ -449,7 +432,11 @@ class VehicleStatusPublisher(Node):
             'armed': status.armed if status.armed is not None else False,
             # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
             'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
-            'mav_status': status.system_status if status.system_status is not None else 0,
+            # 'latitude': status.position.latitude if status.position.latitude else 0.0,
+            # 'longitude': status.position.longitude if status.position.longitude else 0.0,
+            # 'altitude': status.position.altitude if status.position.altitude else 0.0,
+            # 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
+            'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
             'connection_type': vehicle.connected_via.value,
             'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
         }
@@ -458,62 +445,6 @@ class VehicleStatusPublisher(Node):
         msg.data = json.dumps(summary)
         publisher.publish(msg)
     
-    def _publish_system_diagnostics(self, sysid: int, status: mvv.ComponentStatus):
-        """發布 SYS_STATUS 系統診斷資訊"""
-        if not self.rate_controller.should_publish(sysid, 'sys_diags'):
-            return
-        
-        diag = status.sys_diag
-        if diag.sensors_install_mask is None:
-            return
-        
-        publisher = self._get_or_create_publisher(
-            sysid, 'sys_diags', fcmsg.SystemDiagnosticsRaw
-        )
-        
-        if publisher.get_subscription_count() == 0:
-            return
-        
-        msg = fcmsg.SystemDiagnosticsRaw()
-        msg.stamp = self.get_clock().now().to_msg()
-        msg.sensors_install_mask = int(diag.sensors_install_mask)
-        msg.sensors_enabled_mask = int(diag.sensors_enabled_mask or 0)
-        msg.sensors_health_mask = int(diag.sensors_health_mask or 0)
-        msg.mcu_load = int(diag.mcuLoad or 0)
-        msg.bus_error_rate = int(diag.busErrorRate or 0)
-        msg.bus_error_count = int(diag.busErrorCount or 0)
-        msg.errors_count1 = int(diag.errors_count1 or 0)
-        msg.errors_count2 = int(diag.errors_count2 or 0)
-        msg.errors_count3 = int(diag.errors_count3 or 0)
-        msg.errors_count4 = int(diag.errors_count4 or 0)
-        
-        publisher.publish(msg)
-    
-    def _publish_status_text(self, sysid: int, status: mvv.ComponentStatus):
-        """發布 STATUSTEXT 飛控文字 (佇列 drain, 無訂閱者直接丟棄) """
-        # 是否啟用
-        if not self.rate_controller.is_topic_enabled('status_text'):
-            return
-        # 是否有資料
-        queue = status.status_text_queue
-        if not queue:
-            return
-
-        publisher = self._get_or_create_publisher(sysid, 'status_text', std_msgs.msg.String)
-
-        # 是否有監聽者
-        if publisher.get_subscription_count() == 0:
-            queue.clear()
-            return
-
-        while queue:
-            entry = queue.popleft()
-            msg = std_msgs.msg.String()
-            ts = entry.timestamp if entry.timestamp is not None else 0.0
-            sev = entry.severity if entry.severity is not None else -1
-            msg.data = f'[{ts:.3f}] [{sev}] {entry.text}'
-            publisher.publish(msg)
-    
     # ═══════════════════════════════════════════════════════════════
     # 【新增 Topic 位置 3/4】
     # 若要新增 topic，請在此處實作對應的發布方法
@@ -532,6 +463,29 @@ class VehicleStatusPublisher(Node):
     #     # ... 實作發布邏輯
     # ═══════════════════════════════════════════════════════════════
     
+    @staticmethod
+    def _euler_to_quaternion(roll, pitch, yaw):
+        """
+        歐拉角轉四元數
+        
+        Args:
+            roll: 橫滾角 (弧度)
+            pitch: 俯仰角 (弧度)
+            yaw: 偏航角 (弧度)
+            
+        Returns:
+            tuple: (qx, qy, qz, qw)
+        """
+        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
+             math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
+             math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
+        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
+             math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
+        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
+             math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+        return (qx, qy, qz, qw)
+    
     def stop(self):
         """停止發布"""
         self.running = False
@@ -569,14 +523,6 @@ class MavlinkCommandService(Node):
     講白話一點就是
     每次接到一個 service 請求 要整個系統丟某種指令給載具時
     會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
-    
-    然後透過每次 manager spin 
-    會去呼叫 return_router() 方法
-    這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對
-    配對到的解開 Pending 
-
-    解開後 相對應的 handle_XXX 就會開始做事
-    
 
     """
 
@@ -585,7 +531,7 @@ class MavlinkCommandService(Node):
     def __init__(self):
         super().__init__('mavlink_command_service')
         
-        # 狀態標記 
+        # 狀態標記
         self.running = True
         
         # # mavlinkObject 的引用（將由外部設置） 用不到
@@ -631,7 +577,7 @@ class MavlinkCommandService(Node):
             self.handle_set_position_target_global_int
         )
 
-        # logger.info("MavlinkCommandService initialized")
+        logger.info("MavlinkCommandService initialized")
 
     def _index(self, target_sysid, target_compid):
 
@@ -658,19 +604,20 @@ class MavlinkCommandService(Node):
         藉由 event.set() 解開 service 中的 block 
         '''
         return_tuple = mo.return_packet_ring.get()
+        # 確認 return_packet_ring 有資料
         if return_tuple == None: return
 
         socketid, timestamp, msg = return_tuple
         sysid = msg.get_srcSystem()
 
         _pending = self._pending_by_sysid.get(sysid)
-        if _pending is None:
+        # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
+        if _pending == None:
             return
 
         if _pending.match_fn(msg):
             _pending.result_mav_msg = msg
             _pending.event.set()
-        return
 
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 發送 TIMESYNC 可以作為模板
@@ -755,20 +702,20 @@ class MavlinkCommandService(Node):
         # 設定失效回應
         response.success = False
         response.message = "Unknown error"
-        response.ack_result = ServiceAckResult.FCSYS_UNKNOWN
+        response.ack_result = serviceAckResult.UNKNOWN
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
             response.message = f"sysid {request.target_sysid} already has pending request"
-            response.ack_result = ServiceAckResult.FCSYS_BUSY
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
             return response
 
         # 2) 找到 socket 標地
         socketObject = self._index(request.target_sysid, request.target_compid)
         if socketObject is None:
             response.message = "This system id not found."
-            response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
             return response
 
         # 3) 接收封包系統 的設定
@@ -799,13 +746,12 @@ class MavlinkCommandService(Node):
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
             response.message = "mavlink timeout - CommLONG"
-            response.ack_result = ServiceAckResult.FCSYS_MAVLINK_TIMEOUT
+            response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
             fail_skip = True
 
         # 6) 處理回應封包
         if not fail_skip:
             ack_msg = _pending.result_mav_msg
-            # response.success = ServiceAckResult.FCSYS_MAVLINK_BREAK # dev 故意觸發錯誤用的
             response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
             response.message = "" # 沒有消息就是好消息
             response.ack_result = ack_msg.result
@@ -829,20 +775,20 @@ class MavlinkCommandService(Node):
         '''
         # 設定失效回應
         response.message = "Unknown error"
-        response.ack_result = ServiceAckResult.FCSYS_UNKNOWN
+        response.ack_result = serviceAckResult.UNKNOWN
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
             response.message = f"sysid {request.target_sysid} already has pending request"
-            response.ack_result = ServiceAckResult.FCSYS_BUSY
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
             return response
 
         # 2) 找到 socket 標地
         socketObject = self._index(request.target_sysid, request.target_compid)
         if socketObject is None:
             response.message = "This system id not found."
-            response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
             return response
 
         # 3) 接收封包系統 的設定
@@ -877,7 +823,7 @@ class MavlinkCommandService(Node):
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
             response.message = "mavlink timeout - SET POSITION 86"
-            response.ack_result = ServiceAckResult.FCSYS_NO_EXPECTED_FEEDBACK
+            response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
             fail_skip = True
 
         # 6) 處理回應封包
@@ -885,7 +831,7 @@ class MavlinkCommandService(Node):
             ack_msg = _pending.result_mav_msg
 
             response.message = ""
-            response.ack_result = ServiceAckResult.MAV_RESULT_ACCEPTED
+            response.ack_result = 0
             response.r_time_boot_ms = ack_msg.time_boot_ms 
             response.r_type_mask = ack_msg.type_mask
             response.r_lat_int = ack_msg.lat_int
@@ -1002,235 +948,11 @@ class MavlinkCommandService(Node):
         return response
 
 
+
     def stop(self):
         """停止服務"""
         self.running = False
-        # logger.info("MavlinkCommandService stopped")
-
-
-# ============================================================================
-#                          RtcmRelay Node
-# ============================================================================
-
-class RtcmRelay(Node):
-    """
-    RTCM 轉發節點 - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
-
-    職責:
-    - 訂閱外部 RTCM 資料（mavros_msgs/msg/RTCM）
-    - 過期丟棄：封包 Header.stamp 超過 1 秒則丟棄
-    - 去重：與上一筆 payload blake2s hash 相同則丟棄
-    - 節流：最短轉發間隔保護（可參數化）
-    - 對所有在線載具直接編碼並送出 MAVLink GPS_RTCM_DATA（含分片）
-
-    MAVLink GPS_RTCM_DATA (msg_id=233) 每片 payload 上限 180 bytes
-    超過 180 bytes 需分片，最多 4 片 = 720 bytes
-    """
-
-    FRAGMENT_MAX_LEN = 180
-    MAX_TOTAL_LEN = 4 * 180  # 720 bytes
-    STALE_THRESHOLD_SEC = 1.0
-
-    def __init__(self, min_forward_interval_ms: float = 0.0, clock_offset_ms: float = 0.0):
-        super().__init__('rtcm_relay')
-
-        self.running = True
-        self.min_forward_interval_sec = min_forward_interval_ms / 1000.0
-        self.clock_offset_sec = clock_offset_ms / 1000.0
-
-        # 去重：上一筆 payload 的短 hash
-        self._last_hash: Optional[bytes] = None
-
-        # 節流：上次成功轉發的時間
-        self._last_forward_time: float = 0.0
-
-        # MAVLink GPS_RTCM_DATA 分片序號 (0~31 循環，同一筆 RTCM 的所有分片共用)
-        self._seq: int = 0
-
-        # 觀測 counters
-        self.counters = {
-            'received': 0,
-            'forwarded': 0,
-            'dropped_stale': 0,
-            'dropped_duplicate': 0,
-            'dropped_throttle': 0,
-            'dropped_oversize': 0,
-            'no_route': 0,
-            'send_fail': 0,
-        }
-
-        # QoS: 低延遲、弱可靠 — 寧可掉舊包也不排隊
-        rtcm_qos = QoSProfile(
-            history=HistoryPolicy.KEEP_LAST,
-            depth=1,
-            reliability=ReliabilityPolicy.BEST_EFFORT,
-            durability=DurabilityPolicy.VOLATILE,
-        )
-
-        self.subscription = self.create_subscription(
-            mavros_msgs.msg.RTCM,
-            '/fc_network/rtcm_input',
-            self._rtcm_callback,
-            rtcm_qos,
-        )
-
-        # logger.info("RtcmRelay initialized")
-
-    # ── callback ──────────────────────────────────────────────────────
-
-    def _rtcm_callback(self, msg):
-        """處理順序：過期檢查 → 去重 → 節流 → 轉發"""
-        if not self.running:
-            return
-
-        self.counters['received'] += 1
-        data = bytes(msg.data)
-        now = time.time()
-
-        # 1) 過期丟棄
-        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
-        age = (now + self.clock_offset_sec) - stamp_sec
-        if age > self.STALE_THRESHOLD_SEC:
-            self.counters['dropped_stale'] += 1
-            return
-
-        # 2) 去重：blake2s 短 hash 與上一筆比對
-        h = hashlib.blake2s(data, digest_size=8).digest()
-        if h == self._last_hash:
-            self.counters['dropped_duplicate'] += 1
-            return
-        self._last_hash = h
-
-        # 3) 節流：最短轉發間隔
-        if self.min_forward_interval_sec > 0:
-            if (now - self._last_forward_time) < self.min_forward_interval_sec:
-                self.counters['dropped_throttle'] += 1
-                return
-
-        # 4) 超長丟棄
-        if len(data) > self.MAX_TOTAL_LEN:
-            self.counters['dropped_oversize'] += 1
-            logger.warning(f"RTCM data too large ({len(data)} bytes), max {self.MAX_TOTAL_LEN}")
-            return
-
-        # 5) 取得本次的分片序號，然後遞增給下一筆
-        seq = self._seq
-        self._seq = (self._seq + 1) & 0x1F
-
-        # 6) 轉發給所有載具
-        forwarded_any = False
-        for sysid, vehicle in mvv.vehicle_registry.read_all():
-            socket_id = vehicle.custom_meta.get("socket_id")
-            if socket_id is None:
-                self.counters['no_route'] += 1
-                continue
-
-            mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
-            if mav_obj is None:
-                self.counters['no_route'] += 1
-                continue
-
-            try:
-                self._send_rtcm(mav_obj, data, seq)
-                forwarded_any = True
-            except Exception as e:
-                self.counters['send_fail'] += 1
-                logger.debug(f"RTCM send fail sysid={sysid}: {e}")
-
-        if forwarded_any:
-            self.counters['forwarded'] += 1
-            self._last_forward_time = now
-
-    # ── MAVLink 編碼與分片 ────────────────────────────────────────────
-
-    def _send_rtcm(self, mav_obj, data: bytes, seq: int):
-        """
-        對單一載具送出 GPS_RTCM_DATA 必要時做分片
-
-        GPS_RTCM_DATA flags 欄位:
-          bit 0     : 1=fragmented
-          bit 1-2   : fragment_id (0~3)
-          bit 3-7   : sequence number (0~31)
-        """
-        if len(data) <= self.FRAGMENT_MAX_LEN:
-            flags = (seq & 0x1F) << 3
-            padded = data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(data))
-            mav_obj.MAVLink.gps_rtcm_data_send(flags, len(data), padded)
-        else:
-            fragments = [
-                data[i:i + self.FRAGMENT_MAX_LEN]
-                for i in range(0, len(data), self.FRAGMENT_MAX_LEN)
-            ]
-            for frag_id, frag_data in enumerate(fragments):
-                flags = 1 | ((frag_id & 0x03) << 1) | ((seq & 0x1F) << 3)
-                padded = frag_data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(frag_data))
-                mav_obj.MAVLink.gps_rtcm_data_send(flags, len(frag_data), padded)
-
-    # ── 外部介面 ──────────────────────────────────────────────────────
-
-    def set_clock_offset(self, offset_ms: float):
-        """更新時鐘偏移量（毫秒），用於與 RTCM 來源的時間對齊"""
-        self.clock_offset_sec = offset_ms / 1000.0
-
-    def get_counters(self) -> dict:
-        return dict(self.counters)
-
-    def stop(self):
-        """停止轉發"""
-        self.running = False
-        # logger.info("RtcmRelay stopped")
-
-
-# ============================================================================
-#                    FC Network Log Publisher Node
-# ============================================================================
-
-class FcNetworkLogPublisher(Node):
-    """從 Python logging queue 發布結構化 /fc_network/logs 訊息。"""
-
-    def __init__(self):
-        super().__init__('fc_network_log_publisher')
-        qos = QoSProfile(
-            history=HistoryPolicy.KEEP_LAST,
-            depth=200,
-            reliability=ReliabilityPolicy.RELIABLE,
-            durability=DurabilityPolicy.VOLATILE,
-        )
-        self.publisher = self.create_publisher(
-            fcmsg.FcNetworkLog, '/fc_network/logs', qos)
-        self.log_queue = get_ros_log_queue()
-        self.running = True
-        self.timer = self.create_timer(0.05, self._drain_queue)
-
-    def _drain_queue(self):
-        if not self.running:
-            return
-
-        for _ in range(100):
-            try:
-                item = self.log_queue.get_nowait()
-            except queue.Empty:
-                break
-
-            created = max(0.0, float(item.get('created', 0.0)))
-            seconds = int(created)
-            nanoseconds = int((created - seconds) * 1_000_000_000)
-
-            msg = fcmsg.FcNetworkLog()
-            msg.stamp.sec = seconds
-            msg.stamp.nanosec = min(max(nanoseconds, 0), 999_999_999)
-            msg.level = min(max(int(item.get('level', 20)), 0), 255)
-            msg.source = str(item.get('source', 'fc_network'))
-            msg.event_code = str(item.get('event_code', ''))
-            msg.sysid = min(
-                max(int(item.get('sysid', -1)), -2_147_483_648),
-                2_147_483_647
-            )
-            msg.message = str(item.get('message', ''))
-            self.publisher.publish(msg)
-
-    def stop(self):
-        self.running = False
+        logger.info("MavlinkCommandService stopped")
 
 
 # ============================================================================
@@ -1246,31 +968,20 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理四個獨立的 ROS2 Node :
+    管理兩個獨立的 ROS2 Node：
     - VehicleStatusPublisher
     - MavlinkCommandService
-    - RtcmRelay
-    - FcNetworkLogPublisher
     
     提供統一的啟動/停止介面給 mainOrchestrator
-
-    另外 這邊用到 MultiThreadedExecutor 會開出額外的 thread 的特性 
-    使得就算 executor 在跑一些需要等待的方法 
-    常態的 spin_once 也不會被 block (spin_thread 是另一個支線)
     """
     
     def __init__(self):
         self.initialized = False
         self.running = False
-        self._restarting = False
-        self._pending_restart: Optional[str] = None
-        self._restart_lock = threading.Lock()
         
-        # node 實例
+        # 两个 node 實例
         self.status_publisher: Optional[VehicleStatusPublisher] = None
         self.command_service: Optional[MavlinkCommandService] = None
-        self.rtcm_relay: Optional[RtcmRelay] = None
-        self.log_publisher: Optional[FcNetworkLogPublisher] = None
         
         # Executor & Thread
         self.spin_thread: Optional[threading.Thread] = None
@@ -1283,27 +994,18 @@ class fc_ros_manager:
             return False
         
         try:
-            os.environ.setdefault("ROS_DOMAIN_ID", FC_ROS_DOMAIN_ID)
+            # 初始化 ROS2
             if not rclpy.ok():
                 rclpy.init(args=None)
             
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
             self.command_service = MavlinkCommandService()
-            self.rtcm_relay = RtcmRelay()
-            if hasattr(fcmsg, 'FcNetworkLog'):
-                self.log_publisher = FcNetworkLogPublisher()
-            else:
-                logger.warning(
-                    "FcNetworkLog interface unavailable; /fc_network/logs disabled")
     
             # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
             self.executor = MultiThreadedExecutor()
             self.executor.add_node(self.status_publisher)
             self.executor.add_node(self.command_service)
-            self.executor.add_node(self.rtcm_relay)
-            if self.log_publisher:
-                self.executor.add_node(self.log_publisher)
             
             self.initialized = True
             # logger.info("fc_ros_manager initialized")
@@ -1325,12 +1027,6 @@ class fc_ros_manager:
         
         try:
             self.running = True
-
-            self.status_publisher.running = True
-            self.command_service.running = True
-            self.rtcm_relay.running = True
-            if self.log_publisher:
-                self.log_publisher.running = True
             
             self.spin_thread = threading.Thread(
                 target=self._spin_executor,
@@ -1346,105 +1042,24 @@ class fc_ros_manager:
             logger.error(f"Failed to start fc_ros_manager: {e}")
             self.running = False
             return False
-
-    # 給 orchestrator 呼叫的函數
-    def schedule_restart_node(self, node_key: str) -> bool:
-        """由 orchestrator 排程單一 node 重啟（於 spin thread 下一輪執行）"""
-        if node_key not in NODE_KEYS:
-            logger.warning(f"schedule_restart_node: unknown key {node_key!r}")
-            return False
-        if not self.running:
-            logger.warning("schedule_restart_node: fc_ros_manager not running")
-            return False
-        if not self.spin_thread or not self.spin_thread.is_alive():
-            logger.warning("schedule_restart_node: spin thread not alive")
-            return False
-        with self._restart_lock:
-            self._pending_restart = node_key
-        logger.info(f"schedule_restart_node: queued {node_key}")
-        return True
-
+    
     # 循環執行的地方
     def _spin_executor(self):
         """在 thread 中運行的 executor"""
         try:
+            # logger.info("ROS2 executor spinning...")
             while self.running:
                 self.executor.spin_once(timeout_sec=0.1)
-
-                pending = None
-                with self._restart_lock:
-                    if self._pending_restart:
-                        pending = self._pending_restart
-                        self._pending_restart = None
-
-                if pending:
-                    self._restart_node(pending)
-                elif self.command_service and not self._restarting:
-                    self.command_service.return_router()
+                self.command_service.return_router()
         except Exception as e:
             logger.error(f"fc_ros_manager error in spinning executor: {e}")
-        finally:
-            self.running = False
-
-    def _restart_node(self, node_key: str) -> bool:
-        """僅由 spin thread 呼叫 : destroy 並重建單一 node"""
-        old_node = getattr(self, node_key, None)
-        if old_node is None:
-            logger.error(f"_restart_node: {node_key} is None")
-            return False
-
-        # 重置前保留舊有或處理設定
-        saved_topic_intervals = None
-        if node_key == "status_publisher":
-            saved_topic_intervals = dict(old_node.rate_controller.topic_intervals)
-
-        # 如果是 command_service 清除所有等待的 pending 並且清除 return message 
-        if node_key == "command_service":
-            old_node._pending_by_sysid.clear()
-            for sockObj in mo.mavlink_object.mavlinkObjects.values():
-                sockObj.set_return_message_types([])
-
-        try:
-            self._restarting = True
-
-            # 停止運作 移出執行迴圈 銷毀資源
-            old_node.stop()
-            self.executor.remove_node(old_node)
-            old_node.destroy_node()
-
-            # 建立新的 node
-            if node_key == "status_publisher":
-                new_node = VehicleStatusPublisher()
-                if saved_topic_intervals is not None:
-                    new_node.rate_controller.topic_intervals = saved_topic_intervals
-            elif node_key == "command_service":
-                new_node = MavlinkCommandService()
-            elif node_key == "rtcm_relay":
-                new_node = RtcmRelay()
-            else:
-                return False
-
-            # 加回去管理系統中
-            setattr(self, node_key, new_node)
-            self.executor.add_node(new_node)
-            logger.info(f"_restart_node: {node_key} restarted")
-            return True
-        except Exception as e:
-            logger.error(f"_restart_node failed for {node_key}: {e}")
-            return False
-        finally:
-            self._restarting = False
-
+    
     def stop(self):
         """停止 ROS2 nodes"""
-        # if not self.running:
-        #     logger.warning("fc_ros_manager not running")
-        #     return False
-
-        if not self.spin_thread:
-            logger.warning("fc_ros_manager without thread to stop")
+        if not self.running:
+            logger.warning("fc_ros_manager not running")
             return False
-
+        
         try:            
             # 標記停止
             self.running = False
@@ -1454,10 +1069,6 @@ class fc_ros_manager:
                 self.status_publisher.stop()
             if self.command_service:
                 self.command_service.stop()
-            if self.rtcm_relay:
-                self.rtcm_relay.stop()
-            if self.log_publisher:
-                self.log_publisher.stop()
             
             # 等待 spin 執行緒結束
             if self.spin_thread and self.spin_thread.is_alive():
@@ -1482,10 +1093,6 @@ class fc_ros_manager:
                     self.status_publisher.destroy_node()
                 if self.command_service:
                     self.command_service.destroy_node()
-                if self.rtcm_relay:
-                    self.rtcm_relay.destroy_node()
-                if self.log_publisher:
-                    self.log_publisher.destroy_node()
                 
                 # 關閉 ROS2
                 if rclpy.ok():
@@ -1497,22 +1104,14 @@ class fc_ros_manager:
             except Exception as e:
                 logger.error(f"Error during shutdown: {e}")
     
-    def get_status(self):
+    def get_status(self) -> dict:
         return {
             'initialized': self.initialized,
             'running': self.running,
             'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
             'command_service_active': self.command_service is not None,
-            'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
-            'log_publisher_active': self.log_publisher is not None and self.log_publisher.running,
         }
 
-    
-
-
-        
-
-
 
 # ============================================================================
 #                              全域實例
@@ -1538,24 +1137,10 @@ ros2_manager = fc_ros_manager()
 2026.04.07
 1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
 2. 優化 response.ack_result 回傳值的資訊
-3. 新增 _publish_position_ned() 發布剛體座標的訊息
-4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
-
-2026.05.11
-1. 新增 RtcmRelay node - 訂閱 mavros_msgs/RTCM topic 轉發為 MAVLink GPS_RTCM_DATA
-2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
-3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
-
-2026.05.22
-1. initialize() 設定 ROS_DOMAIN_ID ( FC_ROS_DOMAIN_ID )
-2. schedule_restart_node / _restart_node : 手動重啟單一 node (spin thread 內執行）
-3. orchestrator cmd: ("RESTART_ROS_NODE", node_key), node_key 見 NODE_KEYS
-
-2026.06.10
-1. 增加了 _publish_system_diagnostics 與 _publish_status_text 功能
 
 
 TODO 
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
-
+2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
 '''
+

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,7 +5,6 @@ VehicleView - Pure State Container
 """
 
 import os
-from collections import deque
 from typing import Dict, Optional, Any, Tuple
 from dataclasses import dataclass, field
 from enum import Enum
@@ -13,7 +12,7 @@ from enum import Enum
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "1.10"
+MODULE_VER = "1.00"
 
 # ====================== Enums =====================
 
@@ -118,30 +117,6 @@ class VFR:
     timestamp: Optional[float] = None  # 時間戳記
 
 
-@dataclass
-class StatusTextEntry:
-    """飛控狀態文字（來源：MAVLink STATUSTEXT）"""
-    text: str
-    severity: Optional[int] = None
-    timestamp: Optional[float] = None
-
-
-@dataclass
-class SystemDiagnostics:
-    """系統診斷資訊（來源：MAVLink SYS_STATUS，不含電池欄位）"""
-    sensors_install_mask: Optional[int] = None
-    sensors_enabled_mask: Optional[int] = None
-    sensors_health_mask: Optional[int] = None
-    mcuLoad: Optional[int] = None          # 單位 1%
-    busErrorRate: Optional[int] = None     # 單位 0.1%
-    busErrorCount: Optional[int] = None
-    errors_count1: Optional[int] = None
-    errors_count2: Optional[int] = None
-    errors_count3: Optional[int] = None
-    errors_count4: Optional[int] = None
-    timestamp: Optional[float] = None
-
-
 @dataclass
 class ComponentStatus:
     """組件狀態容器"""
@@ -152,8 +127,6 @@ class ComponentStatus:
     ekf: EKF = field(default_factory=EKF)
     gps: GPS = field(default_factory=GPS)
     vfr: VFR = field(default_factory=VFR)
-    sys_diag: SystemDiagnostics = field(default_factory=SystemDiagnostics)
-    status_text_queue: deque = field(default_factory=lambda: deque(maxlen=64))
     
     # 系統狀態
     system_status: Optional[int] = None  # MAV_STATE
@@ -187,7 +160,6 @@ class RFStatus:
 @dataclass
 class SocketInfo:
     """Socket連接資訊"""
-    src64_addr: Optional[bytes] = None # 模組的物理定址
     ip: Optional[str] = None  # IP位址
     port: Optional[int] = None  # 埠號
     local_ip: Optional[str] = None  # 本地IP
@@ -311,7 +283,6 @@ class RFModule:
 
 class VehicleView:
     """
-    最上層
     載具視圖 - 純狀態容器
     
     特點:

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -14,30 +14,20 @@ import signal
 import time
 import threading
 import struct
-from collections import deque
 from enum import Enum, auto
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
-from typing import Callable, Optional
-from typing import List
 
 # # XBee 模組
 # from xbee.frame import APIFrame
 
 # 自定義的 import
-from .mavlinkVehicleView import (
-    vehicle_registry, # 儲存全部物件的地方
-    VehicleView,
-    RFModule,
-    RFModuleType,
-)
 from .utils import RingBuffer, setup_logger
-from .utils import pollStrategy
 
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "2.00"
+MODULE_VER = "0.80"
 
 rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 
@@ -47,8 +37,7 @@ rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
 class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
-    XBEEAPI2AT = auto()    # XBee API-AT 模式
-    XBEEAPI_espv1 = auto()  # XBee API-API 模式 esp v1
+    XBEEAPI2AT = auto()    # XBee API 模式
     NOT_USE = auto()       # 不使用
 
 
@@ -111,28 +100,19 @@ class RawFrameProcessor(FrameProcessor):
         return data
 
 
-class XBeeFrameProcessor_Base(FrameProcessor):
+class XBeeFrameProcessor(FrameProcessor):
     """
     XBee API 協議處理器
-    處理 XBEE API 端口 對應 -> 遠端 XBEE AT Mode
-    For SerialMode.XBEEAPI2AT
 
     職責：
     - XBee API frame 的拆幀 / 組幀
     - 0x90 (RX Packet)   -> 解出 payload 回傳
     - 0x88 (AT Response) -> 轉交 at_handler 處理（若有注入）
-    - 0x8B (TX Status)   -> 目前寫LOG (開發用)
+    - 0x8B (TX Status)   -> 目前忽略
     - 其他 frame type    -> 記 warning 忽略
 
-    若未來要做變化型 XBee (例如 API2 escape mode、不同 addressing)
+    若未來要做變化型 XBee（例如 API2 escape mode、不同 addressing），
     繼承此類並覆寫 _encapsulate / _decapsulate / _try_extract_frame 即可。
-
-    硬體產品系列 (Product Family) : XB3-24
-    晶片世代: Digi XBee 3 代架構（搭載 Silicon Labs EFR32 微控制器）。
-    運作頻段: 2.4 GHz RF。
-    硬體構型: TH (Through-Hole)，標準針腳插孔式引腳設計。
-    運行協議 (Protocol / Function Set): 802.15.4
-    韌體版本 (Firmware Version): 2014 -> 2 (XBee 3 平台) + 0 (802.15.4 協議代碼) + 14 (次要版本號)。
     """
 
     # XBee API frame type
@@ -141,12 +121,6 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     FRAME_TYPE_TX_STATUS   = 0x8B
     FRAME_TYPE_RX_PACKET   = 0x90
 
-    # ADDR16 選項
-    DEST_ADDR16_UNICAST = b'\xFF\xFE'
-    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
-
-    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\x00\x00'
-
     def __init__(self, at_handler: "ATCommandHandler" = None):
         super().__init__()
         self.at_handler = at_handler
@@ -199,28 +173,18 @@ class XBeeFrameProcessor_Base(FrameProcessor):
         return None
 
     def _dispatch_frame(self, frame: bytes) -> bytes:
-        """
-        根據 frame type 分派
-        1. 若是 RX payload 則 return bytes 遞交出去
-        2. 若是 AT command 則 進到相依類別 ATCommandHandler 處理
-        """
+        """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
         frame_type = frame[3]
 
-        if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
-            return self._decapsulate(frame)[0]
+        if frame_type == self.FRAME_TYPE_RX_PACKET:
+            return self._decapsulate(frame)
 
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
             if self.at_handler is not None:
                 self.at_handler.handle_frame(frame)
             return None
 
         if frame_type == self.FRAME_TYPE_TX_STATUS:
-            length = (frame[1] << 8) | frame[2] # for debug
-            logger.info(
-                f"TX Status raw={frame.hex()}, api_len={length}, "
-                f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
-                f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
-            ) # for debug
             return None
 
         logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
@@ -230,8 +194,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
     @staticmethod
     def _encapsulate(
         data: bytes,
-        dest_addr64: bytes = DEST_ADDR64_BRAODCAST,
-        dest_addr16 = DEST_ADDR16_BRAODCAST,
+        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
         frame_id: int = 0x01,
     ) -> bytes:
         """
@@ -239,7 +202,8 @@ class XBeeFrameProcessor_Base(FrameProcessor):
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = XBeeFrameProcessor_Base.FRAME_TYPE_TX_REQUEST
+        frame_type = XBeeFrameProcessor.FRAME_TYPE_TX_REQUEST
+        dest_addr16 = b'\xFF\xFE'
         broadcast_radius = 0x00
         options = 0x00
 
@@ -254,557 +218,7 @@ class XBeeFrameProcessor_Base(FrameProcessor):
         """從 RX Packet (0x90) 取出 payload"""
         length = (frame[1] << 8) | frame[2]
         rf_data_start = 3 + 12
-        payload = frame[rf_data_start:3 + length]
-        senderAddr = frame[4:12]
-        return payload, senderAddr
-
-
-class XBeeFrameProcessor_ESPv1(XBeeFrameProcessor_Base):
-    '''
-    ESP32 封包分類
-    - Mavlink 資料封包 : 
-        - Payload = mavlink [方向] GCS -> UAV (broadcast) 
-        - Payload = mavlink [方向] UAV -> GCS (with src64 Addr)
-    - Discovery 階段
-        - Payload = DISC_HEADER [方向] GCS -> UAV (broadcast)
-        - Payload = HELLO_HEADER + esp_sysid(1) [方向] UAV -> GCS (with src64 Addr)
-    - Poll 階段
-        - Payload = POLL_HEADER + esp_sysid(1) + grant_bytes(2) [方向] GCS -> UAV (with src64 Addr)
-        - Payload = DONE_HEADER + sysid(1) + sent_len(2) + remain_len(2) [方向] UAV -> GCS (with src64 Addr)
-
-    這邊認定 esp_sysid 會等於 mavlink sysid
-
-    硬體產品系列 (Product Family) : XBP9B-DM
-    晶片世代: XBee PRO 900HP 200K
-    運作頻段: 900 MHz RF。
-    運行協議 (Protocol / Function Set): DigiMesh
-    韌體版本 (Firmware Version): 8075
-    '''
-
-    DISC_HEADER = b'DISC'
-    HELLO_HEADER = b'HELO'
-    POLL_HEADER = b'POLL'
-    DONE_HEADER = b'DONE'
-
-    MAX_BYTES_PER_FLUSH = 150
-    MAX_PAYLOAD_PER_FRAME = 80
-    CHUNK_SEND_INTERVAL_SEC = 0.01
-
-    # ADDR16 選項
-    DEST_ADDR16_BRAODCAST = b'\xFF\xFF'
-
-    DEST_ADDR64_BRAODCAST = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
-
-    class Esp32DeviceInfo:
-        def __init__(self, system_id, address_64, last_hello_time):
-            self.system_id = system_id
-            self.address_64 = address_64
-            self.last_hello_time = last_hello_time
-
-            self.remain_bytes = 0 # 剩餘 buffer 量
-            self.last_poll_time = 0.0
-            self.last_done_time = 0.0 # 最後送出Done的時間
-            self.received_len = 0 # 收到封包累計
-
-    def __init__(self, at_handler: "ATCommandHandler" = None):
-        super().__init__(at_handler)
-
-        self.max_discovery_window_ms = 220
-        self.is_discovery_phase = False
-        self.esp32_address_mapping = {}
-
-        self.operator_busy = False
-        self.operator_running = False
-
-        self.serial_writer: Optional[Callable[[bytes], None]] = None
-        self.event_loop: Optional[asyncio.AbstractEventLoop] = None
-        self.serial_baudrate = 115200
-
-        self.gcs_transmit_queue: deque[bytearray] = deque()
-        self.poll_scheduler_state = pollStrategy.PollSchedulerState()
-
-        self.command_pending_event: Optional[asyncio.Event] = None
-        self.poll_done_event: Optional[asyncio.Event] = None
-        self.current_poll_address_64: Optional[bytes] = None
-
-        self.last_discovery_time = 0.0 # 這個是最後做廣播 discovery 的時間  
-        self.last_recieve_mavlink = 0.0 # 這個是最後收到 mavlink payload 時間 為了定義 poll-done 之間不要超時用的
-        self.MAX_mavPack_interval_timeout = 100  # ms，poll 期間 MAVLink/DONE 最大閒置間隔
-        self.discovery_interval_seconds = 30.0 # 每次做 discovery 程序的間隔時間
-        self.device_offline_timeout = self.discovery_interval_seconds * 2 # 遠端沒有回應會被踢出 超時時限
-        self.operator_tick_interval_seconds = 0.03 # 
-        self.guard_milliseconds = 50 # POLL DONE 的保底時間間隔
-
-        self.pending_manual_discovery = False
-        self.pending_manual_poll: Optional[tuple[int, Optional[int]]] = None
-
-    # ---- 注入與設定 ----
-
-    def set_writer(self, writer: Callable[[bytes], None]) -> None:
-        self.serial_writer = writer
-
-    def set_event_loop(self, loop: asyncio.AbstractEventLoop) -> None:
-        self.event_loop = loop
-        self._ensure_async_primitives()
-
-    def set_serial_baudrate(self, baudrate: int) -> None:
-        self.serial_baudrate = baudrate
-
-    def _ensure_async_primitives(self) -> None:
-        if self.command_pending_event is None:
-            self.command_pending_event = asyncio.Event()
-        if self.poll_done_event is None:
-            self.poll_done_event = asyncio.Event()
-
-    # 有急事 叫醒 operator 做下一個 tick
-    def wake_operator(self) -> None:
-        self._ensure_async_primitives()
-        self.command_pending_event.set()
-
-    # ---- 拆幀分派 ----
-
-    def _dispatch_frame(self, frame: bytes) -> Optional[bytes]:
-        frame_type = frame[3]
-
-        if frame_type == self.FRAME_TYPE_RX_PACKET:
-            payload, sender_address_64 = self._decapsulate(frame)
-
-            if payload.startswith(self.HELLO_HEADER) and len(payload) == 5:
-                self.handle_hello_report(payload, sender_address_64)
-                return None
-
-            if payload.startswith(self.DONE_HEADER) and len(payload) == 9:
-                self.handle_done_report(payload, sender_address_64)
-                return None
-
-            remote_device = self.esp32_address_mapping.get(sender_address_64)
-            if remote_device is not None:
-                remote_device.received_len += len(payload)
-                if (self.operator_busy and self.current_poll_address_64 == sender_address_64):
-                    self.last_recieve_mavlink = time.time()
-            return payload
-
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
-            if self.at_handler is not None:
-                self.at_handler.handle_frame(frame)
-            return None
-
-        if frame_type == self.FRAME_TYPE_TX_STATUS:
-            length = (frame[1] << 8) | frame[2]
-            logger.debug(
-                f"TX Status raw={frame.hex()}, api_len={length}, "
-                f"fid=0x{frame[4]:02X}, dest16=0x{(frame[5]<<8)|frame[6]:04X}, "
-                f"retry={frame[7]}, delivery={frame[8]}, discovery={frame[9]}"
-            )
-            return None
-
-        logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
-        return None
-
-    # ---- DISC / POLL 封裝 ----
-
-    def pack_discovery(self) -> bytes:
-        return self._encapsulate(self.DISC_HEADER, frame_id=0x00)
-
-    # 處理每個裝置回傳的 Hello 訊息
-    def handle_hello_report(self, payload: bytes, sender_address_64: bytes) -> None:   
-        system_id = payload[4]
-        remote_device = self.esp32_address_mapping.get(sender_address_64)
-
-        if remote_device is None:
-            self.esp32_address_mapping[sender_address_64] = self.Esp32DeviceInfo(
-                system_id, sender_address_64, time.time()
-            )
-            logger.debug(
-                f"new HELO system_id={system_id}, address_64={sender_address_64.hex()}")
-        elif remote_device.address_64 == sender_address_64:
-            remote_device.last_hello_time = time.time()
-        else:
-            logger.warning(
-                f"SYSID duplicated system_id={system_id}, "
-                f"address_64={sender_address_64.hex()}"
-            )
-        
-        if vehicle:=vehicle_registry.get(system_id):
-            if not vehicle.rf_module:
-                vehicle.rf_module = RFModule(RFModuleType.XBEE)
-
-    def pack_poll(self, target_address_64: bytes, grant_bytes: int = 0) -> Optional[bytes]:
-        remote_device = self.esp32_address_mapping.get(target_address_64)
-        if remote_device is None:
-            return None
-
-        poll_payload = self.POLL_HEADER + struct.pack(
-            '>BH', remote_device.system_id, grant_bytes
-        )
-        remote_device.received_len = 0
-        return self._encapsulate(
-            poll_payload,
-            dest_addr64=remote_device.address_64,
-            dest_addr16=self.DEST_ADDR16_UNICAST,
-            frame_id=0x02,
-        )
-
-    def handle_done_report(self, payload: bytes, sender_address_64: bytes) -> None:
-        remote_device = self.esp32_address_mapping.get(sender_address_64)
-        if remote_device is None:
-            return
-
-        # 這段是有問題的 因為會有整數封包切割問題 以及載具端的 buffer 存量不足 故回傳的資訊量會與要求的不一致
-        # system_id, sent_length, remain_length = struct.unpack('>BHH', payload[4:9])
-        # if sent_length != remote_device.received_len:
-        #     logger.info(
-        #         f"POLL may be missing packets sent={sent_length} "
-        #         f"received={remote_device.received_len} system_id={system_id}"
-        #     )
-
-        # TODO 傳送速率
-        # TODO 累積速率預測
-        remote_device.received_len = 0
-        remote_device.remain_bytes = remain_length
-        remote_device.last_done_time = time.time()
-
-        if (
-            self.current_poll_address_64 is not None
-            and sender_address_64 == self.current_poll_address_64
-            and self.poll_done_event is not None
-        ):
-            self.poll_done_event.set()
-
-    # ---- UDP到Serial 佇列 ----
-
-    def enqueue_gcs_transmit(self, payload: bytes) -> None:
-        if not payload:
-            return
-        self.gcs_transmit_queue.append(bytearray(payload))
-
-    # 只是 show 狀態
-    def get_gcs_queue_byte_count(self) -> int:
-        return sum(len(packet) for packet in self.gcs_transmit_queue)
-
-    # 只是 show 狀態
-    def get_gcs_queue_packet_count(self) -> int:
-        return len(self.gcs_transmit_queue)
-
-    # 把 gcs_transmit_queue 的 mavlink 封包依照大小打包出來
-    def _pop_flush_batch(self, max_bytes: int) -> List[bytes]:
-        if not self.gcs_transmit_queue:
-            return []
-
-        batch: List[bytes] = []
-        total_bytes = 0
-
-        while self.gcs_transmit_queue:
-            next_packet = bytes(self.gcs_transmit_queue[0])
-            packet_length = len(next_packet)
-
-            if not batch:
-                batch.append(self.gcs_transmit_queue.popleft())
-                total_bytes = packet_length
-                if packet_length > max_bytes:
-                    break
-                continue
-
-            if total_bytes + packet_length <= max_bytes:
-                batch.append(self.gcs_transmit_queue.popleft())
-                total_bytes += packet_length
-            else:
-                break
-
-        return batch
-
-    # 把數據封裝好以後 交給 serial_writer 排程送出
-    async def _send_gcs_packet(self, packet: bytes) -> None:
-        # 重複判斷
-        # if self.serial_writer is None:
-        #     return
-
-        sent_offset = 0
-        while sent_offset < len(packet):
-            chunk_end = min(
-                sent_offset + self.MAX_PAYLOAD_PER_FRAME,
-                len(packet),
-            )
-            chunk = packet[sent_offset:chunk_end]
-            sent_offset = chunk_end
-            self.serial_writer(self._encapsulate(chunk))
-            await asyncio.sleep(self.CHUNK_SEND_INTERVAL_SEC)
-
-    # 把上面兩個步驟打包起來 處理從 UDP 來的資訊 丟給 Serial
-    async def flush_gcs_transmit_queue(
-        self,
-        max_bytes: int = MAX_BYTES_PER_FLUSH,
-    ) -> None:
-        if self.serial_writer is None:
-            logger.warning("GCS flush skipped: serial writer not ready")
-            return
-
-        for packet in self._pop_flush_batch(max_bytes):
-            await self._send_gcs_packet(packet)
-
-    # ---- POLL 排程輔助 ----
-
-    # 計算下次要 poll 的對象跟大小
-    def _pick_poll_target(self):
-        poll_devices = [
-            pollStrategy.PollDevice(
-                address_64=address_64,
-                # system_id=device.system_id,
-                remain_bytes=device.remain_bytes,
-                last_done_time=device.last_done_time,
-            )
-            for address_64, device in self.esp32_address_mapping.items()
-        ]
-
-        return pollStrategy.pick_next(
-            poll_devices,
-            self.poll_scheduler_state
-        )
-
-    # 移除長時間沒有 HELLO 或 DONE 的 Dongle
-    def _prune_stale_devices(self) -> None:
-        now = time.time()
-        stale_addresses = [
-            address_64
-            for address_64, device in self.esp32_address_mapping.items()
-            if (now - device.last_hello_time > self.device_offline_timeout) and \
-               (now - device.last_done_time > self.device_offline_timeout)
-        ]
-        for address_64 in stale_addresses:
-            device = self.esp32_address_mapping.pop(address_64, None)
-            if device is not None:
-                logger.info(
-                    f"Removed stale device system_id={device.system_id} "
-                    f"address_64={address_64.hex()}"
-                )
-
-    # 判斷是否進入 discovery 程序
-    def _should_run_discovery(self) -> bool:
-        # 條件1. 目前沒有任何遠端ESP裝置被紀錄 或者 手動啟動
-        if (not self.esp32_address_mapping) or (self.pending_manual_discovery):
-            return True
-        # 條件2. 每個固定週期 會做一次
-        return (
-            time.time() - self.last_discovery_time
-            >= self.discovery_interval_seconds
-        )
-
-    # ---- 手動請求（thread-safe 對外 API）----
-
-    def request_discovery(self) -> bool:
-        """
-        手動觸發 discovery 可從任意 thread 呼叫。
-        實際排程在 serial_manager 的 event loop 內執行。
-        """
-        if self.event_loop is None:
-            logger.warning("ESPv1 request_discovery: event loop not ready")
-            return False
-        if not self.operator_running:
-            logger.warning("ESPv1 request_discovery: operator not running")
-            return False
-        self.event_loop.call_soon_threadsafe(self._apply_request_discovery)
-        return True
-
-    def request_poll(
-        self,
-        target_system_id: int,
-        grant_bytes: Optional[int] = None,
-    ) -> bool:
-        """
-        手動對指定 system_id 排入一輪 POLL 可從任意 thread 呼叫。
-        實際排程在 serial_manager 的 event loop 內執行。
-        """
-        if self.event_loop is None:
-            logger.warning("ESPv1 request_poll: event loop not ready")
-            return False
-        if not self.operator_running:
-            logger.warning("ESPv1 request_poll: operator not running")
-            return False
-        self.event_loop.call_soon_threadsafe(
-            self._apply_request_poll,
-            target_system_id,
-            grant_bytes,
-        )
-        return True
-
-    def _apply_request_discovery(self) -> None:
-        self.pending_manual_discovery = True
-        self.wake_operator()
-
-    def _apply_request_poll(
-        self,
-        target_system_id: int,
-        grant_bytes: Optional[int],
-    ) -> None:
-        self.pending_manual_poll = (target_system_id, grant_bytes)
-        self.wake_operator()
-
-    def get_status_snapshot(self) -> dict:
-        """
-        唯讀狀態快照 for debug
-        從其他 thread 讀取時不保證與 operator 原子一致。
-        """
-        now = time.time()
-        devices = []
-        for address_64, device in self.esp32_address_mapping.items():
-            devices.append({
-                "system_id": device.system_id,
-                "address_64": address_64.hex(),
-                "remain_bytes": device.remain_bytes,
-                "last_hello_age_seconds": now - device.last_hello_time,
-                "last_done_age_seconds": (
-                    now - device.last_done_time if device.last_done_time else None
-                ),
-            })
-        return {
-            "operator_busy": self.operator_busy,
-            "is_discovery_phase": self.is_discovery_phase,
-            "gcs_queue_bytes": self.get_gcs_queue_byte_count(),
-            "gcs_queue_packets": self.get_gcs_queue_packet_count(),
-            "devices": devices,
-        }
-
-    # ---- operator 主循環 ----
-
-    # operator 的運行鐘 有點像是 spin_once
-    async def operator_loop(self) -> None:
-        self._ensure_async_primitives()
-        logger.info("ESPv1 operator loop started")
-
-        while self.operator_running:
-            try:
-                await self._operator_tick() # TODO 最好不要在 while loop 塞 try 想辦法改掉
-            except asyncio.CancelledError:
-                raise
-            except Exception as exc:
-                logger.error(f"ESPv1 operator tick error: {exc}")
-
-            await self._wait_for_next_tick()
-
-        logger.info("ESPv1 operator loop stopped")
-
-    # 安排啥時要醒來做事
-    async def _wait_for_next_tick(self) -> None:
-        self.command_pending_event.clear()
-
-        # 固定時間醒來
-        sleep_task = asyncio.create_task( asyncio.sleep(self.operator_tick_interval_seconds) )
-        # 有"急事"被叫醒
-        wake_task = asyncio.create_task(self.command_pending_event.wait())
-
-        done, pending = await asyncio.wait(
-            {sleep_task, wake_task},
-            return_when=asyncio.FIRST_COMPLETED,
-        )
-
-        for task in pending:
-            task.cancel()
-
-    async def _operator_tick(self) -> None:
-        # 1. 移除沒反應 dongle
-        self._prune_stale_devices()
-
-        # 2. 忙碌時 略過這次循環
-        if self.operator_busy:
-            return
-
-        # 3. (最優先) 處理從 UDP 過來的封包 並且透過 Serial 送出
-        if self.gcs_transmit_queue:
-            await self.flush_gcs_transmit_queue()
-            return
-
-        # 4. 檢測要不要跑 discovery 程序
-        if self._should_run_discovery():
-            await self._run_discovery()
-            return
-
-        # 5. POLL 程序 (若有手動先執行 若無自動則策略決策)
-        if self.pending_manual_poll is not None:
-            target_system_id, grant_bytes = self.pending_manual_poll
-            self.pending_manual_poll = None
-            target_address = self._find_address_by_system_id(target_system_id)
-        else:
-            target_address, grant_bytes = self._pick_poll_target()
-
-        if target_address is None:
-            return
-        await self._run_one_poll(target_address, grant_bytes if grant_bytes is not None else 0)
-
-    def _find_address_by_system_id(self, target_system_id: int) -> Optional[bytes]:
-        for address_64, device in self.esp32_address_mapping.items():
-            if device.system_id == target_system_id:
-                return address_64
-        return None
-
-    # discovery 程序 
-    async def _run_discovery(self) -> None:
-        if self.serial_writer is None:
-            self.pending_manual_discovery = False
-            return
-
-        self.operator_busy = True
-        self.is_discovery_phase = True
-        
-        self.serial_writer(self.pack_discovery())
-        self.last_discovery_time = time.time()
-        await asyncio.sleep(self.max_discovery_window_ms / 1000.0)
-
-        self.operator_busy = False
-        self.is_discovery_phase = False
-        self.pending_manual_discovery = False
-
-    async def _wait_poll_done_with_idle_timeout(self) -> bool:
-        idle_timeout_sec = self.MAX_mavPack_interval_timeout / 1000.0
-        poll_tick = min(0.02, idle_timeout_sec / 2)
-
-        while not self.poll_done_event.is_set():
-            if time.time() - self.last_recieve_mavlink >= idle_timeout_sec:
-                return False
-            try:
-                await asyncio.wait_for(
-                    self.poll_done_event.wait(),
-                    timeout=poll_tick,
-                )
-            except asyncio.TimeoutError:
-                continue
-        return True
-
-    # poll 程序
-    async def _run_one_poll(self, target_address_64: bytes, grant_bytes: int) -> None:
-        if self.serial_writer is None:
-            return
-
-        # 這邊把要求封包最小量放在36 是讓載具端正好可以回傳一個有簽章的 HEARTBEAT 封包的大小
-        # 算是 "探測封包" 這樣讓系統可以更快速的知道載具端殘餘的資料量
-        grant_bytes = max(36, min(int(grant_bytes), 65535))
-
-        poll_frame = self.pack_poll(target_address_64, grant_bytes)
-        if poll_frame is None:
-            return
-
-        self._ensure_async_primitives()
-        self.operator_busy = True
-        self.current_poll_address_64 = target_address_64
-        self.poll_done_event.clear()
-        self.serial_writer(poll_frame)
-        self.last_recieve_mavlink = time.time()
-
-        try:
-            completed = await self._wait_poll_done_with_idle_timeout()
-            if not completed:
-                logger.warning(
-                    f"POLL timeout address_64={target_address_64.hex()} "
-                    f"grant_bytes={grant_bytes}"
-                )
-        finally:
-            self.operator_busy = False
-            self.current_poll_address_64 = None
-            await asyncio.sleep(self.guard_milliseconds / 1000.0)
-
-    def stop_operator(self) -> None:
-        self.operator_running = False
-        self.gcs_transmit_queue.clear()
-        self.wake_operator()
+        return frame[rf_data_start:3 + length]
 
 
 # ====================== Dongle Command Handler =====================
@@ -866,21 +280,21 @@ class ATCommandHandler:
     # ---- 接收端 ----
     def handle_frame(self, frame: bytes) -> None:
         """
-        接收一整個 AT Response frame:
+        接收一整個 AT Response frame：
         1. 解析成 ATResponse
         2. 推進 rx_module_ack 供其他模組消費
         3. 本地 dispatch 給對應的 _handle_xxx
         """
-        parsed_at_ack = self._parse(frame)
-        if parsed_at_ack is None:
+        parsed = self._parse(frame)
+        if parsed is None:
             return
 
-        if not rx_module_ack.put(parsed_at_ack):
+        if not rx_module_ack.put(parsed):
             logger.warning(
-                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed_at_ack.command!r}"
+                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed.command!r}"
             )
 
-        self._dispatch(parsed_at_ack)
+        self._dispatch(parsed)
 
     @staticmethod
     def _parse(frame: bytes) -> ATResponse:
@@ -911,30 +325,29 @@ class ATCommandHandler:
 
         handler = self.handlers.get(response.command)
         if handler:
-            handler(response)
+            handler(response.data)
         else:
             logger.debug(
                 f"[{self.serial_port}] 未處理的 AT 指令: "
                 f"{response.command.decode()}"
             )
 
-    def _handle_rssi(self, response: ATResponse):
+    def _handle_rssi(self, data: bytes):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
-
-        # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
-        logger.debug(f"[{self.serial_port}] RSSI = -{response.data[0]} dBm") # dev
-
-
-
-
-    def _handle_serial_high(self, response: ATResponse):
+        pass
+        # if data:
+            # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+            # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
+        
+    def _handle_serial_high(self, data: bytes):
         """處理 SH (Serial Number High)"""
         pass
 
-    def _handle_serial_low(self, response: ATResponse):
+    def _handle_serial_low(self, data: bytes):
         """處理 SL (Serial Number Low)"""
         pass
 
+
 # ================ Serial UDP Socket Object ==============
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
@@ -953,13 +366,9 @@ class SerialHandler(asyncio.Protocol):
         if self.serial_mode == SerialMode.STRAIGHT:
             return RawFrameProcessor()
 
-        elif self.serial_mode == SerialMode.XBEEAPI2AT:
+        if self.serial_mode == SerialMode.XBEEAPI2AT:
             at_handler = ATCommandHandler(self.serial_port_str)
-            return XBeeFrameProcessor_Base(at_handler=at_handler)
-
-        elif self.serial_mode == SerialMode.XBEEAPI_espv1:
-            at_handler = ATCommandHandler(self.serial_port_str)
-            return XBeeFrameProcessor_ESPv1(at_handler=at_handler)
+            return XBeeFrameProcessor(at_handler=at_handler)
 
         logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
         return RawFrameProcessor()
@@ -972,13 +381,6 @@ class SerialHandler(asyncio.Protocol):
         if self.serial_mode == SerialMode.XBEEAPI2AT:
             self.processor.at_handler.set_writer(self.transport.write)
 
-        elif self.serial_mode == SerialMode.XBEEAPI_espv1:
-            if isinstance(self.processor, XBeeFrameProcessor_ESPv1):
-                self.processor.set_writer(self.transport.write)
-                self.processor.set_event_loop(asyncio.get_running_loop())
-                if self.processor.at_handler is not None:
-                    self.processor.at_handler.set_writer(self.transport.write)
-
         if hasattr(self.udp_handler, 'set_serial_handler'):
             self.udp_handler.set_serial_handler(self)
         logger.debug(f"Serial port {self.serial_port_str} connected")
@@ -1021,15 +423,11 @@ class UDPHandler(asyncio.DatagramProtocol):
         if not self.serial_handler:
             logger.warning("Serial handler not set, dropping UDP packet")
             return
-
-        if self.serial_mode == SerialMode.XBEEAPI_espv1:
-            processor = self.serial_handler.processor
-            # if isinstance(processor, XBeeFrameProcessor_ESPv1): # 預設唯一綁定 少一個 if 多一點效率
-            processor.enqueue_gcs_transmit(data)
-            processor.wake_operator()
-            return
-
+        
+        # 使用 processor 封裝數據
         processed_data = self.serial_handler.processor.process_outgoing(data)
+        
+        # 發送到串口
         self.serial_handler.transport.write(processed_data)
 
 
@@ -1050,7 +448,6 @@ class serial_manager:
             self.protocol = None
             self.udp_handler = None
             self.serial_handler = None
-            self.operator_task = None
 
     def __init__(self):
         self.thread = None
@@ -1191,16 +588,6 @@ class serial_manager:
             
             logger.debug(f"Serial connection created for {serial_port}")
 
-            if serial_mode == SerialMode.XBEEAPI_espv1:
-                processor = serial_obj.serial_handler.processor
-                if isinstance(processor, XBeeFrameProcessor_ESPv1):
-                    processor.set_serial_baudrate(baudrate)
-                    processor.operator_running = True
-                    serial_obj.operator_task = asyncio.create_task(
-                        processor.operator_loop()
-                    )
-                    logger.debug(f"ESPv1 operator task started for {serial_port}")
-
             # 將 serial_object 加入管理列表
             serial_id = self.serial_count + 1
             self.serial_objects[serial_id] = serial_obj
@@ -1253,19 +640,7 @@ class serial_manager:
         
         try:
             serial_obj = self.serial_objects[serial_id]
-
-            if serial_obj.serial_mode == SerialMode.XBEEAPI_espv1:
-                processor = serial_obj.serial_handler.processor
-                if isinstance(processor, XBeeFrameProcessor_ESPv1):
-                    processor.stop_operator()
-                if serial_obj.operator_task is not None:
-                    serial_obj.operator_task.cancel()
-                    try:
-                        await serial_obj.operator_task
-                    except asyncio.CancelledError:
-                        pass
-                    serial_obj.operator_task = None
-
+            
             # 關閉 UDP transport
             if hasattr(serial_obj, 'transport') and serial_obj.transport:
                 serial_obj.transport.close()
@@ -1293,7 +668,7 @@ class serial_manager:
 
     def send_at_command(self, serial_id, request: ATRequest) -> bool:
         """
-        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令 (thread-safe)
+        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令（thread-safe）
         - serial_id: create_serial_link 取得的編號
         - request: ATRequest 物件，攜帶 command / parameter / frame_id
         回傳是否成功排進事件圈
@@ -1306,7 +681,6 @@ class serial_manager:
             logger.error(f"Serial object {serial_id} not found")
             return False
 
-        # TODO 這邊的防呆 應該可以不用 if 有空再改
         serial_obj = self.serial_objects[serial_id]
         if serial_obj.serial_mode != SerialMode.XBEEAPI2AT:
             logger.error(
@@ -1319,18 +693,6 @@ class serial_manager:
         self.loop.call_soon_threadsafe(at_handler.send_command, request)
         return True
 
-    def get_espv1_processor(self, serial_id: int) -> Optional[XBeeFrameProcessor_ESPv1]:
-        """依 serial_id 取得 ESPv1 processor 不存在或模式不符回傳 None。"""
-        if serial_id not in self.serial_objects:
-            return None
-        serial_obj = self.serial_objects[serial_id]
-        if serial_obj.serial_mode != SerialMode.XBEEAPI_espv1:
-            return None
-        processor = serial_obj.serial_handler.processor
-        if not isinstance(processor, XBeeFrameProcessor_ESPv1):
-            return None
-        return processor
-
     @staticmethod
     def check_serial_port(serial_port, baudrate):
         """檢查串口是否存在與可用"""
@@ -1368,105 +730,42 @@ if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
 
-    # # 測試項一 
     # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
     # SERIAL_BAUDRATE = 115200
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
-    # 測試項二
-    print("運行 測試項二")
     SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
     SERIAL_BAUDRATE = 115200
     UDP_REMOTE_PORT = 14561
     sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-    serial_id = 1
-    device_sys_id = 10
 
     linked_serial = sm.get_serial_link()
-    print(f"連結完成 : {linked_serial}. 等待兩秒")
+    print(linked_serial)
 
     # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
-    time.sleep(2)
-    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=device_sys_id)
-    print(f"手動送出 DB AT Command:")
-    for i in range(20):
+    time.sleep(5)
+    rssi_request = ATRequest(command=b'DB', parameter=b'', frame_id=0x52)
+    for i in range(60):
         sm.send_at_command(1, rssi_request)
         time.sleep(1)
 
     sm.remove_serial_link(1)
-    time.sleep(2)
+    time.sleep(3)
     sm.shutdown()
-    print("結束運行")
 
-    # # 測試項三
-    # SERIAL_PORT = '/dev/ttyUSB0'
-    # SERIAL_BAUDRATE = 115200
-    # UDP_REMOTE_PORT = 14561
-    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI_espv1)
 
-    # time.sleep(2)  # 等 serial 連線與 operator 啟動
-
-    # serial_id = 1
-    # processor = sm.get_espv1_processor(serial_id)
-    # if processor is not None:
-    #     processor.request_discovery()
-    #     processor.request_poll(target_system_id=1)
-    #     processor.request_poll(target_system_id=1, grant_bytes=200)
-    #     print(processor.get_status_snapshot())
-    #     print(processor.get_gcs_queue_byte_count())
-
-    # sm.remove_serial_link(serial_id)
-    # time.sleep(30)
-    # sm.shutdown()
 
 '''
 ================= 改版記錄 ============================
 2026.4.20
 1. XBeeFrameHandler 結構移除
-2. XBeeFrameProcessor_Base 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
-3. XBeeFrameProcessor_Base 新增 _try_extract_frame 處理被可能截斷的 UART 封包
-4. XBeeFrameProcessor_Base 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
+2. XBeeFrameProcessor 新增 _encapsulate, _decapsulate 編碼解碼 xbee 封包的功能 (原來在 XBeeFrameHandler 中)
+3. XBeeFrameProcessor 新增 _try_extract_frame 處理被可能截斷的 UART 封包
+4. XBeeFrameProcessor 新增 _dispatch_frame 分配封包到 UDP 或者 Dongle Command Handler 
 5. ATCommandHandler 新增 _parse 去拆解 0x88 AT Command Response
 6. ATCommandHandler 新增 _dispatch 把拆解的結果 分配到 _handle_XXX
 7. ATCommandHandler 新增各項 _handle_XXX (未實作)
 
-2026.06.15
-1. 修改 XBeeFrameProcessor_Base _decapsulate 使其解出發送端 dongle 的 src64 地址
-2. 新增 XBeeFrameProcessor_ESPv1 這個類別繼承 XBeeFrameProcessor_Base 
-3. 新增 XBeeFrameProcessor_ESPv1 組合 DISC 跟 POLL 訊息 處理 HELO 跟 DONE 的能力
-4. DISC 自動化與手動
-5. POLL 的自動化
-
-2026.06.16
-1. 移除 DongleCommandHandler_ESPv1
-2. ESPv1 對外 API（request_discovery / request_poll）移至 XBeeFrameProcessor_ESPv1，thread-safe
-3. serial_manager 僅保留 get_espv1_processor(serial_id) lookup
-
-
-註解1 : FRAME_TYPE_TX_STATUS 的對應解說 (我不喜歡程式塞太多為了顯示而顯示的東西 錯誤碼自己下來看)
-    TX_DELIVERY_STATUS
-        0x00: "Success",
-        0x01: "No ACK received",
-        0x02: "CCA failure",
-        0x15: "Invalid destination endpoint",
-        0x21: "Network ACK failure",
-        0x22: "Not joined to network",
-        0x23: "Self-addressed",
-        0x24: "Address not found",
-        0x25: "Route not found",
-        0x26: "Broadcast source relay",
-        0x27: "Insufficient data",
-        0x28: "TX buffered",
-        0x32: "Invalid send flag",
-        0x74: "Resource error",
-
-    TX_DISCOVERY_STATUS
-        0x00: "No discovery overhead",
-        0x01: "Address discovery",
-        0x02: "Route discovery",
-        0x03: "Address and route discovery",
-
-
 '''
 

src/fc_network_adapter/fc_network_adapter/utils/__init__.py

@@ -2,6 +2,6 @@
 共用工具模組
 """
 from .ringBuffer import RingBuffer
-from .theLogger import get_ros_log_queue, setup_logger
+from .theLogger import setup_logger
 
-__all__ = ['RingBuffer', 'get_ros_log_queue', 'setup_logger']
+__all__ = ['RingBuffer', 'setup_logger']

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -1,48 +0,0 @@
-"""
-POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
-
-不依賴 serialManager，僅接受裝置快照與排程狀態。
-"""
-
-from dataclasses import dataclass
-from typing import List
-
-
-@dataclass(frozen=True)
-class PollDevice:
-    """從 esp32AddrMapping 抽出的唯讀快照。"""
-    address_64: bytes
-    # system_id: int
-    remain_bytes: int
-    last_done_time: float
-
-
-@dataclass
-class PollSchedulerState:
-    """每條 serial link 一份，保存 round-robin 索引。"""
-    round_robin_index: int = 0
-
-
-def pick_next(
-    devices: List[PollDevice],
-    scheduler_state: PollSchedulerState
-):
-    """
-    選下一個 POLL 目標。
-
-    回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
-    """
-    if not devices:
-        return None, 0
-
-    device_count = len(devices)
-    selected_index = scheduler_state.round_robin_index % device_count
-    selected_device = devices[selected_index]
-    scheduler_state.round_robin_index += 1
-
-    grant_bytes = 0
-    if selected_device.remain_bytes > 0:
-        grant_bytes = min( 65535, max(0, selected_device.remain_bytes))
-
-    return selected_device.address_64, grant_bytes
-

src/fc_network_adapter/fc_network_adapter/utils/theLogger.py

@@ -1,47 +1,9 @@
 import logging
 import os
-import queue
 from logging.handlers import TimedRotatingFileHandler
 
 # 全域 Logger 實例
 _global_logger = None
-_ros_log_queue = queue.Queue(maxsize=2000)
-
-
-class RosTopicQueueHandler(logging.Handler):
-    """將 FC Network log record 非阻塞地排入 ROS publisher 佇列。"""
-
-    def emit(self, record: logging.LogRecord) -> None:
-        try:
-            sysid = int(getattr(record, 'sysid', -1))
-        except (TypeError, ValueError):
-            sysid = -1
-
-        item = {
-            'created': float(record.created),
-            'level': int(record.levelno),
-            'source': str(record.name),
-            'event_code': str(getattr(record, 'event_code', '')),
-            'sysid': sysid,
-            'message': record.getMessage(),
-        }
-        try:
-            _ros_log_queue.put_nowait(item)
-        except queue.Full:
-            # GUI 不應反過來拖慢通訊；滿載時淘汰最舊一筆。
-            try:
-                _ros_log_queue.get_nowait()
-            except queue.Empty:
-                return
-            try:
-                _ros_log_queue.put_nowait(item)
-            except queue.Full:
-                pass
-
-
-def get_ros_log_queue():
-    """供 ROS publisher node 取得共用、thread-safe 的紀錄佇列。"""
-    return _ros_log_queue
 
 def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger:
     global _global_logger
@@ -75,10 +37,6 @@ def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> loggi
         console_handler.setFormatter(formatter)
         _global_logger.addHandler(console_handler)
 
-        ros_queue_handler = RosTopicQueueHandler()
-        ros_queue_handler.setLevel(logging.INFO)
-        _global_logger.addHandler(ros_queue_handler)
-
     # 為每個模組建立子 Logger，並設定名稱
     module_logger = _global_logger.getChild(name)
     module_logger.name = name  # 修改子 Logger 的名稱，僅保留子 Logger 名稱

src/fc_network_adapter/package.xml

@@ -7,13 +7,6 @@
   <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
   <license>TODO: License declaration</license>
 
-  <exec_depend>fc_interfaces</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>mavros_msgs</exec_depend>
-
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -17,7 +17,6 @@ import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
 import mavros_msgs.msg
-import fc_interfaces.msg
 
 # 專案 imports
 from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -40,7 +39,7 @@ class TestSubscriber(Node):
         
         self.sysid = sysid
         self.received_messages = {
-            'position_gnss': [],
+            'position': [],
             'attitude': [],
             'velocity': [],
             'battery': [],
@@ -61,9 +60,9 @@ class TestSubscriber(Node):
         
         # Position
         self.create_subscription(
-            fc_interfaces.msg.GnssRaw,
-            f'{base_topic}/position_gnss',
-            lambda msg: self._on_message('position_gnss', msg),
+            sensor_msgs.msg.NavSatFix,
+            f'{base_topic}/position',
+            lambda msg: self._on_message('position', msg),
             10
         )
         
@@ -122,7 +121,7 @@ class TestSubscriber(Node):
     
     def _format_msg(self, topic_name: str, msg) -> str:
         """格式化消息以便顯示"""
-        if topic_name == 'position_gnss':
+        if topic_name == 'position':
             return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
         elif topic_name == 'attitude':
             return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -265,7 +264,7 @@ def test_basic_publishing():
     # 檢查收到的消息
     print("\n--- 消息統計 ---")
     total_messages = 0
-    for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+    for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
         count = test_sub.get_message_count(topic)
         total_messages += count
         print(f"  {topic:15s}: {count:3d} 條消息")
@@ -305,7 +304,7 @@ def test_frequency_control():
     # 修改頻率設定
     publisher_node = ros2_manager.status_publisher
     publisher_node.rate_controller.topic_intervals = {
-        'position_gnss': 1.5,   
+        'position': 1.5,   
         'attitude': 1.0,    
         'velocity': 1.0,     
         'battery': 1.0,   
@@ -330,7 +329,7 @@ def test_frequency_control():
     print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
     
     print("2Hz Topics (預期 ~6 條):")
-    for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
+    for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
     for topic in ['battery', 'mode', 'summary']:

src/fc_network_apps/longCommand.py

@@ -14,7 +14,6 @@ from fc_interfaces.srv import MavCommandLong
 COMMAND_DO_SET_MODE = 176
 COMMAND_NAV_LAND = 21
 COMMAND_NAV_TAKEOFF = 22
-COMMAND_PREFLIGHT_REBOOT_SHUTDOWN = 246
 COMMAND_COMPONENT_ARM_DISARM = 400
 
 DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
@@ -192,28 +191,6 @@ class CommandLongClient(Node):
             timeout_sec=timeout_sec,
         )
 
-    def reboot_autopilot(
-        self,
-        *,
-        target_sysid: int,
-        target_compid: int = 0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    ) -> CommandLongResult:
-        return self._send_command_long(
-            target_sysid=target_sysid,
-            target_compid=target_compid,
-            command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
-            confirmation=0,
-            param1=1.0,
-            param2=0.0,
-            param3=0.0,
-            param4=0.0,
-            param5=0.0,
-            param6=0.0,
-            param7=0.0,
-            timeout_sec=timeout_sec,
-        )
-
     # ============================================================================
     # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
     # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
@@ -376,29 +353,6 @@ class CommandLongClient(Node):
             timeout_sec=timeout_sec,
         )
 
-    async def reboot_autopilot_async(
-        self,
-        *,
-        target_sysid: int,
-        target_compid: int = 0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    ) -> CommandLongResult:
-        """非阻塞 async 版本的 autopilot reboot."""
-        return await self._send_command_long_async(
-            target_sysid=target_sysid,
-            target_compid=target_compid,
-            command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
-            confirmation=0,
-            param1=1.0,
-            param2=0.0,
-            param3=0.0,
-            param4=0.0,
-            param5=0.0,
-            param6=0.0,
-            param7=0.0,
-            timeout_sec=timeout_sec,
-        )
-
     async def land_async(
         self,
         *,

src/fc_network_apps/vehicleDiagShow.py

@@ -1,544 +0,0 @@
-#!/usr/bin/env python3
-"""Standalone curses TUI for fc_network vehicle diagnostics (sys_diags, status_text, summary)."""
-
-from __future__ import annotations
-
-import curses
-import json
-import re
-import sys
-import threading
-import time
-from dataclasses import dataclass, field
-from typing import Dict, List, Optional, Tuple
-
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-
-from fc_interfaces.msg import SystemDiagnosticsRaw
-
-VEHICLE_TOPIC_RE = re.compile(
-    r'^/fc_network/vehicle/(sys\d+)/(sys_diags|status_text|summary)$'
-)
-STATUS_TEXT_RE = re.compile(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$')
-
-STATUS_TEXT_TTL_SEC = 30.0
-STALE_SEC = 3.0
-RESCAN_SEC = 2.0
-OFFLINE_SEC = 10.0
-DRAW_HZ = 5.0
-
-# MAV_SYS_STATUS_SENSOR bit labels (MAVLink common)
-SENSOR_BITS: Tuple[Tuple[int, str], ...] = (
-    (268435456, 'PreArm'),
-    (1, 'Gyro'),
-    (2, 'Accel'),
-    (4, 'Mag'),
-    (8, 'Baro'),
-    (16, 'DiffPress'),
-    (32, 'GPS'),
-    (64, 'OptFlow'),
-    (128, 'Vision'),
-    (256, 'Laser'),
-    (512, 'ExtGT'),
-    (1024, 'AngRate'),
-    (2048, 'AttStab'),
-    (4096, 'YawPos'),
-    (8192, 'AltCtrl'),
-    (16384, 'XYCtrl'),
-    (32768, 'Motor'),
-    (65536, 'RC'),
-    (131072, 'Gyro2'),
-    (262144, 'Accel2'),
-    (524288, 'Mag2'),
-    (1048576, 'Geofence'),
-    (2097152, 'AHRS'),
-    (4194304, 'Terrain'),
-    (8388608, 'RevMotor'),
-    (16777216, 'Log'),
-    (33554432, 'Batt'),
-    (67108864, 'Prox'),
-    (134217728, 'Satcom'),
-    (536870912, 'ObsAvoid'),
-    (1073741824, 'Propulsion'),
-)
-
-MAV_STATE_LABELS = {
-    0: 'UNINIT',
-    1: 'BOOT',
-    2: 'CAL',
-    3: 'STANDBY',
-    4: 'ACTIVE',
-    5: 'CRITICAL',
-    6: 'EMERGENCY',
-    7: 'OFF',
-    8: 'TERM',
-}
-
-SEVERITY_LABELS = {
-    0: 'EMERG',
-    1: 'ALERT',
-    2: 'CRIT',
-    3: 'ERROR',
-    4: 'WARN',
-    5: 'NOTICE',
-    6: 'INFO',
-    7: 'DEBUG',
-}
-
-
-@dataclass
-class StatusTextEntry:
-    received_at: float
-    severity: int
-    text: str
-    raw: str
-    sys_key: str
-
-
-@dataclass
-class VehicleDiagState:
-    sys_key: str
-    sysid: int
-    summary: Optional[dict] = None
-    diag: Optional[SystemDiagnosticsRaw] = None
-    status_entries: List[StatusTextEntry] = field(default_factory=list)
-    last_summary_ts: float = 0.0
-    last_diag_ts: float = 0.0
-    last_topic_seen_ts: float = 0.0
-    offline: bool = False
-
-
-def sys_key_to_id(sys_key: str) -> int:
-    return int(sys_key.replace('sys', '', 1))
-
-
-def purge_status_entries(entries: List[StatusTextEntry], now: float, ttl: float = STATUS_TEXT_TTL_SEC) -> None:
-    cutoff = now - ttl
-    entries[:] = [e for e in entries if e.received_at >= cutoff]
-
-
-def parse_status_text(raw: str, sys_key: str, received_at: float) -> StatusTextEntry:
-    match = STATUS_TEXT_RE.match(raw.strip())
-    if match:
-        severity = int(match.group(2))
-        text = match.group(3)
-    else:
-        severity = -1
-        text = raw
-    return StatusTextEntry(
-        received_at=received_at,
-        severity=severity,
-        text=text,
-        raw=raw,
-        sys_key=sys_key,
-    )
-
-
-def decode_sensors(install: int, enabled: int, health: int) -> List[Tuple[str, str]]:
-    """Return list of (label, status) where status is OK / FAIL / OFF."""
-    items: List[Tuple[str, str]] = []
-    for bit, label in SENSOR_BITS:
-        if not (install & bit):
-            continue
-        if not (enabled & bit):
-            items.append((label, 'OFF'))
-        elif health & bit:
-            items.append((label, 'OK'))
-        else:
-            items.append((label, 'FAIL'))
-    return items
-
-
-def format_age(seconds: float) -> str:
-    if seconds < 0:
-        return 'never'
-    if seconds < 1.0:
-        return f'{seconds * 1000:.0f}ms'
-    if seconds < 60.0:
-        return f'{seconds:.1f}s'
-    return f'{seconds / 60:.1f}m'
-
-
-def truncate(text: str, width: int) -> str:
-    if width <= 0:
-        return ''
-    if len(text) <= width:
-        return text
-    if width <= 3:
-        return text[:width]
-    return text[: width - 3] + '...'
-
-
-class VehicleDiagNode(Node):
-    """ROS2 node: discover vehicles and subscribe to diagnostic topics."""
-
-    def __init__(self) -> None:
-        super().__init__('vehicle_diag_show')
-        self._lock = threading.Lock()
-        self._vehicles: Dict[str, VehicleDiagState] = {}
-        self._subs: Dict[str, Dict[str, object]] = {}
-        self._scan_timer = self.create_timer(RESCAN_SEC, self._rescan_topics)
-        self._rescan_topics()
-
-    def snapshot(self) -> Dict[str, VehicleDiagState]:
-        now = time.monotonic()
-        with self._lock:
-            for state in self._vehicles.values():
-                purge_status_entries(state.status_entries, now)
-                if state.last_topic_seen_ts > 0:
-                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
-            return {k: state for k, state in self._vehicles.items()}
-
-    def _get_or_create_state(self, sys_key: str) -> VehicleDiagState:
-        if sys_key not in self._vehicles:
-            self._vehicles[sys_key] = VehicleDiagState(
-                sys_key=sys_key,
-                sysid=sys_key_to_id(sys_key),
-            )
-        return self._vehicles[sys_key]
-
-    def _rescan_topics(self) -> None:
-        now = time.monotonic()
-        found: set = set()
-        for topic_name, _ in self.get_topic_names_and_types():
-            match = VEHICLE_TOPIC_RE.match(topic_name)
-            if not match:
-                continue
-            sys_key = match.group(1)
-            found.add(sys_key)
-            with self._lock:
-                state = self._get_or_create_state(sys_key)
-                state.last_topic_seen_ts = now
-                state.offline = False
-            self._ensure_subscriptions(sys_key)
-
-        with self._lock:
-            for sys_key, state in self._vehicles.items():
-                if sys_key not in found and state.last_topic_seen_ts > 0:
-                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
-
-    def _ensure_subscriptions(self, sys_key: str) -> None:
-        if sys_key in self._subs:
-            return
-        base = f'/fc_network/vehicle/{sys_key}'
-        subs = {
-            'summary': self.create_subscription(
-                String,
-                f'{base}/summary',
-                lambda msg, sk=sys_key: self._on_summary(sk, msg),
-                10,
-            ),
-            'sys_diags': self.create_subscription(
-                SystemDiagnosticsRaw,
-                f'{base}/sys_diags',
-                lambda msg, sk=sys_key: self._on_sys_diags(sk, msg),
-                10,
-            ),
-            'status_text': self.create_subscription(
-                String,
-                f'{base}/status_text',
-                lambda msg, sk=sys_key: self._on_status_text(sk, msg),
-                10,
-            ),
-        }
-        with self._lock:
-            self._subs[sys_key] = subs
-
-    def _on_summary(self, sys_key: str, msg: String) -> None:
-        now = time.monotonic()
-        try:
-            summary = json.loads(msg.data)
-        except json.JSONDecodeError:
-            summary = {'raw': msg.data}
-        with self._lock:
-            state = self._get_or_create_state(sys_key)
-            state.summary = summary
-            state.last_summary_ts = now
-            state.last_topic_seen_ts = now
-
-    def _on_sys_diags(self, sys_key: str, msg: SystemDiagnosticsRaw) -> None:
-        now = time.monotonic()
-        with self._lock:
-            state = self._get_or_create_state(sys_key)
-            state.diag = msg
-            state.last_diag_ts = now
-            state.last_topic_seen_ts = now
-
-    def _on_status_text(self, sys_key: str, msg: String) -> None:
-        now = time.monotonic()
-        entry = parse_status_text(msg.data, sys_key, now)
-        with self._lock:
-            state = self._get_or_create_state(sys_key)
-            state.status_entries.append(entry)
-            purge_status_entries(state.status_entries, now)
-            state.last_topic_seen_ts = now
-
-
-class DiagCursesApp:
-    """Curses front-end for vehicle diagnostics."""
-
-    def __init__(self, node: VehicleDiagNode) -> None:
-        self._node = node
-        self._running = True
-
-    def run(self) -> None:
-        curses.wrapper(self._main)
-
-    def _init_colors(self) -> None:
-        curses.start_color()
-        curses.use_default_colors()
-        curses.init_pair(1, curses.COLOR_GREEN, -1)    # OK / DISARMED / INFO
-        curses.init_pair(2, curses.COLOR_RED, -1)        # FAIL / ARMED / ERROR+
-        curses.init_pair(3, curses.COLOR_YELLOW, -1)   # WARN / OFF
-        curses.init_pair(4, curses.COLOR_CYAN, -1)     # header / INFO
-        curses.init_pair(5, curses.COLOR_WHITE, -1)    # dim / offline
-        curses.init_pair(6, curses.COLOR_MAGENTA, -1)  # NOTICE
-
-    def _main(self, stdscr) -> None:
-        curses.curs_set(0)
-        stdscr.nodelay(True)
-        stdscr.keypad(True)
-        self._init_colors()
-
-        while self._running and rclpy.ok():
-            try:
-                self._draw(stdscr)
-            except curses.error:
-                pass
-            key = stdscr.getch()
-            if key in (ord('q'), ord('Q'), 27):
-                self._running = False
-            time.sleep(1.0 / DRAW_HZ)
-
-    def _draw(self, stdscr) -> None:
-        stdscr.erase()
-        height, width = stdscr.getmaxyx()
-        now = time.monotonic()
-        vehicles = self._node.snapshot()
-        sorted_keys = sorted(vehicles.keys(), key=sys_key_to_id)
-
-        header = f' Vehicle Diagnostics Monitor  vehicles:{len(sorted_keys)}  Q:quit '
-        stdscr.attron(curses.color_pair(4) | curses.A_BOLD)
-        stdscr.addstr(0, 0, truncate(header.ljust(width - 1), width - 1))
-        stdscr.attroff(curses.color_pair(4) | curses.A_BOLD)
-
-        row = 1
-        if not sorted_keys:
-            msg = ' Waiting for vehicles... (need /fc_network/vehicle/sysN/* topics) '
-            if row < height - 1:
-                stdscr.addstr(row, max(0, (width - len(msg)) // 2), truncate(msg, width - 1))
-            row += 2
-
-        vehicle_rows_end = max(row, height - 8)
-        for sys_key in sorted_keys:
-            if row >= vehicle_rows_end:
-                break
-            state = vehicles[sys_key]
-            row = self._draw_vehicle_block(stdscr, row, width, state, now)
-            if row < height - 1:
-                try:
-                    stdscr.addstr(row, 0, truncate('-' * (width - 1), width - 1))
-                except curses.error:
-                    pass
-                row += 1
-
-        log_title = ' Status Text (last 30s) '
-        if row < height - 1:
-            stdscr.attron(curses.color_pair(4))
-            stdscr.addstr(row, 0, truncate(log_title.ljust(width - 1), width - 1))
-            stdscr.attroff(curses.color_pair(4))
-            row += 1
-
-        all_entries: List[StatusTextEntry] = []
-        for sys_key in sorted_keys:
-            all_entries.extend(vehicles[sys_key].status_entries)
-        all_entries.sort(key=lambda e: e.received_at)
-
-        log_rows = height - row - 1
-        if log_rows <= 0:
-            stdscr.refresh()
-            return
-
-        if not all_entries:
-            empty = ' (no messages in last 30s) '
-            if row < height - 1:
-                stdscr.addstr(row, 0, truncate(empty, width - 1))
-        else:
-            visible = all_entries[-log_rows:]
-            for entry in visible:
-                if row >= height - 1:
-                    break
-                self._draw_status_line(stdscr, row, width, entry)
-                row += 1
-
-        stdscr.refresh()
-
-    def _draw_vehicle_block(
-        self,
-        stdscr,
-        row: int,
-        width: int,
-        state: VehicleDiagState,
-        now: float,
-    ) -> int:
-        summary = state.summary or {}
-        mode = summary.get('mode_name', 'UNKNOWN')
-        armed = summary.get('armed', False)
-        conn = summary.get('connection_type', '?')
-        socket_id = summary.get('socket_id', -1)
-        mav_status = summary.get('mav_status', 0)
-        mav_label = MAV_STATE_LABELS.get(int(mav_status), str(mav_status))
-
-        last_update = max(state.last_summary_ts, state.last_diag_ts)
-        age_sec = (now - last_update) if last_update > 0 else -1.0
-        stale = age_sec >= 0 and age_sec > STALE_SEC
-
-        armed_str = 'ARMED' if armed else 'DISARMED'
-        flags = []
-        if state.offline:
-            flags.append('OFFLINE')
-        if stale and not state.offline:
-            flags.append('STALE')
-        flag_str = f"  [{' '.join(flags)}]" if flags else ''
-
-        line1 = (
-            f' {state.sys_key} (MAV {state.sysid})  {mode}  {armed_str}  '
-            f'{conn}  socket:{socket_id}  {mav_label}  '
-            f'updated {format_age(age_sec)}{flag_str}'
-        )
-        if row < stdscr.getmaxyx()[0] - 1:
-            if state.offline:
-                stdscr.attron(curses.color_pair(5))
-                stdscr.addstr(row, 0, truncate(line1, width - 1))
-                stdscr.attroff(curses.color_pair(5))
-            else:
-                stdscr.addstr(row, 0, truncate(line1, width - 1))
-                armed_pos = line1.find(armed_str)
-                if armed_pos >= 0:
-                    pair = curses.color_pair(2) if armed else curses.color_pair(1)
-                    stdscr.attron(pair | curses.A_BOLD)
-                    stdscr.addstr(row, armed_pos, armed_str)
-                    stdscr.attroff(pair | curses.A_BOLD)
-        row += 1
-
-        if state.diag is not None and row < stdscr.getmaxyx()[0] - 1:
-            load_pct = state.diag.mcu_load / 10.0
-            drop_pct = state.diag.bus_error_rate / 10.0
-            line2 = f' MCU {load_pct:.1f}%  CommDrop {drop_pct:.1f}%'
-            stdscr.addstr(row, 0, truncate(line2, width - 1))
-            row += 1
-
-            sensors = decode_sensors(
-                state.diag.sensors_install_mask,
-                state.diag.sensors_enabled_mask,
-                state.diag.sensors_health_mask,
-            )
-            row = self._draw_sensor_line(stdscr, row, width, sensors)
-        elif row < stdscr.getmaxyx()[0] - 1:
-            stdscr.addstr(row, 0, truncate(' (no sys_diags yet)', width - 1))
-            row += 1
-
-        return row
-
-    def _draw_sensor_line(
-        self,
-        stdscr,
-        row: int,
-        width: int,
-        sensors: List[Tuple[str, str]],
-    ) -> int:
-        max_row, _ = stdscr.getmaxyx()
-        if not sensors:
-            if row < max_row - 1:
-                stdscr.addstr(row, 0, truncate(' Sensors: (none installed)', width - 1))
-            return row + 1
-
-        lines: List[List[Tuple[str, str]]] = [[]]
-        line_len = len(' Sensors: ')
-        for label, status in sensors:
-            token = f'{label}:{status}  '
-            if lines[-1] and line_len + len(token) >= width - 1:
-                lines.append([])
-                line_len = 2
-            lines[-1].append((label, status))
-            line_len += len(token)
-
-        for idx, line_items in enumerate(lines):
-            if row >= max_row - 1:
-                break
-            prefix = ' Sensors: ' if idx == 0 else '  '
-            col = 0
-            stdscr.addstr(row, col, prefix)
-            col += len(prefix)
-            for label, status in line_items:
-                token = f'{label}:{status}  '
-                if col + len(token) >= width - 1:
-                    stdscr.addstr(row, col, truncate('...', width - col - 1))
-                    return row + 1
-                pair = curses.A_NORMAL
-                if status == 'OK':
-                    pair = curses.color_pair(1)
-                elif status == 'FAIL':
-                    pair = curses.color_pair(2) | curses.A_BOLD
-                elif status == 'OFF':
-                    pair = curses.color_pair(3)
-                stdscr.attron(pair)
-                stdscr.addstr(row, col, token)
-                stdscr.attroff(pair)
-                col += len(token)
-            row += 1
-        return row
-
-    def _draw_status_line(
-        self,
-        stdscr,
-        row: int,
-        width: int,
-        entry: StatusTextEntry,
-    ) -> None:
-        sev_label = SEVERITY_LABELS.get(entry.severity, str(entry.severity))
-        prefix = f'[{entry.sys_key:5}] [{entry.severity} {sev_label:5}] '
-        body = truncate(entry.text, max(0, width - len(prefix) - 2))
-        line = prefix + body
-
-        pair = curses.A_NORMAL
-        if entry.severity <= 3:
-            pair = curses.color_pair(2)
-        elif entry.severity == 4:
-            pair = curses.color_pair(3)
-        elif entry.severity == 5:
-            pair = curses.color_pair(6)
-        elif entry.severity == 6:
-            pair = curses.color_pair(1)
-        elif entry.severity == 7:
-            pair = curses.color_pair(5)
-
-        stdscr.attron(pair)
-        stdscr.addstr(row, 0, truncate(line, width - 1))
-        stdscr.attroff(pair)
-
-
-def main() -> int:
-    if not sys.stdout.isatty():
-        print('vehicleDiagShow requires an interactive terminal (TTY).', file=sys.stderr)
-        return 1
-
-    rclpy.init()
-    node = VehicleDiagNode()
-    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
-    spin_thread.start()
-
-    try:
-        DiagCursesApp(node).run()
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-        spin_thread.join(timeout=2.0)
-
-    return 0
-
-
-if __name__ == '__main__':
-    sys.exit(main())

src/fc_network_module/fc_network_module/ntrip_client.py

@@ -1,256 +0,0 @@
-import socket
-import base64
-import threading
-import time
-from typing import Optional, Tuple
-
-import rclpy
-from rclpy.node import Node
-from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
-from mavros_msgs.msg import RTCM
-
-# TODO: ROS_DOMAIN_ID 要補一下
-
-class GGA_stream():
-
-    @classmethod
-    def nmea_checksum(cls, body: str) -> str:
-        """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
-        value = 0
-        for ch in body:
-            value ^= ord(ch)
-        return f"{value:02X}"
-
-    @classmethod
-    def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> Tuple[str, str]:
-        """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
-        if is_latitude:
-            hemi = "N" if deg >= 0 else "S"
-            deg_width = 2
-        else:
-            hemi = "E" if deg >= 0 else "W"
-            deg_width = 3
-        deg = abs(deg)
-        d = int(deg)
-        m = (deg - d) * 60.0
-        return f"{d:0{deg_width}d}{m:07.4f}", hemi
-
-    @classmethod
-    def build_gga_sentence(cls, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
-        """
-        組一筆 $GPGGA 句子（含 checksum) 回傳 bytes 可直接 sock.sendall。
-
-        lat_deg / lon_deg : 十進位經緯度(北、東為正)。
-        測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
-        """
-        utc = time.gmtime()
-        t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
-
-        lat_dm, ns = cls.decimal_to_nmea_dm(lat_deg, is_latitude=True)
-        lon_dm, ew = cls.decimal_to_nmea_dm(lon_deg, is_latitude=False)
-
-        # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
-        body = (
-            f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
-            f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
-        )
-        sentence = f"${body}*{cls.nmea_checksum(body)}\r\n"
-        return sentence.encode("ascii")
-
-    @classmethod
-    def send_gga(cls, sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
-        """送出 GGA，回傳可讀句子供列印。"""
-        payload = cls.build_gga_sentence(lat_deg, lon_deg, alt_m)
-        sock.sendall(payload)
-        return payload.decode("ascii").strip()
-
-class NtripClientNode(Node):
-    """連線 NTRIP caster, 接收 RTCM3 資料流並發布至 ROS2 topic """
-
-    RECONNECT_BASE_SEC = 10.0
-    RECONNECT_MAX_SEC = 60.0
-
-    def __init__(self):
-        super().__init__('ntrip_client')
-
-        self.declare_parameter('host', 'rtk2go.com')
-        self.declare_parameter('port', 2101)
-        self.declare_parameter('mountpoint', '')
-        self.declare_parameter('username', 'template@mail.com')
-        self.declare_parameter('password', '')
-
-        self.declare_parameter('gga_on', False)
-        self.declare_parameter('gga_lat', None)
-        self.declare_parameter('gga_lon', None)
-        self.declare_parameter('gga_alt_m', 100)
-        self.declare_parameter('gga_interval_sec', 60)
-
-        rtcm_qos = QoSProfile(
-            history=HistoryPolicy.KEEP_LAST,
-            depth=1,
-            reliability=ReliabilityPolicy.BEST_EFFORT,
-            durability=DurabilityPolicy.VOLATILE,
-        )
-        self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
-
-        self._stop_event = threading.Event()
-        self._sock_lock = threading.Lock()
-        self._active_sock: Optional[socket.socket] = None
-        self._thread = threading.Thread(target=self._receive_loop, daemon=True)
-        self._thread.start()
-
-        self.get_logger().info('NtripClientNode 已啟動')
-
-    # ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
-
-    def _request_stop(self):
-        self._stop_event.set()
-        with self._sock_lock:
-            if self._active_sock is not None:
-                try:
-                    self._active_sock.close()
-                except OSError:
-                    pass
-
-    def _receive_loop(self):
-        backoff = self.RECONNECT_BASE_SEC
-
-        host = self.get_parameter('host').value
-        port = self.get_parameter('port').value
-        mountpoint = self.get_parameter('mountpoint').value
-        user = self.get_parameter('username').value
-        pwd = self.get_parameter('password').value
-
-        if not mountpoint:
-            self.get_logger().error('mountpoint 參數未設定...')
-            return
-
-        while not self._stop_event.is_set():
-
-            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-            sock.settimeout(10)
-            with self._sock_lock:
-                self._active_sock = sock
-
-            self.gga_lat = self.get_parameter('gga_lat').value
-            self.gga_lon = self.get_parameter('gga_lon').value
-            self.gga_alt_m = self.get_parameter('gga_alt_m').value
-            self.gga_interval_sec = self.get_parameter('gga_interval_sec').value
-            self.gga_on = self.get_parameter('gga_on').value
-            self.gga_on = self.gga_lat is not None and self.gga_lon is not None and self.gga_on
-
-
-            try:
-                self.get_logger().info(f'正在連線 {host}:{port}/{mountpoint} ...')
-                sock.connect((host, port))
-
-                auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
-                request = (
-                    f'GET /{mountpoint} HTTP/1.0\r\n'
-                    f'User-Agent: NTRIP PythonClient\r\n'
-                    f'Authorization: Basic {auth}\r\n'
-                    f'Connection: close\r\n\r\n'
-                )
-                sock.sendall(request.encode())
-
-                resp = sock.recv(4096)
-                if b'ICY 200 OK' not in resp:
-                    self.get_logger().error(
-                        f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
-                    )
-                    raise ConnectionError('handshake failed')
-
-                self.get_logger().info(f'已成功連接至 {mountpoint}')
-
-
-                backoff = self.RECONNECT_BASE_SEC
-
-                self._read_stream(sock)
-
-            except Exception as e:
-                if self._stop_event.is_set():
-                    break
-                self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
-                if self._stop_event.wait(timeout=backoff):
-                    break
-                backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
-            finally:
-                with self._sock_lock:
-                    if self._active_sock is sock:
-                        self._active_sock = None
-                try:
-                    sock.close()
-                except OSError:
-                    pass
-
-    def _read_stream(self, sock: socket.socket):
-        buf = b''
-        last_gga_time = 0.0
-
-        while not self._stop_event.is_set():
-            now = time.time()
-            if self.gga_on and (now - last_gga_time >= self.gga_interval_sec):
-                line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m)
-                self.get_logger().info(f"[GGA] {line}")
-                last_gga_time = now
-
-            try:
-                chunk = sock.recv(4096)
-            except socket.timeout:
-                if self._stop_event.is_set():
-                    break
-                continue
-            except OSError:
-                if self._stop_event.is_set():
-                    break
-                raise
-            if not chunk:
-                if self._stop_event.is_set():
-                    break
-                raise ConnectionError("stream ended. Empty Chunk")
-            buf += chunk
-
-            while len(buf) >= 6:
-                if buf[0] != 0xD3:
-                    buf = buf[1:]
-                    continue
-
-                payload_len = ((buf[1] & 0x03) << 8) | buf[2]
-                frame_len = payload_len + 6  # header(3) + payload + CRC(3)
-
-                if len(buf) < frame_len:
-                    break
-
-                packet = buf[:frame_len]
-                buf = buf[frame_len:]
-
-                self._publish_rtcm(packet)
-
-    def _publish_rtcm(self, raw_packet: bytes):
-        msg = RTCM()
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.data = list(raw_packet)
-        self.publisher_.publish(msg)
-
-    def destroy_node(self):
-        self._request_stop()
-        if self._thread.is_alive():
-            self._thread.join(timeout=5.0)
-            if self._thread.is_alive():
-                self.get_logger().warn('receive thread 未在時限內結束')
-        super().destroy_node()
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = NtripClientNode()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/fc_network_module/package.xml

@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>fc_network_module</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
-  <license>TODO: License declaration</license>
-
-  <depend>rclpy</depend>
-  <depend>mavros_msgs</depend>
-  <depend>std_msgs</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>

src/fc_network_module/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/fc_network_module
-[install]
-install_scripts=$base/lib/fc_network_module

src/fc_network_module/setup.py

@@ -1,26 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'fc_network_module'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='picars',
-    maintainer_email='chiyu1468@hotmail.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-            'ntrip_client = fc_network_module.ntrip_client:main',
-        ],
-    },
-)

src/fc_network_module/test/test_copyright.py

@@ -1,25 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-# Remove the `skip` decorator once the source file(s) have a copyright header
-@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found errors'

src/fc_network_module/test/test_flake8.py

@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
-    rc, errors = main_with_errors(argv=[])
-    assert rc == 0, \
-        'Found %d code style errors / warnings:\n' % len(errors) + \
-        '\n'.join(errors)

src/fc_network_module/test/test_pep257.py

@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
-    rc = main(argv=['.', 'test'])
-    assert rc == 0, 'Found code style errors / warnings'

src/unitdev02/unitdev02/devnote.txt

@@ -1,25 +1,21 @@
 備選需要的功能
+ - serail 對於 telemetry 的支援
  - serial_manager.serial_object.transport 這些變數可能不需要
 
 不用動
  - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async
+ - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) 
 
 這一步
- RTK
-   跟 RTK2go 抓取列表 Done
-   從特定 mount point 得到數據 Done
-   做一個 ros2 service 接到數據並包裝 Done
-   RTK 實機測試
- 
+ 研究 ros2 service
 
 下一步
-  重啟做好了 但是還沒自動化
 
 下下一步
-  整合 xbee api-api 功能 
+ 
 
 後面
-
+ rssi 資訊提取s
 
 
 
@@ -31,24 +27,8 @@ python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode
 
-python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com
-
-python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart
-
-python3 -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart -p gga_on:=True -p gga_lat:=24.155792000 -p gga_lon:=120.630679000 -p gga_interval_sec:=60
-
-watch GPS_RAW_INT
-
-export ROS_DOMAIN_ID=1
-ros2 daemon stop
-ros2 daemon start
-
 ros2 topic list
 ros2 topic echo /fc_network/vehicle/sys3/battery
-ros2 topic echo hz /fc_network/vehicle/sys3/battery
-
-ros2 topic bw /fc_network/vehicle/sys3/attitude
-ros2 topic hz /fc_network/vehicle/sys3/attitude
 
 
 /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
@@ -59,7 +39,7 @@ ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5
 ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
 
 ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
-ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 10, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
 
 ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
 
@@ -68,4 +48,10 @@ sudo tcpdump -i lo 'udp dst port 14561' -X
 sudo tcpdump -i lo 'udp dst port 14550' -X -vv
 sudo tcpdump -i lo -X udp port 14550
 
-
+幾個要討論的
+1. 專案文件中 .mat 檔案是幹嘛的?
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
+3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.

src/unitdev04/esp32-final.py

@@ -1,151 +0,0 @@
-from machine import UART
-import time
-import struct
-import gc
-
-# ================= 設定區 =================
-FC_BAUDRATE = 115200 
-XB_BAUDRATE = 115200
-DEST_64 = b'\x00\x13\xA2\x00\x42\x5B\x9D\xC8' 
-XBEE_MAX_PAYLOAD = 100
-MY_SYSID = 15  # 第二台就改成 10，第三台 15
-POLL_MAGIC = b'POLL'
-
-tx_buf = bytearray()
-MAX_BUF_SIZE = 6144 # 保留安全餘裕的緩衝區大小
-# =========================================
-
-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)
-
-rx_buf = bytearray()
-
-def get_checksum(data):
-    return 0xFF - (sum(data) & 0xFF)
-
-def send_to_xbee_chunked(payload):
-    total_len = len(payload)
-    sent_len = 0
-    
-    while sent_len < total_len:
-        end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
-        chunk = payload[sent_len : end_len]
-        sent_len = end_len
-        
-        frame_content = bytearray()
-        frame_content.append(0x10)       
-        frame_content.append(0x00) # 0x00 代表不要求 XBee ACK，極大加快發送速度       
-        frame_content.extend(DEST_64)    
-        frame_content.extend(b'\xFF\xFE')
-        frame_content.append(0x00)       
-        frame_content.append(0x00)       
-        frame_content.extend(chunk)      
-        
-        length = len(frame_content)
-        checksum = get_checksum(frame_content)
-        
-        packet = bytearray()
-        packet.append(0x7E)
-        packet.append((length >> 8) & 0xFF)
-        packet.append(length & 0xFF)
-        packet.extend(frame_content)
-        packet.append(checksum)
-        
-        uart_xb.write(packet)
-        # 僅需極短暫延遲，讓硬體發送緩衝區消化即可
-        time.sleep_ms(1)
-
-def process_xbee_buffer():
-    global rx_buf, tx_buf
-    while True:
-        start_pos = rx_buf.find(b'\x7E')
-        if start_pos == -1:
-            rx_buf = bytearray() # 避免用 clear()
-            return 
-            
-        if start_pos > 0:
-            rx_buf = rx_buf[start_pos:]
-            
-        if len(rx_buf) < 3: return
-            
-        pkt_len = (rx_buf[1] << 8) | rx_buf[2]
-        total_len = pkt_len + 4 
-        
-        if pkt_len > 300:
-            rx_buf = rx_buf[1:]
-            continue
-        
-        if len(rx_buf) < total_len: return 
-            
-        packet = rx_buf[3 : 3+pkt_len]
-        checksum_recv = rx_buf[3+pkt_len]
-        
-        if get_checksum(packet) == checksum_recv:
-            if packet[0] == 0x90:
-                real_data = packet[12:]
-                if len(real_data) == 5 and real_data.startswith(POLL_MAGIC):
-                    if real_data[4] == MY_SYSID:
-                        flush_tx_buffer()
-                else:
-                    uart_fc.write(real_data)
-            
-            rx_buf = rx_buf[total_len:]
-        else:
-            rx_buf = rx_buf[1:]
-
-def flush_tx_buffer():
-    global tx_buf
-    if len(tx_buf) == 0:
-        return
-        
-    # 單次最大發送量設為 1200 bytes，避免在空中飛行超過時槽導致碰撞
-    send_limit = min(len(tx_buf), 1200) 
-    
-    cut_idx = send_limit
-    if send_limit < len(tx_buf):
-        last_fd_pos = tx_buf[:send_limit].rfind(b'\xFD')
-        if last_fd_pos > 0:
-            cut_idx = last_fd_pos 
-            
-    data_to_send = tx_buf[:cut_idx]
-    tx_buf = tx_buf[cut_idx:] 
-    
-    send_to_xbee_chunked(data_to_send)
-
-# 初始清理
-gc.collect()
-
-while True:
-    try:
-        # FC -> 緩衝區
-        if uart_fc.any():
-            data = uart_fc.read(250)
-            if data:
-                tx_buf.extend(data)
-                
-                # 聰明的溫和防爆機制 (砍掉舊的 2/3，保留最新 1/3)
-                if len(tx_buf) > MAX_BUF_SIZE:
-                    safe_cut = tx_buf.find(b'\xFD', (MAX_BUF_SIZE // 3) * 2)
-                    if safe_cut != -1:
-                        tx_buf = tx_buf[safe_cut:] 
-                    else:
-                        tx_buf = tx_buf[(MAX_BUF_SIZE // 2):] 
-
-        # XBee -> FC
-        if uart_xb.any():
-            chunk = uart_xb.read()
-            if chunk:
-                rx_buf.extend(chunk)
-            process_xbee_buffer()
-            
-        time.sleep_ms(1) 
-        
-    except MemoryError:
-        # 發生 OOM 時的「斷尾求生」法，強制捨棄舊變數以釋放空間，防止死機
-        tx_buf = bytearray()
-        rx_buf = bytearray()
-        gc.collect()
-        time.sleep_ms(10)
-        
-    except Exception as e:
-        time.sleep_ms(2)

src/unitdev04/esp32.py

@@ -0,0 +1,635 @@
+from machine import UART
+import time
+import struct
+import gc
+
+# =========================================================
+# ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge
+# Packet-size TDMA + DONE + SYSID / GCS address 自動學習版
+#
+# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
+# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
+#
+# 重要：
+# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。
+# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。
+# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING；
+#   之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。
+#
+# 封包格式：
+# GCS -> UAV:
+#   DISC
+#   POLL + target_sysid(1) + grant_bytes(2)
+#   COMM / RTK broadcast downlink bytes
+#
+# UAV -> GCS:
+#   HELO + sysid(1)
+#   DONE + sysid(1) + sent_len(2) + remain_len(2)
+#
+# 注意：
+# - HELO 不帶 remain。
+# - DONE 保留 remain。
+# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
+# - DEST_64 不寫死，由 DISC 的 XBee 0x90 src64 自動學。
+# - POLL 只負責 TDMA 授權；COMM / RTK 直接 forward。
+# =========================================================
+
+
+# ================= 設定區 =================
+FC_BAUDRATE = 115200
+XB_BAUDRATE = 115200
+
+DEST_64 = None
+MY_SYSID = None
+
+SYSID_LEARN_CONFIRM_COUNT = 3
+_sysid_candidate = None
+_sysid_candidate_count = 0
+
+# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。
+# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。
+DISC_PENDING = False
+
+XBEE_MAX_PAYLOAD = 100
+
+# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。
+DISC_MAGIC = b'DISC'
+
+# UAV -> GCS hello：
+# HELO + sysid(1)
+HELLO_MAGIC = b'HELO'
+
+HELLO_MIN_INTERVAL_MS = 1000
+_last_hello_ms = 0
+
+# 保留此變數只作狀態記錄；本版 DISC 會 force=True，所以不會被 HELLO_SENT 擋掉。
+HELLO_SENT = False
+
+POLL_MAGIC = b'POLL'
+DEFAULT_GRANT_BYTES = 600
+
+DONE_MAGIC = b'DONE'
+
+MAX_BUF_SIZE = 6144
+READ_FC_BYTES = 250
+
+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(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。
+    """
+    global DEST_64
+    global HELLO_SENT
+    global DISC_PENDING
+
+    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
+        DISC_PENDING = False
+        return True
+
+    return False
+
+
+def build_api_tx_frame(payload):
+    """
+    建立 XBee API frame type 0x10，把 payload 送到 DEST_64。
+    若 DEST_64 尚未學到，回傳 None。
+    """
+    if DEST_64 is None:
+        return None
+
+    frame_content = bytearray()
+    frame_content.append(0x10)
+    frame_content.append(0x00)
+    frame_content.extend(DEST_64)
+    frame_content.extend(b'\xFF\xFE')
+    frame_content.append(0x00)
+    frame_content.append(0x00)
+    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 送出。
+    """
+    if DEST_64 is None:
+        return False
+
+    total_len = len(payload)
+    sent_len = 0
+
+    while sent_len < total_len:
+        end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
+        chunk = payload[sent_len:end_len]
+        sent_len = end_len
+
+        pkt = build_api_tx_frame(chunk)
+        if pkt is None:
+            return False
+
+        uart_xb.write(pkt)
+        time.sleep_ms(1)
+
+    return True
+
+
+# =========================================================
+# HELO / DONE 回報
+# =========================================================
+
+def send_hello_report(force=False):
+    """
+    UAV 回報自己存在：
+        HELO + MY_SYSID
+
+    本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。
+    """
+    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 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):
+    """
+    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)
+    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):
+    pos_fe = buf.find(b'\xFE')
+    pos_fd = buf.find(b'\xFD')
+
+    if pos_fe == -1:
+        return pos_fd
+    if pos_fd == -1:
+        return pos_fe
+    return pos_fe if pos_fe < pos_fd else pos_fd
+
+
+def mavlink_frame_length(buf, start_idx):
+    if start_idx >= len(buf):
+        return None
+
+    magic = buf[start_idx]
+
+    if magic == 0xFE:
+        if len(buf) - start_idx < 2:
+            return None
+        payload_len = buf[start_idx + 1]
+        total_len = payload_len + 8
+        if len(buf) - start_idx < total_len:
+            return None
+        return total_len
+
+    if magic == 0xFD:
+        if len(buf) - start_idx < 3:
+            return None
+        payload_len = buf[start_idx + 1]
+        incompat_flags = buf[start_idx + 2]
+        signed = (incompat_flags & 0x01) != 0
+        total_len = payload_len + 12 + (13 if signed else 0)
+        if len(buf) - start_idx < total_len:
+            return None
+        return total_len
+
+    return None
+
+
+def get_mavlink_sysid(buf, start_idx):
+    if start_idx >= len(buf):
+        return None
+
+    magic = buf[start_idx]
+
+    if magic == 0xFE:
+        if len(buf) - start_idx >= 6:
+            return buf[start_idx + 3]
+
+    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，建議重新上電 ESP32，讓 MY_SYSID 重新學習。
+    """
+    global MY_SYSID
+    global _sysid_candidate
+    global _sysid_candidate_count
+    global HELLO_SENT
+    global tx_buf
+
+    # 已經學到 SYSID 後，不再掃 tx_buf，減少 ESP32 運算量
+    if MY_SYSID is not None:
+        return
+
+    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 _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
+                HELLO_SENT = False
+                return
+
+        idx = start + frame_len
+        checked += 1
+
+
+# =========================================================
+# tx_buf MAVLink frame 取出與裁切
+# =========================================================
+
+def pop_mavlink_frames_by_quota(quota_bytes):
+    global tx_buf
+
+    if quota_bytes <= 0 or len(tx_buf) == 0:
+        return b''
+
+    start = find_first_mavlink_magic(tx_buf)
+    if start == -1:
+        tx_buf = bytearray()
+        return b''
+
+    if start > 0:
+        tx_buf = tx_buf[start:]
+
+    out = bytearray()
+
+    while len(tx_buf) > 0:
+        if tx_buf[0] not in (0xFE, 0xFD):
+            start = find_first_mavlink_magic(tx_buf)
+            if start == -1:
+                tx_buf = bytearray()
+                break
+            tx_buf = tx_buf[start:]
+
+        frame_len = mavlink_frame_length(tx_buf, 0)
+        if frame_len is None:
+            break
+
+        if len(out) > 0 and (len(out) + frame_len) > quota_bytes:
+            break
+
+        if len(out) == 0 and frame_len > quota_bytes:
+            out.extend(tx_buf[:frame_len])
+            tx_buf = tx_buf[frame_len:]
+            break
+
+        out.extend(tx_buf[:frame_len])
+        tx_buf = tx_buf[frame_len:]
+
+        if len(out) >= quota_bytes:
+            break
+
+    return bytes(out)
+
+
+def trim_tx_buffer_if_needed():
+    global tx_buf
+
+    if len(tx_buf) <= MAX_BUF_SIZE:
+        return
+
+    target_size = MAX_BUF_SIZE // 2
+
+    while len(tx_buf) > target_size:
+        start = find_first_mavlink_magic(tx_buf)
+        if start == -1:
+            tx_buf = bytearray()
+            return
+
+        if start > 0:
+            tx_buf = tx_buf[start:]
+            continue
+
+        frame_len = mavlink_frame_length(tx_buf, 0)
+        if frame_len is None:
+            if len(tx_buf) > target_size:
+                tx_buf = tx_buf[-target_size:]
+            return
+
+        tx_buf = tx_buf[frame_len:]
+
+
+def flush_tx_buffer(grant_bytes):
+    global tx_buf
+
+    # DEST_64 未學到時不能送資料，保留防護避免先 pop tx_buf 造成資料遺失
+    if DEST_64 is None:
+        return
+
+    if grant_bytes <= 0:
+        send_done_report(0, len(tx_buf))
+        return
+
+    data_to_send = pop_mavlink_frames_by_quota(grant_bytes)
+    sent_len = len(data_to_send)
+
+    if sent_len > 0:
+        send_to_xbee_chunked(data_to_send)
+
+    send_done_report(sent_len, len(tx_buf))
+
+
+# =========================================================
+# GCS control payload 解析
+# =========================================================
+
+def parse_poll_payload(real_data):
+    if not real_data.startswith(POLL_MAGIC):
+        return None, None
+
+    if len(real_data) == 5:
+        return real_data[4], DEFAULT_GRANT_BYTES
+
+    if len(real_data) == 7:
+        target_sysid = real_data[4]
+        grant_bytes = (real_data[5] << 8) | real_data[6]
+        return target_sysid, grant_bytes
+
+    return None, None
+
+
+def is_discovery_payload(real_data):
+    return real_data.startswith(DISC_MAGIC)
+
+
+# =========================================================
+# XBee API RX parser
+# =========================================================
+
+def process_xbee_buffer():
+    global rx_buf
+    global DISC_PENDING
+
+    while True:
+        start_pos = rx_buf.find(b'\x7E')
+        if start_pos == -1:
+            rx_buf = bytearray()
+            return
+
+        if start_pos > 0:
+            rx_buf = rx_buf[start_pos:]
+
+        if len(rx_buf) < 3:
+            return
+
+        pkt_len = (rx_buf[1] << 8) | rx_buf[2]
+        total_len = pkt_len + 4
+
+        if pkt_len > 300:
+            rx_buf = rx_buf[1:]
+            continue
+
+        if len(rx_buf) < total_len:
+            return
+
+        packet = rx_buf[3:3 + pkt_len]
+        checksum_recv = rx_buf[3 + pkt_len]
+
+        if get_checksum(packet) == checksum_recv:
+            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:]
+
+                    # -------------------------------------------------
+                    # DISC：本版唯一 discovery 封包。
+                    # 功能等同原本 DSCF：收到後強制回 HELO。
+                    # -------------------------------------------------
+                    if is_discovery_payload(real_data):
+                        learn_dest64_from_src64(src64)
+                        learn_my_sysid_from_tx_buf()
+
+                        if MY_SYSID is not None and DEST_64 is not None:
+                            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:
+                        # -------------------------------------------------
+                        # 判斷是不是 POLL
+                        # -------------------------------------------------
+                        target_sysid, grant_bytes = parse_poll_payload(real_data)
+
+                        if target_sysid is not None:
+                            # POLL 只負責 TDMA 授權；DISC 已負責 GCS address / SYSID 學習
+                            if MY_SYSID is not None and target_sysid == MY_SYSID:
+                                flush_tx_buffer(grant_bytes)
+
+                        else:
+                            # COMM / RTK / 一般 GCS broadcast 下行資料
+                            # DISC 與 POLL 已經在前面處理掉，剩下資料直接轉給 FC / RTK module
+                            uart_fc.write(real_data)
+
+            rx_buf = rx_buf[total_len:]
+
+        else:
+            rx_buf = rx_buf[1:]
+
+
+# =========================================================
+# 初始清理
+# =========================================================
+
+try:
+    gc.collect()
+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)
+
+                learn_my_sysid_from_tx_buf()
+                try_send_pending_hello()
+                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)
+
+    except MemoryError:
+        tx_buf = bytearray()
+        rx_buf = bytearray()
+
+        try:
+            gc.collect()
+        except Exception:
+            pass
+
+        time.sleep_ms(10)
+
+    except Exception:
+        time.sleep_ms(2)

src/unitdev04/udptest8-2.py

@@ -1,500 +0,0 @@
-import asyncio
-import serial_asyncio
-import struct
-import serial
-import time
-import threading
-import tkinter as tk
-from tkinter import ttk
-from collections import deque, defaultdict
-from pymavlink import mavutil
-import matplotlib.pyplot as plt
-import matplotlib.animation as animation
-from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
-
-# === 多組設備設定 ===
-CONFIGS = [
-    {"serial_port": "/dev/ttyUSB0", "udp_port": 14551},
-    {"serial_port": "COM15", "udp_port": 14570},
-    {"serial_port": "/dev/ttyUSB2", "udp_port": 14553},
-    {"serial_port": "/dev/ttyUSB3", "udp_port": 14554},
-]
-
-SERIAL_BAUDRATE = 115200
-UDP_REMOTE_IP = '127.0.0.1'
-TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' 
-ACTIVE_SYSIDS = [3, 10, 15]
-POLL_MAGIC = b'POLL'
-
-# === 全域變數與狀態追蹤 ===
-current_base_slot_ms = 250  # 預設 TDMA 時槽
-LOSS_TIME_WINDOW_SEC = 5.0  # 丟包率計算的時間視窗 (5秒)
-
-uav_states = {
-    sysid: {
-        "mode": "NORMAL", 
-    } for sysid in ACTIVE_SYSIDS
-}
-
-rssi_history = defaultdict(lambda: deque(maxlen=5000))
-time_history = defaultdict(lambda: deque(maxlen=5000))
-packet_loss_history = defaultdict(lambda: deque(maxlen=1000))
-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})
-serial_to_sysid = {}
-serial_last_mavlink_time = {}
-last_db_query_time = {}
-
-# === 核心邏輯區 ===
-
-def calculate_packet_loss(sysid, compid, current_seq):
-    global mavlink_sequence_tracker, packet_loss_stats
-    tracker = mavlink_sequence_tracker[sysid]
-    now = time.time()
-    
-    if compid not in tracker:
-        tracker[compid] = {
-            'last_seq': current_seq, 
-            'history': deque() 
-        }
-        return 0.0
-    
-    comp_tracker = tracker[compid]
-    last_seq = comp_tracker['last_seq']
-    
-    if current_seq > last_seq:
-        expected = current_seq - last_seq
-    elif current_seq < last_seq:
-        expected = (255 - last_seq) + current_seq + 1
-    else:
-        return packet_loss_history[sysid][-1] if packet_loss_history[sysid] else 0.0
-    
-    lost = max(0, expected - 1)
-    
-    comp_tracker['history'].append((now, expected, lost))
-    comp_tracker['last_seq'] = current_seq
-    
-    total_expected_all = 0
-    total_lost_all = 0
-    
-    for c_id, c_data in tracker.items():
-        if 'history' in c_data:
-            while c_data['history'] and (now - c_data['history'][0][0]) > LOSS_TIME_WINDOW_SEC:
-                c_data['history'].popleft()
-            
-            total_expected_all += sum(item[1] for item in c_data['history'])
-            total_lost_all += sum(item[2] for item in c_data['history'])
-            
-    overall_loss_rate = (total_lost_all / total_expected_all) * 100.0 if total_expected_all > 0 else 0.0
-    
-    packet_loss_stats[sysid] = {
-        'loss_rate': overall_loss_rate,
-        'total_received': total_expected_all - total_lost_all,
-        'total_lost': total_lost_all
-    }
-    
-    packet_loss_history[sysid].append(overall_loss_rate)
-    packet_loss_time_history[sysid].append(now)
-    return overall_loss_rate
-
-def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x00) -> bytes:
-    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))
-
-class SerialToUDP(asyncio.Protocol):
-    def send_poll(self, target_sysid):
-        poll_payload = POLL_MAGIC + struct.pack("B", target_sysid)
-        api_frame = build_api_tx_frame(poll_payload, TARGET_ADDR64, 0x00)
-        self.transport.write(api_frame)
-
-    def __init__(self, udp_protocol, serial_port):
-        self.udp_protocol = udp_protocol
-        self.serial_port = serial_port
-        self.buffer = bytearray()
-        self.gcs_tx_queue = bytearray() 
-
-    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.")
-
-    def data_received(self, data):
-        self.buffer.extend(data)
-        while True:
-            try:
-                start_idx = self.buffer.index(0x7E)
-                if start_idx > 0:
-                    del self.buffer[:start_idx]
-            except ValueError:
-                self.buffer.clear()
-                return
-
-            if len(self.buffer) < 3: return
-                
-            length = (self.buffer[1] << 8) | self.buffer[2]
-            full_length = 3 + length + 1
-
-            if length > 300:
-                self.buffer.pop(0)
-                continue
-
-            if len(self.buffer) < full_length: return
-
-            frame = self.buffer[:full_length]
-            checksum = 0xFF - (sum(frame[3:-1]) & 0xFF)
-
-            if checksum != frame[-1]:
-                self.buffer.pop(0)
-                continue
-
-            del self.buffer[:full_length]
-
-            if hasattr(self.udp_protocol, 'send_udp'):
-                self.udp_protocol.send_udp(bytes(frame))
-
-            if frame[3] == 0x88 and frame[5:7] == b'DB':
-                status = frame[7]
-                if status == 0x00 and len(frame) > 8:
-                    rssi_value = frame[8]
-                    now = time.time()
-                    last_time = serial_last_mavlink_time.get(self.serial_port, 0)
-                    if now - last_time <= 0.5:
-                        sysid = serial_to_sysid.get(self.serial_port, None)
-                        if sysid is not None:
-                            rssi_history[sysid].append(-rssi_value)
-                            time_history[sysid].append(now)
-
-    def write_to_serial(self, data):
-        self.gcs_tx_queue.extend(data)
-
-    def flush_gcs_queue(self):
-        if not self.gcs_tx_queue: return
-        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:]
-        asyncio.create_task(self._async_send_chunks(data_to_send))
-
-    async def _async_send_chunks(self, data):
-        try:
-            MAX_PAYLOAD = 80 
-            sent_len = 0
-            while sent_len < len(data):
-                end_len = min(sent_len + MAX_PAYLOAD, len(data))
-                chunk = data[sent_len:end_len]
-                sent_len = end_len
-                
-                api_frame = build_api_tx_frame(chunk, TARGET_ADDR64, 0x00)
-                self.transport.write(api_frame)
-                await asyncio.sleep(0.01) 
-        except Exception:
-            pass
-
-    def send_at_command_db(self):
-        try:
-            frame_type = 0x08
-            frame_id = 0x52
-            at_command = b'DB'
-            parameter = b''
-            frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + at_command + parameter
-            checksum = 0xFF - (sum(frame) & 0xFF)
-            api_frame = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
-            self.transport.write(api_frame)
-        except Exception:
-            pass
-
-class UDPHandler(asyncio.DatagramProtocol):
-    def __init__(self, udp_port):
-        self.udp_port = udp_port
-        self.serial_transport = None
-        self.transport = None
-        self.mav_decoder = mavutil.mavlink.MAVLink(None)
-
-    def connection_made(self, transport):
-        self.transport = transport
-
-    def set_serial_transport(self, serial_transport):
-        self.serial_transport = serial_transport
-
-    def datagram_received(self, data, addr):
-        if self.serial_transport:
-            self.serial_transport.write_to_serial(data)
-
-    def decapsulate_data(self, data):
-        try:
-            if not data or data[0] != 0x7E:
-                return None
-            length = (data[1] << 8) | data[2]
-            if len(data) < length + 4:
-                return None
-            frame_type = data[3]
-            if frame_type == 0x90:
-                rf_data_start = 3 + 12
-                return data[rf_data_start:3 + length]
-            else:
-                return None
-        except Exception:
-            return None
-
-    def send_udp(self, data):
-        decoded_data = self.decapsulate_data(data)
-        if decoded_data is None: return
-        
-        try:
-            for byte in decoded_data:
-                msg = self.mav_decoder.parse_char(bytes([byte]))
-                if msg:
-                    sysid = msg.get_srcSystem()
-                    compid = msg.get_srcComponent()
-                    seq = msg.get_seq()
-                    if sysid == 0: continue
-                    
-                    if self.serial_transport:
-                        port_name = self.serial_transport.serial_port
-                        serial_to_sysid[port_name] = sysid
-                        serial_last_mavlink_time[port_name] = time.time()
-                        
-                        now = time.time()
-                        last_query = last_db_query_time.get(port_name, 0)
-                        if now - last_query >= 0.5:
-                            self.serial_transport.send_at_command_db()
-                            last_db_query_time[port_name] = now
-                            
-                    calculate_packet_loss(sysid, compid, seq)
-        except Exception:
-            pass
-
-        if self.transport:
-            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, self.udp_port))
-
-async def setup_bridge(config):
-    port, udp = config["serial_port"], config["udp_port"]
-    try:
-        ser = serial.Serial(port, SERIAL_BAUDRATE)
-        ser.close()
-    except: return None
-    
-    loop = asyncio.get_running_loop()
-    udp_handler = UDPHandler(udp)
-    await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', 0))
-    
-    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 tdma_scheduler(serial_protocols):
-    print(f"TDMA Scheduler Started... 找到 {len(serial_protocols)} 個可用端口")
-    while True:
-        # GCS 下行時槽
-        for sp in serial_protocols:
-            if hasattr(sp, 'flush_gcs_queue'): 
-                sp.flush_gcs_queue()
-        await asyncio.sleep(0.1) 
-
-        # UAV 上行時槽
-        for sysid in ACTIVE_SYSIDS:
-            state = uav_states[sysid]
-            
-            if state["mode"] == "INITIALIZING":
-                # VIP 模式：0.8秒內連發4次點名，暴力獲取參數
-                for _ in range(4):
-                    for sp in serial_protocols:
-                        if hasattr(sp, 'send_poll'): 
-                            sp.send_poll(sysid)
-                    await asyncio.sleep(0.2)
-            else:
-                # NORMAL 模式：聽從滑桿設定的時間
-                slot_time = current_base_slot_ms / 1000.0
-                for sp in serial_protocols:
-                    if hasattr(sp, 'send_poll'): 
-                        sp.send_poll(sysid)
-                await asyncio.sleep(slot_time)
-
-async def async_main():
-    protocols = await asyncio.gather(*(setup_bridge(cfg) for cfg in CONFIGS))
-    valid_protocols = [p for p in protocols if p is not None]
-    if valid_protocols:
-        asyncio.create_task(tdma_scheduler(valid_protocols))
-    await asyncio.Future()
-
-# === GUI 介面區 ===
-
-def start_gui():
-    root = tk.Tk()
-    root.title("UAV TDMA Control Station")
-    root.geometry("1200x800")
-    
-    # --- 左側控制面板 ---
-    control_frame = tk.Frame(root, width=300, bg="#f0f0f0", padx=20, pady=20)
-    control_frame.pack(side=tk.LEFT, fill=tk.Y)
-    
-    tk.Label(control_frame, text="TDMA 基礎時槽控制", font=("Arial", 14, "bold"), bg="#f0f0f0").pack(pady=10)
-    
-    slider_val = tk.IntVar(value=current_base_slot_ms)
-    def on_slider_change(val):
-        global current_base_slot_ms
-        current_base_slot_ms = int(float(val))
-        val_label.config(text=f"當前設定: {current_base_slot_ms} ms")
-
-    slider = ttk.Scale(control_frame, from_=50, to_=1000, orient='horizontal', variable=slider_val, command=on_slider_change)
-    slider.pack(fill=tk.X, pady=10)
-    val_label = tk.Label(control_frame, text=f"當前設定: {current_base_slot_ms} ms", bg="#f0f0f0")
-    val_label.pack()
-    
-    tk.Label(control_frame, text="群機初始化控制 (INIT)", font=("Arial", 14, "bold"), bg="#f0f0f0").pack(pady=(30, 10))
-    
-    # === 1. 改為單選按鈕 (Radiobutton) ===
-    init_var = tk.IntVar(value=0) # 0 代表全體 NORMAL
-    radios = {}
-    status_labels = {}
-    
-    def on_radio_change():
-        selected = init_var.get()
-        for sysid in ACTIVE_SYSIDS:
-            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))
-
-    for sysid in ACTIVE_SYSIDS:
-        frame = tk.Frame(control_frame, bg="#f0f0f0")
-        frame.pack(fill=tk.X, pady=5)
-        
-        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
-        
-        lbl = tk.Label(frame, text="NORMAL", font=("Arial", 10, "bold"), fg="green", bg="#f0f0f0")
-        lbl.pack(side=tk.RIGHT, padx=5)
-        status_labels[sysid] = lbl
-
-    # === 2. 改為開關式的鎖定按鈕 (Toggle) ===
-    is_locked = False
-    def toggle_tdma_mode():
-        nonlocal is_locked
-        is_locked = not is_locked
-        
-        if is_locked:
-            # 鎖定：強制切回 0 (全體 NORMAL)，並禁用單選按鈕
-            init_var.set(0)
-            on_radio_change()
-            for rb in radios.values(): 
-                rb.config(state=tk.DISABLED)
-            # 改變按鈕外觀為「解鎖」狀態
-            lock_btn.config(text="點擊解鎖 (允許重新下載參數)", bg="orange")
-            print("已鎖定進入純 TDMA 模式")
-        else:
-            # 解鎖：恢復單選按鈕功能
-            for rb in radios.values(): 
-                rb.config(state=tk.NORMAL)
-            # 改變按鈕外觀為「鎖定」狀態
-            lock_btn.config(text="參數載入完畢，鎖定進入 TDMA", bg="#4CAF50")
-            print("已解除鎖定，可重新分配特權")
-
-    lock_btn = tk.Button(control_frame, text="參數載入完畢，鎖定進入 TDMA", 
-                         font=("Arial", 12, "bold"), bg="#4CAF50", fg="white", 
-                         command=toggle_tdma_mode)
-    lock_btn.pack(pady=30, fill=tk.X)
-
-    # 動態更新狀態標籤
-    def update_status_gui():
-        for sysid, lbl in status_labels.items():
-            mode = uav_states[sysid]["mode"]
-            if mode == "INITIALIZING":
-                lbl.config(text="INIT (特權下載中)", fg="orange")
-            else:
-                lbl.config(text="NORMAL", fg="green")
-        root.after(500, update_status_gui)
-        
-    update_status_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, 8), 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("RSSI", fontsize=12); ax1.set_xlim(10, 0); ax1.set_ylim(-100, -10); ax1.grid(True, alpha=0.3)
-        ax2.set_title("Packet Loss Rate (5s Window)", fontsize=12); ax2.set_xlim(10, 0); ax2.set_ylim(0, 100); ax2.grid(True, alpha=0.3)
-        
-        now = time.time()
-        colors = ['blue', 'red', 'green', 'orange', 'purple']
-        
-        try: sysids = sorted(list(rssi_history.keys()))
-        except RuntimeError: return
-
-        loss_labels = []
-
-        for i, sysid in enumerate(sysids):
-            color = colors[i % len(colors)]
-            try:
-                t_hist, r_hist = list(time_history[sysid]), list(rssi_history[sysid])
-                lt_hist, l_hist = list(packet_loss_time_history.get(sysid, [])), list(packet_loss_history.get(sysid, []))
-            except RuntimeError: continue
-                
-            rssi_recent = [idx for idx, ts in enumerate(t_hist) if now - ts <= 10]
-            if rssi_recent:
-                ax1.plot([now - t_hist[idx] for idx in rssi_recent], [r_hist[idx] for idx in rssi_recent], label=f"SYSID:{sysid}", color=color)
-            
-            loss_recent = [idx for idx, ts in enumerate(lt_hist) if now - ts <= 10]
-            if loss_recent:
-                loss_t = [now - lt_hist[idx] for idx in loss_recent]
-                loss_r = [l_hist[idx] for idx in loss_recent]
-                ax2.plot(loss_t, loss_r, label=f"SYSID:{sysid}", color=color, marker='o', markersize=3)
-                
-                if loss_r:
-                    loss_labels.append({
-                        'sysid': sysid, 'y_real': loss_r[-1], 'x_real': loss_t[-1], 'color': color
-                    })
-
-        if loss_labels:
-            loss_labels = sorted(loss_labels, key=lambda k: k['y_real'])
-            min_gap = 12.0  
-            y_positions = [lbl['y_real'] for lbl in loss_labels]
-            
-            for j in range(1, len(y_positions)):
-                if y_positions[j] - y_positions[j-1] < min_gap:
-                    y_positions[j] = y_positions[j-1] + min_gap
-            
-            if y_positions[-1] > 90:
-                shift = y_positions[-1] - 90
-                y_positions = [y - shift for y in y_positions]
-
-            for j, lbl in enumerate(loss_labels):
-                sysid = lbl['sysid']
-                color = lbl['color']
-                real_y = lbl['y_real']
-                text_y = y_positions[j]
-                
-                ax2.text(0.5, text_y, f'ID:{sysid} ({real_y:.1f}%)', 
-                        bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.8),
-                        fontsize=10, fontweight='bold', color='white',
-                        horizontalalignment='right', verticalalignment='center')
-                
-                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")
-        
-    ani = animation.FuncAnimation(fig, update_plot, interval=1000)
-    
-    def on_closing():
-        root.quit()
-        root.destroy()
-    root.protocol("WM_DELETE_WINDOW", on_closing)
-    root.mainloop()
-
-if __name__ == '__main__':
-    threading.Thread(target=lambda: asyncio.run(async_main()), daemon=True).start()
-    start_gui()