feat(GUI): 新增模擬驗證滑動開關（ToggleSwitch）

在清除軌跡按鈕左邊加入滑動開關，可控制執行任務時是否開啟 matplotlib 驗證視窗。
預設開啟，OFF 時跳過驗證。

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

README.md

@@ -1,7 +1,5 @@
 這是天雷系統的專案
 
-
-
 ===
 ## 功能簡介
 1. mavlink 多對多支援平台

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,10 +1,78 @@
 #!/usr/bin/env python3
-from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
-                            QPushButton, QLineEdit, QComboBox)
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
+                            QPushButton, QLineEdit, QComboBox, QApplication)
+from PyQt6.QtGui import QFont
 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):
     """通讯设置面板类"""
     
@@ -26,6 +94,7 @@ class CommPanel(QWidget):
         self.ws_connections = []
         self.serial_connections = []
         self._init_ui()
+        self.apply_font_scale()
     
     def _init_ui(self):
         """初始化UI"""
@@ -35,7 +104,7 @@ class CommPanel(QWidget):
         
         # ========== UDP MAVLink 區域 ==========
         udp_title = QLabel("UDP")
-        udp_title.setStyleSheet("""
+        _set_scaled_stylesheet(udp_title, """
             color: #DDD;
             font-size: 14px;
             font-weight: bold;
@@ -60,7 +129,7 @@ class CommPanel(QWidget):
         self.udp_ip_input = QLineEdit()
         self.udp_ip_input.setText("127.0.0.1")
         self.udp_ip_input.setPlaceholderText("IP")
-        self.udp_ip_input.setStyleSheet("""
+        _set_scaled_stylesheet(self.udp_ip_input, """
             QLineEdit {
                 background-color: #333;
                 color: #DDD;
@@ -74,7 +143,7 @@ class CommPanel(QWidget):
         self.udp_port_input.setText("14550")
         self.udp_port_input.setPlaceholderText("Port")
         self.udp_port_input.setFixedWidth(80)
-        self.udp_port_input.setStyleSheet("""
+        _set_scaled_stylesheet(self.udp_port_input, """
             QLineEdit {
                 background-color: #333;
                 color: #DDD;
@@ -86,7 +155,7 @@ class CommPanel(QWidget):
         
         add_udp_btn = QPushButton("添加")
         add_udp_btn.clicked.connect(self._handle_add_udp)
-        add_udp_btn.setStyleSheet("""
+        _set_scaled_stylesheet(add_udp_btn, """
             QPushButton {
                 background-color: #4CAF50;
                 color: white;
@@ -98,9 +167,13 @@ class CommPanel(QWidget):
             QPushButton:hover { background-color: #45a049; }
         """)
         
-        add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
+        ip_label = QLabel("IP:")
+        _set_scaled_stylesheet(ip_label, "color: #DDD;")
+        add_udp_layout.addWidget(ip_label)
         add_udp_layout.addWidget(self.udp_ip_input)
-        add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+        port_label = QLabel("Port:")
+        _set_scaled_stylesheet(port_label, "color: #DDD;")
+        add_udp_layout.addWidget(port_label)
         add_udp_layout.addWidget(self.udp_port_input)
         add_udp_layout.addWidget(add_udp_btn)
         
@@ -113,7 +186,7 @@ class CommPanel(QWidget):
         
         # ========== Serial 區域 ==========
         serial_title = QLabel("Serial")
-        serial_title.setStyleSheet("""
+        _set_scaled_stylesheet(serial_title, """
             color: #DDD;
             font-size: 14px;
             font-weight: bold;
@@ -136,7 +209,7 @@ class CommPanel(QWidget):
         add_serial_layout.setContentsMargins(0, 0, 0, 0)
         
         self.serial_port_combo = QComboBox()
-        self.serial_port_combo.setStyleSheet("""
+        _set_scaled_stylesheet(self.serial_port_combo, """
             QComboBox {
                 background-color: #333;
                 color: #DDD;
@@ -160,7 +233,7 @@ class CommPanel(QWidget):
         refresh_ports_btn.setFixedWidth(35)
         refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
         refresh_ports_btn.setToolTip("重新掃描串口")
-        refresh_ports_btn.setStyleSheet("""
+        _set_scaled_stylesheet(refresh_ports_btn, """
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -176,7 +249,7 @@ class CommPanel(QWidget):
         self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
         self.serial_baudrate_combo.setCurrentText('57600')
         self.serial_baudrate_combo.setFixedWidth(100)
-        self.serial_baudrate_combo.setStyleSheet("""
+        _set_scaled_stylesheet(self.serial_baudrate_combo, """
             QComboBox {
                 background-color: #333;
                 color: #DDD;
@@ -197,7 +270,7 @@ class CommPanel(QWidget):
         
         add_serial_btn = QPushButton("添加")
         add_serial_btn.clicked.connect(self._handle_add_serial)
-        add_serial_btn.setStyleSheet("""
+        _set_scaled_stylesheet(add_serial_btn, """
             QPushButton {
                 background-color: #4CAF50;
                 color: white;
@@ -209,10 +282,14 @@ class CommPanel(QWidget):
             QPushButton:hover { background-color: #45a049; }
         """)
         
-        add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+        serial_port_label = QLabel("Port:")
+        _set_scaled_stylesheet(serial_port_label, "color: #DDD;")
+        add_serial_layout.addWidget(serial_port_label)
         add_serial_layout.addWidget(self.serial_port_combo)
         add_serial_layout.addWidget(refresh_ports_btn)
-        add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;"))
+        baud_label = QLabel("Baud:")
+        _set_scaled_stylesheet(baud_label, "color: #DDD;")
+        add_serial_layout.addWidget(baud_label)
         add_serial_layout.addWidget(self.serial_baudrate_combo)
         add_serial_layout.addWidget(add_serial_btn)
         
@@ -225,7 +302,7 @@ class CommPanel(QWidget):
         
         # ========== WebSocket 區域 ==========
         ws_title = QLabel("WebSocket")
-        ws_title.setStyleSheet("""
+        _set_scaled_stylesheet(ws_title, """
             color: #DDD;
             font-size: 14px;
             font-weight: bold;
@@ -249,7 +326,7 @@ class CommPanel(QWidget):
         
         self.ws_url_input = QLineEdit()
         self.ws_url_input.setPlaceholderText("host")
-        self.ws_url_input.setStyleSheet("""
+        _set_scaled_stylesheet(self.ws_url_input, """
             QLineEdit {
                 background-color: #333;
                 color: #DDD;
@@ -261,7 +338,7 @@ class CommPanel(QWidget):
         
         add_ws_btn = QPushButton("添加")
         add_ws_btn.clicked.connect(self._handle_add_ws)
-        add_ws_btn.setStyleSheet("""
+        _set_scaled_stylesheet(add_ws_btn, """
             QPushButton {
                 background-color: #4CAF50;
                 color: white;
@@ -273,7 +350,9 @@ class CommPanel(QWidget):
             QPushButton:hover { background-color: #45a049; }
         """)
         
-        add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
+        url_label = QLabel("URL:")
+        _set_scaled_stylesheet(url_label, "color: #DDD;")
+        add_ws_layout.addWidget(url_label)
         add_ws_layout.addWidget(self.ws_url_input)
         add_ws_layout.addWidget(add_ws_btn)
         
@@ -437,7 +516,7 @@ class CommPanel(QWidget):
     def create_udp_connection_panel(self, conn):
         """創建 UDP 連接面板"""
         panel = QWidget()
-        panel.setStyleSheet("""
+        _set_scaled_stylesheet(panel, """
             QWidget {
                 background-color: #2A2A2A;
                 border-radius: 6px;
@@ -451,22 +530,22 @@ class CommPanel(QWidget):
         
         # 連接資訊
         info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
-        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
         
         # 狀態指示器
         status_label = QLabel("●")
         if conn.get('enabled', False):
-            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
             status_label.setToolTip("運行中")
         else:
-            status_label.setStyleSheet("color: #888; font-size: 16px;")
+            _set_scaled_stylesheet(status_label, "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))
-        toggle_btn.setStyleSheet("""
+        _set_scaled_stylesheet(toggle_btn, """
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -481,7 +560,7 @@ class CommPanel(QWidget):
         remove_btn = QPushButton("移除")
         remove_btn.setFixedWidth(60)
         remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
-        remove_btn.setStyleSheet("""
+        _set_scaled_stylesheet(remove_btn, """
             QPushButton {
                 background-color: #d32f2f;
                 color: white;
@@ -509,7 +588,7 @@ class CommPanel(QWidget):
     def create_ws_connection_panel(self, conn):
         """創建 WebSocket 連接面板"""
         panel = QWidget()
-        panel.setStyleSheet("""
+        _set_scaled_stylesheet(panel, """
             QWidget {
                 background-color: #2A2A2A;
                 border-radius: 6px;
@@ -523,22 +602,22 @@ class CommPanel(QWidget):
         
         # 連接資訊
         info_label = QLabel(f"{conn['name']} - {conn['url']}")
-        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
         
         # 狀態指示器
         status_label = QLabel("●")
         if conn.get('enabled', False):
-            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
             status_label.setToolTip("運行中")
         else:
-            status_label.setStyleSheet("color: #888; font-size: 16px;")
+            _set_scaled_stylesheet(status_label, "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))
-        toggle_btn.setStyleSheet("""
+        _set_scaled_stylesheet(toggle_btn, """
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -553,7 +632,7 @@ class CommPanel(QWidget):
         remove_btn = QPushButton("移除")
         remove_btn.setFixedWidth(60)
         remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
-        remove_btn.setStyleSheet("""
+        _set_scaled_stylesheet(remove_btn, """
             QPushButton {
                 background-color: #d32f2f;
                 color: white;
@@ -605,7 +684,7 @@ class CommPanel(QWidget):
     def create_serial_connection_panel(self, conn):
         """創建 Serial 連接面板"""
         panel = QWidget()
-        panel.setStyleSheet("""
+        _set_scaled_stylesheet(panel, """
             QWidget {
                 background-color: #2A2A2A;
                 border-radius: 6px;
@@ -619,22 +698,22 @@ class CommPanel(QWidget):
         
         # 連接資訊
         info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
-        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
         
         # 狀態指示器
         status_label = QLabel("●")
         if conn.get('enabled', False):
-            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
             status_label.setToolTip("運行中")
         else:
-            status_label.setStyleSheet("color: #888; font-size: 16px;")
+            _set_scaled_stylesheet(status_label, "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))
-        toggle_btn.setStyleSheet("""
+        _set_scaled_stylesheet(toggle_btn, """
             QPushButton {
                 background-color: #444;
                 color: #DDD;
@@ -649,7 +728,7 @@ class CommPanel(QWidget):
         remove_btn = QPushButton("移除")
         remove_btn.setFixedWidth(60)
         remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
-        remove_btn.setStyleSheet("""
+        _set_scaled_stylesheet(remove_btn, """
             QPushButton {
                 background-color: #d32f2f;
                 color: white;
@@ -684,4 +763,12 @@ class CommPanel(QWidget):
     def remove_serial_connection_from_list(self, conn):
         """從列表中移除 Serial 連接"""
         if conn in self.serial_connections:
-            self.serial_connections.remove(conn)
+            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)

src/GUI/command_sender.py

@@ -16,6 +16,10 @@ from PyQt6.QtCore import QObject, pyqtSignal
 from pymavlink import mavutil
 
 
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
 class CommandSender(ABC):
     """指令發送抽象介面"""
 
@@ -63,7 +67,7 @@ class MavlinkSender(CommandSender):
         """
         self.connection_string = connection_string
         self.mav = mavutil.mavlink_connection(connection_string)
-        print(f"MavlinkSender 已建立連線: {connection_string}")
+        _log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
 
     def send_position_global(self, drone_id, sysid, lat, lon, alt):
         """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
@@ -86,7 +90,7 @@ class MavlinkSender(CommandSender):
         if self.mav:
             self.mav.close()
             self.mav = None
-            print("MavlinkSender 已關閉")
+            _log("INFO", "MavlinkSender 已關閉")
 
 
 class Ros2CommandSender(QObject):
@@ -162,4 +166,4 @@ class Ros2CommandSender(QObject):
             if not task.done():
                 task.cancel()
         self._pending.clear()
-        print("Ros2CommandSender 已關閉")
+        _log("INFO", "Ros2CommandSender 已關閉")

src/GUI/communication.py

@@ -13,12 +13,18 @@ import sys
 import os
 import traceback
 from pymavlink import mavutil
-from geometry_msgs.msg import Point, Vector3
+from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
 from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
+from nav_msgs.msg import Odometry
 from mavros_msgs.srv import CommandBool, CommandTOL
 
+
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
 # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
 _src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
 if _src_path not in sys.path:
@@ -29,10 +35,9 @@ try:
     from fc_network_apps.longCommand import CommandLongClient
 except ImportError as e:
     import traceback
-    print(f"⚠️  警告: 無法導入 CommandLongClient")
-    print(f"   错误: {e}")
-    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
-    print(f"   完整堆栈跟踪:")
+    _log("WARN", "無法導入 CommandLongClient")
+    _log("ERROR", f"錯誤: {e}")
+    _log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
     traceback.print_exc()
     CommandLongClient = None
 
@@ -41,11 +46,18 @@ try:
     from fc_network_apps.navigation import PositionTargetGlobalIntClient
 except ImportError as e:
     import traceback
-    print(f"⚠️  警告: 無法導入 PositionTargetGlobalIntClient")
-    print(f"   错误: {e}")
+    _log("WARN", "無法導入 PositionTargetGlobalIntClient")
+    _log("ERROR", f"錯誤: {e}")
     traceback.print_exc()
     PositionTargetGlobalIntClient = None
 
+try:
+    from fc_interfaces.msg import AttitudeRaw
+except ImportError as e:
+    _log("WARN", "無法導入 AttitudeRaw")
+    _log("ERROR", f"錯誤: {e}")
+    AttitudeRaw = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -527,25 +539,25 @@ class DroneMonitor(Node):
                     unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
                     client = CommandLongClient(node_name=unique_name)
                     self.command_long_clients[drone_id] = client
-                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
+                    _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})")
                     
                     # 將新 client 添加到主執行器（這樣它的回調才能被處理）
                     if self.executor:
                         self.executor.add_node(client)
-                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
                     
                 except TypeError:
                     # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
                     client = CommandLongClient()
                     self.command_long_clients[drone_id] = client
-                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
+                    _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
                     
                     if self.executor:
                         self.executor.add_node(client)
-                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
                         
                 except Exception as e:
-                    print(f"⚠️  無法為 {drone_id} 創建 CommandLongClient: {e}")
+                    _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
                     return None
             return self.command_long_clients[drone_id]
 
@@ -560,12 +572,12 @@ class DroneMonitor(Node):
                     unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
                     client = PositionTargetGlobalIntClient(node_name=unique_name)
                     self.position_target_clients[drone_id] = client
-                    print(f"   ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
+                    _log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})")
                     if self.executor:
                         self.executor.add_node(client)
-                        print(f"   ✓ 將 {drone_id} 的 position client 添加到主執行器")
+                        _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
                 except Exception as e:
-                    print(f"⚠️  無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
+                    _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
                     return None
             return self.position_target_clients[drone_id]
 
@@ -588,8 +600,38 @@ class DroneMonitor(Node):
                 # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
                 self.sys_to_socket_id[sys_id] = 0
             
-            if not hasattr(self, f'drone_{sys_id}_subs'):
+            subs_attr = f'drone_{sys_id}_subs'
+            if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
+            else:
+                # 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
+                subs = getattr(self, subs_attr, {})
+                if isinstance(subs, dict) and 'position_ned' not in subs:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        pos_ned_sub = self.create_subscription(
+                            Odometry,
+                            f'{base_topic}/position_ned',
+                            lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                            10
+                        )
+                        subs['position_ned'] = pos_ned_sub
+                        setattr(self, subs_attr, subs)  # 明確保存更新後的字典
+                    except Exception as e:
+                        pass
+                if isinstance(subs, dict) and 'attitude' not in subs and AttitudeRaw is not None:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        attitude_sub = self.create_subscription(
+                            AttitudeRaw,
+                            f'{base_topic}/attitude',
+                            lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
+                            10
+                        )
+                        subs['attitude'] = attitude_sub
+                        setattr(self, subs_attr, subs)
+                    except Exception:
+                        pass
     
     def setup_drone(self, sys_id):
         # sys_id 格式： sys11, sys12, ...
@@ -636,8 +678,22 @@ class DroneMonitor(Node):
                 f'{base_topic}/vfr_hud',
                 lambda msg, sid=sys_id: self.hud_callback(sid, msg),
                 10
+            ),
+            'position_ned': self.create_subscription(
+                Odometry,
+                f'{base_topic}/position_ned',
+                lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                10
             )
         }
+
+        if AttitudeRaw is not None:
+            subs['attitude'] = self.create_subscription(
+                AttitudeRaw,
+                f'{base_topic}/attitude',
+                lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
+                10
+            )
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
@@ -675,22 +731,22 @@ class DroneMonitor(Node):
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
             if len(parts) < 2:
-                print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                _log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
                 return False
             sysid = int(parts[-1])
             
             # 獲取模式對應的 custom_mode 值
             custom_mode = self.MODE_MAPPING.get(mode_name)
             if custom_mode is None:
-                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+                _log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
                 return False
             
-            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+            _log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
             
             # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
             client = self.get_or_create_client(drone_id)
             if not client:
-                print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
+                _log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -703,13 +759,13 @@ class DroneMonitor(Node):
             )
             
             if result and result.success:
-                print(f"✅ [SET_MODE] 模式切換成功")
+                _log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
                 return True
             else:
-                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+                _log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
-            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            _log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
             traceback.print_exc()
             return False
 
@@ -719,17 +775,17 @@ class DroneMonitor(Node):
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
             if len(parts) < 2:
-                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+                _log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
                 return False
             sysid = int(parts[-1])
             
             action_name = "解鎖" if arm else "上鎖"
-            print(f"\n📢 [ARM] {drone_id} → {action_name}")
+            _log("INFO", f"[ARM] {drone_id} -> {action_name}")
             
             # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
             client = self.get_or_create_client(drone_id)
             if not client:
-                print(f"❌ [ARM] CommandLongClient 無法初始化")
+                _log("ERROR", "[ARM] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -741,13 +797,13 @@ class DroneMonitor(Node):
             )
             
             if result and result.success:
-                print(f"✅ [ARM] {action_name}成功")
+                _log("INFO", f"[ARM] {drone_id} {action_name}成功")
                 return True
             else:
-                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+                _log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
-            print(f"❌ [ARM] 例外錯誤: {e}")
+            _log("ERROR", f"[ARM] 例外錯誤: {e}")
             traceback.print_exc()
             return False
 
@@ -757,16 +813,16 @@ class DroneMonitor(Node):
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
             if len(parts) < 2:
-                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                _log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
                 return False
             sysid = int(parts[-1])
             
-            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+            _log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
             
             # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
             client = self.get_or_create_client(drone_id)
             if not client:
-                print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
+                _log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -778,13 +834,13 @@ class DroneMonitor(Node):
             )
             
             if result and result.success:
-                print(f"✅ [TAKEOFF] 起飛成功")
+                _log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
                 return True
             else:
-                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                _log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
-            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            _log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
             traceback.print_exc()
             return False
 
@@ -816,17 +872,42 @@ class DroneMonitor(Node):
         return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
 
     # callbacks
-    def attitude_callback(self, drone_id, msg):
-        if hasattr(msg, 'orientation'):
-            roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
-            self.latest_data[(drone_id, 'attitude')] = {
-                'roll': roll,
-                'pitch': pitch,
-                'yaw': yaw,
-                'rates': (msg.angular_velocity.x, 
-                         msg.angular_velocity.y,
-                         msg.angular_velocity.z)
-            }
+    def attitude_callback(self, sys_id, msg):
+        """處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。"""
+        actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+        if actual_drone_id is None:
+            return
+
+        try:
+            if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'):
+                data = {
+                    'roll': math.degrees(msg.roll),
+                    'pitch': math.degrees(msg.pitch),
+                    'yaw': math.degrees(msg.yaw),
+                    'rates': (
+                        getattr(msg, 'rollspeed', 0.0),
+                        getattr(msg, 'pitchspeed', 0.0),
+                        getattr(msg, 'yawspeed', 0.0),
+                    )
+                }
+            elif hasattr(msg, 'orientation'):
+                roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
+                data = {
+                    'roll': roll,
+                    'pitch': pitch,
+                    'yaw': yaw,
+                    'rates': (
+                        msg.angular_velocity.x,
+                        msg.angular_velocity.y,
+                        msg.angular_velocity.z,
+                    )
+                }
+            else:
+                return
+
+            self.latest_data[(actual_drone_id, 'attitude')] = data
+        except Exception as e:
+            print(f"Error parsing attitude msg for {sys_id}: {e}")
 
     def battery_callback(self, sys_id, msg):
         # 使用映射獲取實際的 drone_id
@@ -879,6 +960,7 @@ class DroneMonitor(Node):
                 sys_num = sys_id.replace('sys', '')
                 actual_drone_id = f's{assigned_socket_id}_{sys_num}'
                 self.sys_to_actual_id[sys_id] = actual_drone_id
+                _log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
             
             # 先發送連接類型資訊
             self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -959,6 +1041,55 @@ class DroneMonitor(Node):
             'climb': msg.climb
         }
 
+    def position_ned_callback(self, sys_id, msg):
+        """處理 position_ned topic (nav_msgs/msg/Odometry)
+        NED 座標系中的位置（在 msg.pose.pose.position）：
+        - x: 北向位移 (m)
+        - y: 東向位移 (m)
+        - z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1)
+        """
+        try:
+            # 使用映射獲取實際的 drone_id
+            actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+            # 如果還沒有從 summary 獲取到映射，則不處理
+            if actual_drone_id is None:
+                return
+            
+            # 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
+            # Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist
+            x = msg.pose.pose.position.y  # NED 座標系中交換 x/y（與 local_pose 對齐）
+            y = msg.pose.pose.position.x
+            z = -msg.pose.pose.position.z  # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
+            vx = msg.twist.twist.linear.y
+            vy = msg.twist.twist.linear.x
+            vz = -msg.twist.twist.linear.z
+            
+            # 儲存高度信息
+            self.latest_data[(actual_drone_id, 'altitude')] = {
+                'altitude': z
+            }
+            
+            # 儲存本地位置信息
+            self.latest_data[(actual_drone_id, 'local_pose')] = {
+                'x': x,
+                'y': y,
+                'z': z
+            }
+
+            # 儲存速度資訊，供總覽頁「XY速度」欄位顯示
+            self.latest_data[(actual_drone_id, 'velocity')] = {
+                'vx': vx,
+                'vy': vy,
+                'vz': vz
+            }
+            
+            # 發送信號給 GUI 更新高度顯示
+            self.signals.update_signal.emit('altitude', actual_drone_id, {
+                'altitude': z
+            })
+        except Exception as e:
+            pass
+
     def loss_rate_callback(self, drone_id, msg):
         self.latest_data[(drone_id, 'loss_rate')] = {
             'loss_rate': msg.data
@@ -975,5 +1106,5 @@ class DroneMonitor(Node):
         receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
         receiver.start()
         self.serial_receivers.append(receiver)
-        print(f"Started serial connection on {port} at {baudrate} baud")
-        return receiver
+        _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
+        return receiver

src/GUI/drone_panel.py

@@ -1,10 +1,77 @@
 #!/usr/bin/env python3
-from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox, QApplication)
 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):
     """單個無人機面板類別"""
     
@@ -32,12 +99,13 @@ 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(140)
-        self.setStyleSheet("""
+        _set_scaled_stylesheet(self, """
             background-color: #2A2A2A;
             border-radius: 8px;
         """)
@@ -49,7 +117,7 @@ class DronePanel(QWidget):
         
         # 創建內容容器（包含 info 和 control）
         content_widget = QWidget()
-        content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
+        _set_scaled_stylesheet(content_widget, "background-color: #333; border-radius: 6px;")
         content_layout = QHBoxLayout(content_widget)
         content_layout.setContentsMargins(8, 8, 8, 8)
         content_layout.setSpacing(8)
@@ -82,7 +150,7 @@ class DronePanel(QWidget):
         # 勾選框
         self.checkbox = QCheckBox()
         self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
-        self.checkbox.setStyleSheet("""
+        _set_scaled_stylesheet(self.checkbox, """
             QCheckBox { 
                 color: #DDD;
             }
@@ -103,8 +171,8 @@ class DronePanel(QWidget):
         )
         
         # ID 顯示
-        id_label = QLabel(self.display_id)
-        id_label.setStyleSheet("""
+        self.id_label = QLabel(self.display_id)
+        _set_scaled_stylesheet(self.id_label, """
             font-weight: bold; 
             font-size: 14px; 
             color: #7FFFD4;
@@ -112,7 +180,7 @@ class DronePanel(QWidget):
         """)
         
         header_layout.addWidget(self.checkbox)
-        header_layout.addWidget(id_label)
+        header_layout.addWidget(self.id_label)
         header_layout.addStretch()
         
         info_layout.addWidget(header)
@@ -148,15 +216,15 @@ class DronePanel(QWidget):
         status_layout.setContentsMargins(0, 0, 0, 0)
         
         status_title = QLabel("狀態：")
-        status_title.setStyleSheet("color: #888; min-width: 50px;")
+        _set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;")
         
         self.mode_label = QLabel("--")
         self.mode_label.setObjectName(f"{self.drone_id}_mode")
-        self.mode_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.mode_label, "color: #DDD;")
         
         self.armed_label = QLabel("--")
         self.armed_label.setObjectName(f"{self.drone_id}_armed")
-        self.armed_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.armed_label, "color: #DDD;")
         
         status_layout.addWidget(status_title)
         status_layout.addWidget(self.mode_label)
@@ -172,15 +240,15 @@ class DronePanel(QWidget):
         connection_layout.setContentsMargins(0, 0, 0, 0)
         
         connection_title = QLabel("Socket：")
-        connection_title.setStyleSheet("color: #888; min-width: 50px;")
+        _set_scaled_stylesheet(connection_title, "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")
-        self.socket_seq_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.socket_seq_label, "color: #DDD;")
         
         connection_sep = QLabel(" - ")
-        connection_sep.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(connection_sep, "color: #DDD;")
         
         # 設定連接方式顯示
         connection_type_map = {
@@ -193,7 +261,7 @@ class DronePanel(QWidget):
         
         self.connection_type_label = QLabel(connection_type)
         self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
-        self.connection_type_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.connection_type_label, "color: #DDD;")
         
         connection_layout.addWidget(connection_title)
         connection_layout.addWidget(self.socket_seq_label)
@@ -210,29 +278,29 @@ class DronePanel(QWidget):
         battery_layout.setContentsMargins(0, 0, 0, 0)
         # 顯示百分比
         battery_title = QLabel("電池：")
-        battery_title.setStyleSheet("color: #888; min-width: 50px;")
+        _set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;")
 
         self.battery_pct_label = QLabel("--")
         self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
-        self.battery_pct_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.battery_pct_label, "color: #DDD;")
 
         # 分隔符
         separator1 = QLabel(" - ")
-        separator1.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(separator1, "color: #DDD;")
 
         # 顯示電壓
         self.battery_vol_label = QLabel("--")
         self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
-        self.battery_vol_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.battery_vol_label, "color: #DDD;")
 
         # 分隔符
         separator2 = QLabel(" - ")
-        separator2.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(separator2, "color: #DDD;")
 
         # 顯示電池節數 (S count)
         self.battery_cells_label = QLabel("--")
         self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
-        self.battery_cells_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.battery_cells_label, "color: #DDD;")
 
         battery_layout.addWidget(battery_title)
         battery_layout.addWidget(self.battery_pct_label)
@@ -251,18 +319,18 @@ class DronePanel(QWidget):
         altitude_layout.setContentsMargins(0, 0, 0, 0)
         
         altitude_title = QLabel("高度：")
-        altitude_title.setStyleSheet("color: #888; min-width: 50px;")
+        _set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;")
         
         self.altitude_label = QLabel("--")
         self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
-        self.altitude_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.altitude_label, "color: #DDD;")
         
         speed_title = QLabel("速度：")
-        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+        _set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;")
         
         self.speed_label = QLabel("--")
         self.speed_label.setObjectName(f"{self.drone_id}_speed")
-        self.speed_label.setStyleSheet("color: #DDD;")
+        _set_scaled_stylesheet(self.speed_label, "color: #DDD;")
         
         altitude_layout.addWidget(altitude_title)
         altitude_layout.addWidget(self.altitude_label)
@@ -320,6 +388,16 @@ 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):
     # 定義信號
@@ -331,11 +409,12 @@ 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}")
-        self.setStyleSheet("""
+        _set_scaled_stylesheet(self, """
             background-color: #1E1E1E;
             border-radius: 12px;
         """)
@@ -352,7 +431,7 @@ class SocketGroupPanel(QWidget):
         # 分組勾選框
         self.group_checkbox = QCheckBox()
         self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
-        self.group_checkbox.setStyleSheet(f"""
+        _set_scaled_stylesheet(self.group_checkbox, f"""
             QCheckBox {{ color: #DDD; }}
             QCheckBox::indicator {{
                 width: 14px;
@@ -380,7 +459,7 @@ class SocketGroupPanel(QWidget):
         else:
             title_text = f"Socket {self.socket_id}"
         self.title_label = QLabel(title_text)
-        self.title_label.setStyleSheet(f"""
+        _set_scaled_stylesheet(self.title_label, f"""
             font-weight: bold; 
             font-size: 16px; 
             color: {self.color};
@@ -430,6 +509,14 @@ 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):
     """
@@ -503,7 +590,9 @@ class AttitudeIndicator(QWidget):
 
         # pitch ladder (every 10°, ±30°)
         p.setPen(QPen(QColor(255, 255, 255, 180), 1))
-        p.setFont(QFont("Arial", 6))
+        ladder_font = QFont("Arial")
+        ladder_font.setPointSizeF(max(1.0, 6 * _get_font_scale()))
+        p.setFont(ladder_font)
         for deg in range(-30, 31, 10):
             if deg == 0:
                 continue
@@ -581,6 +670,8 @@ class AttitudeIndicator(QWidget):
 
         # heading text centred (bigger)
         p.setPen(QPen(QColor("#FFFFFF")))
-        p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
+        heading_font = QFont("Arial", weight=QFont.Weight.Bold)
+        heading_font.setPointSizeF(max(1.0, 10 * _get_font_scale()))
+        p.setFont(heading_font)
         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

@@ -3,6 +3,11 @@ 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 地圖顯示"""
     
@@ -11,6 +16,7 @@ class DroneMap:
         self.map_view = QWebEngineView()
         self.map_loaded = False
         self.pending_map_updates = {}
+        self.font_scale = 1.0
         
         # 創建橋接對象
         self.bridge = MapBridge()
@@ -31,6 +37,7 @@ 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;
@@ -65,7 +72,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     box-shadow: 0 2px 5px rgba(0,0,0,0.2);
                 }
@@ -85,7 +92,7 @@ class DroneMap:
                 }
                 .mission-info-row {
                     margin-bottom: 8px;
-                    font-size: 12px;
+                    font-size: calc(12px * var(--ui-font-scale));
                     color: #333;
                 }
                 .mission-info-label {
@@ -104,7 +111,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     margin-top: 8px;
                 }
@@ -131,7 +138,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -149,7 +156,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -167,7 +174,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -322,7 +329,7 @@ class DroneMap:
                     align-items: center;
                     justify-content: center;
                     font-weight: bold;
-                    font-size: 12px;
+                    font-size: calc(12px * var(--ui-font-scale));
                     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],
@@ -564,7 +571,7 @@ class DroneMap:
                             'align-items: center;' +
                             'justify-content: center;' +
                             'font-weight: bold;' +
-                            'font-size: 11px;' +
+                            'font-size: calc(11px * var(--ui-font-scale));' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">' + idx + '</div>',
@@ -617,6 +624,10 @@ class DroneMap:
                     }
                 }
                 // ================================================================================
+
+                function setFontScale(scale) {
+                    document.documentElement.style.setProperty('--ui-font-scale', scale);
+                }
                 
                 // 開始任務
                 function startMission() {
@@ -802,7 +813,7 @@ class DroneMap:
                             'width: 22px; height: 22px;' +
                             'border-radius: 50%;' +
                             'display: flex; align-items: center; justify-content: center;' +
-                            'font-weight: bold; font-size: 10px;' +
+                            'font-weight: bold; font-size: calc(10px * var(--ui-font-scale));' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">' + groupId + '</div>',
@@ -822,7 +833,7 @@ class DroneMap:
                             'width: 22px; height: 22px;' +
                             'border-radius: 50%;' +
                             'display: flex; align-items: center; justify-content: center;' +
-                            'font-weight: bold; font-size: 14px;' +
+                            'font-weight: bold; font-size: calc(14px * var(--ui-font-scale));' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">★</div>',
@@ -873,8 +884,9 @@ class DroneMap:
         """地圖加載完成回調"""
         if ok:
             self.map_loaded = True
+            self.set_font_scale(self.font_scale)
         else:
-            print("⚠️ 地圖加載失敗")
+            _log("ERROR", "地圖載入失敗")
     
     def update_drone_position(self, drone_id, lat, lon, heading):
         """更新無人機位置（加入待處理隊列）"""
@@ -904,6 +916,12 @@ class DroneMap:
         """聚焦到指定無人機"""
         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});")
     
     # ================================================================================
     # 任務規劃視覺化方法
@@ -924,20 +942,24 @@ class DroneMap:
                 f"{target_lat:.6f}, {target_lon:.6f});"
             )
             self.map_view.page().runJavaScript(js_code)
-            print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
-                  f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
+            _log(
+                "INFO",
+                f"地圖已繪製 Group {group_id} 任務規劃: "
+                f"C({center_lat:.6f}, {center_lon:.6f}) -> "
+                f"T({target_lat:.6f}, {target_lon:.6f})",
+            )
 
     def clear_mission_plan(self):
         """清除地圖上所有任務規劃標記"""
         if self.map_loaded:
             self.map_view.page().runJavaScript("clearAllMissionPlans();")
-            print("🗑️ 地圖已清除所有任務規劃")
+            _log("INFO", "地圖已清除所有任務規劃")
 
     def clear_mission_plan_for_group(self, group_id):
         """清除指定群組的任務規劃標記"""
         if self.map_loaded:
             self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
-            print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
+            _log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
 
     def set_mission_mode(self, mode):
         """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
@@ -1033,52 +1055,56 @@ class MapBridge(QObject):
     def clearAllDroneSelection(self):
         """供 JavaScript 調用的方法 - 清除所有無人機選擇"""
         self.clear_all_drone_selection.emit()
-        print("🗑️ 清除所有無人機選擇")
+        _log("INFO", "已清除所有無人機選擇")
     
     @pyqtSlot()
     def toggleSelectAllDrones(self):
         """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
         self.select_all_drones.emit()
-        print("🔄 切換全選無人機")
+        _log("INFO", "已切換全選無人機")
     
     @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)
-        print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
+        _log(
+            "INFO",
+            f"已發出開始任務信號: "
+            f"C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})",
+        )
     
     @pyqtSlot()
     def pauseMissionSignal(self):
         """供 JavaScript 調用的方法 - 暫停任務"""
         self.pause_mission_signal.emit()
-        print("⏸️ 暫停任務信號已發出")
+        _log("INFO", "已發出暫停任務信號")
     
     @pyqtSlot(str)
     def rectangleSelected(self, points_json):
         """供 JavaScript 調用的方法 - 矩形選擇完成"""
         self.rectangle_selected.emit(points_json)
-        print(f"📦 矩形區域已選擇: {points_json}")
+        _log("INFO", f"矩形區域已選擇: {points_json}")
     
     @pyqtSlot(str)
     def polygonSelected(self, points_json):
         """供 JavaScript 調用的方法 - 多邊形選擇完成"""
         self.polygon_selected.emit(points_json)
-        print(f"🔷 多邊形區域已選擇: {points_json}")
+        _log("INFO", f"多邊形區域已選擇: {points_json}")
     
     @pyqtSlot(str)
     def missionModeChanged(self, mode):
         """供 JavaScript 調用的方法 - 任務模式切換"""
         self.mission_mode_changed.emit(mode)
-        print(f"🔄 任務模式切換: {mode}")
+        _log("INFO", f"任務模式已切換: {mode}")
     
     @pyqtSlot(str)
     def routeConfirmed(self, points_json):
         """供 JavaScript 調用的方法 - 路徑確認"""
         self.route_confirmed.emit(points_json)
-        print(f"📍 路徑已確認: {points_json}")
+        _log("INFO", f"路徑已確認: {points_json}")
 
     @pyqtSlot(str)
     def droneBoxSelected(self, drone_ids_json):
         """供 JavaScript 調用的方法 - 框選無人機完成"""
         self.drone_box_selected.emit(drone_ids_json)
-        print(f"📦 框選無人機: {drone_ids_json}")
+        _log("INFO", f"框選無人機完成: {drone_ids_json}")

src/GUI/mission_executor.py

@@ -19,6 +19,10 @@ from enum import Enum
 from PyQt6.QtCore import QObject, QTimer, pyqtSignal
 
 
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
 class MissionState(Enum):
     """整體任務狀態"""
     IDLE = "idle"
@@ -109,7 +113,7 @@ class MissionExecutor(QObject):
 
     def start(self, planned_waypoints):
         if self.state == MissionState.RUNNING:
-            print("任務已在執行中")
+            _log("WARN", "任務已在執行中")
             return
 
         self.tasks.clear()
@@ -132,31 +136,34 @@ class MissionExecutor(QObject):
             f"rendezvous WP={sorted(self.rendezvous_indices)}"
             if self.rendezvous_indices else "無 rendezvous (各自跑)"
         )
-        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}")
+        _log(
+            "INFO",
+            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
-            print("任務暫停")
+            _log("INFO", "任務已暫停")
 
     def resume(self):
         if self.state == MissionState.PAUSED:
             self._timer.start(self._interval_ms)
             self.state = MissionState.RUNNING
-            print("任務恢復")
+            _log("INFO", "任務已恢復")
 
     def stop(self):
         self._timer.stop()
         self.tasks.clear()
         self.rendezvous_indices = set()
         self.state = MissionState.IDLE
-        print("任務停止")
+        _log("INFO", "任務已停止")
 
     # ------------------------------------------------------------------ 控制迴圈
 
@@ -197,7 +204,7 @@ class MissionExecutor(QObject):
                 # rendezvous 點 → 不推進，進入 barrier 等待
                 task.status = TaskStatus.WAITING_AT_BARRIER
                 task.waiting_since = now
-                print(f"  {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
+                _log("INFO", 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}"
@@ -236,7 +243,7 @@ class MissionExecutor(QObject):
             self._timer.stop()
             self.state = MissionState.IDLE
             self.mission_completed.emit()
-            print("===== 任務全部完成 =====")
+            _log("INFO", "任務全部完成")
 
     def _advance_waypoint(self, task, arrived_distance):
         """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
@@ -248,14 +255,20 @@ class MissionExecutor(QObject):
             self.drone_waypoint_reached.emit(
                 task.drone_id, task.wp_index, task.total_waypoints
             )
-            print(f"  {task.drone_id} → DONE "
-                  f"({task.total_waypoints}/{task.total_waypoints})")
+            _log(
+                "INFO",
+                f"{task.drone_id} 已完成所有航點 "
+                f"({task.total_waypoints}/{task.total_waypoints})",
+            )
             return
         self.drone_waypoint_reached.emit(
             task.drone_id, task.wp_index, task.total_waypoints
         )
-        print(f"  {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
-              f"(到達距離: {arrived_distance:.1f}m)")
+        _log(
+            "INFO",
+            f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
+            f"(到達距離: {arrived_distance:.1f}m)",
+        )
 
     def _check_barriers(self, now):
         """檢查每個有人在等的 barrier 能不能釋放。"""
@@ -289,14 +302,13 @@ 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"
-                    print(f"⚠️  barrier WP {barrier_idx} {force_reason}，強制放行")
+                    _log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
                 else:
                     continue  # 還沒到齊、也還沒 timeout → 繼續等
 
             # 釋放：把所有在此 barrier 等待的機一起推進
             tag = "全員到齊" if force_reason is None else force_reason
-            print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
-                  f"推進 {len(waiting_tasks)} 架")
+            _log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
             for task in waiting_tasks:
                 task.status = TaskStatus.NORMAL
                 msg = f"barrier WP {barrier_idx} released ({tag})"
@@ -324,7 +336,7 @@ class MissionExecutor(QObject):
             return
 
         task.fail_count += 1
-        print(f"⚠️  {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
+        _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
 
         if task.fail_count < self.MAX_RETRY:
             task.status = TaskStatus.RETRYING
@@ -339,13 +351,13 @@ class MissionExecutor(QObject):
                 drone_id, task.status.value,
                 f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
             )
-            print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
+            _log("ERROR", 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 派送。"""
         if self.monitor is None:
-            print(f"⚠️  無 monitor，無法將 {drone_id} 切到 LOITER")
+            _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
             return
         try:
             loop = asyncio.get_event_loop()

src/GUI/mission_group.py

@@ -23,19 +23,35 @@ GROUP_COLORS = [
     '#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',
+}
+
 
 class MissionGroup:
     """單一任務群組的資料"""
 
     def __init__(self, group_id, color):
-        self.group_id = group_id          # 'A', 'B', 'C', ...
-        self.color = color                # 顏色 hex
-        self.drone_ids = set()            # 已分配的無人機 ID
-        self.mission_type = 'M_FORMATION' # 預設任務類型
-        self.planned_waypoints = None     # 規劃結果 dict
-        self.executor = None              # MissionExecutor 實例（延遲建立）
-        self.center_origin = None         # 規劃原點
-        self.leader_drone_id = None       # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
+        self.group_id = group_id             # 'A', 'B', 'C', ...
+        self.color = color                   # 顏色 hex
+        
+        # 唯一真實資料來源：該 group 選中的 drone ID
+        # 代表：group 擁有的 drone、被選中的 drone、可以操作的 drone
+        self.selected_drone_ids = set()
+        
+        self.mission_type = 'M_FORMATION'    # 預設任務類型
+        self.planned_waypoints = None        # 規劃結果 dict
+        self.executor = None                 # MissionExecutor 實例（延遲建立）
+        self.center_origin = None            # 規劃原點
+        self.leader_drone_id = None          # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
 
     @property
     def state(self):
@@ -51,13 +67,13 @@ class MissionGroup:
 class DroneAssignDialog(QDialog):
     """無人機分配對話框"""
 
-    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned):
+    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned=None):
         """
         Args:
             parent: 父 widget
             all_drone_ids: 所有可用無人機 ID 列表
             current_assigned: 當前群組已分配的無人機 set
-            other_assigned: 其他群組已佔用的無人機 dict {drone_id: group_id}
+            other_assigned: (忽略 — 現在允許多 group 分配)
         """
         super().__init__(parent)
         self.setWindowTitle("分配無人機")
@@ -67,7 +83,6 @@ class DroneAssignDialog(QDialog):
             QLabel { color: #DDD; }
             QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
             QCheckBox::indicator { width: 16px; height: 16px; }
-            QCheckBox:disabled { color: #666; }
         """)
 
         layout = QVBoxLayout(self)
@@ -90,12 +105,9 @@ class DroneAssignDialog(QDialog):
         for drone_id in sorted_ids:
             cb = QCheckBox(drone_id)
 
+            # 所有 drone 都能被選摘（支持多 group 分配）
             if drone_id in current_assigned:
                 cb.setChecked(True)
-            elif drone_id in other_assigned:
-                cb.setEnabled(False)
-                cb.setToolTip(f"已分配到 Group {other_assigned[drone_id]}")
-                cb.setText(f"{drone_id}  (Group {other_assigned[drone_id]})")
 
             self.checkboxes[drone_id] = cb
             scroll_layout.addWidget(cb)
@@ -157,11 +169,13 @@ class GroupPanel(QWidget):
         QPushButton:disabled {{ background-color: #444; color: #666; }}
     """
 
-    def __init__(self, group: MissionGroup, parent=None):
+    def __init__(self, group: MissionGroup, parent=None, default_params=None):
         super().__init__(parent)
         self.group = group
-        self._is_all_selected = False  # 追蹤全選狀態
         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):
@@ -373,23 +387,23 @@ class GroupPanel(QWidget):
         # 每種任務類型的參數定義: (key, label, default_value)
         self._param_defs = {
             'M_FORMATION': [
-                ('spacing', '間距 (m)', '5.0'),
-                ('base_altitude', '基準高度 (m)', '10.0'),
-                ('altitude_diff', '高低差 (m)', '2.0'),
+                ('spacing', '間距 (m)', self.default_params['spacing']),
+                ('base_altitude', '基準高度 (m)', self.default_params['base_altitude']),
+                ('altitude_diff', '高低差 (m)', self.default_params['altitude_diff']),
             ],
             'CIRCLE_FORMATION': [
-                ('radius', '半徑 (m)', '10.0'),
-                ('altitude', '高度 (m)', '10.0'),
-                ('start_angle', '起始角 (°)', '0'),
+                ('radius', '半徑 (m)', self.default_params['radius']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('start_angle', '起始角 (°)', self.default_params['start_angle']),
             ],
             'LEADER_FOLLOWER': [
-                ('lateral_offset', '橫向偏移 (m)', '3.0'),
-                ('longitudinal_spacing', '縱向間距 (m)', '5.0'),
-                ('altitude', '高度 (m)', '10.0'),
+                ('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
+                ('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
             ],
             'GRID_SWEEP': [
-                ('line_spacing', '掃描線距 (m)', '5.0'),
-                ('altitude', '高度 (m)', '10.0'),
+                ('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
             ],
         }
 
@@ -454,11 +468,11 @@ class GroupPanel(QWidget):
 
     def update_drone_list(self):
         """更新無人機列表顯示"""
-        if not self.group.drone_ids:
+        if not self.group.selected_drone_ids:
             self.drone_list_label.setText("尚未分配")
             self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
         else:
-            sorted_ids = sorted(self.group.drone_ids,
+            sorted_ids = sorted(self.group.selected_drone_ids,
                                 key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
             self.drone_list_label.setText("、".join(sorted_ids))
             self.drone_list_label.setStyleSheet(
@@ -467,7 +481,7 @@ class GroupPanel(QWidget):
 
     def _refresh_leader_options(self):
         """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
-        sorted_ids = sorted(self.group.drone_ids,
+        sorted_ids = sorted(self.group.selected_drone_ids,
                             key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
         current = self.group.leader_drone_id
         self._leader_combo.blockSignals(True)
@@ -498,7 +512,7 @@ class GroupPanel(QWidget):
             self.status_label.setText("⏸ 已暫停")
             self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
         else:
-            n = len(self.group.drone_ids)
+            n = len(self.group.selected_drone_ids)
             total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
             self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
             self.status_label.setStyleSheet(
@@ -508,10 +522,6 @@ class GroupPanel(QWidget):
         """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
         self.select_all_requested.emit(self.group.group_id)
     
-    def set_all_select_state(self, is_selected):
-        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
-        self._is_all_selected = is_selected
-    
     def set_delete_enabled(self, enabled):
         """啟用或禁用刪除群組按鈕"""
         self.delete_group_btn.setEnabled(enabled)
@@ -537,6 +547,13 @@ class GroupPanel(QWidget):
                     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;"
@@ -558,4 +575,3 @@ class GroupPanel(QWidget):
         except ValueError:
             alt = 10.0
         self.takeoff_requested.emit(self.group.group_id, alt)
-

src/GUI/overview_table.py

@@ -7,7 +7,7 @@ class OverviewTable(QTableWidget):
     
     # 默認的資訊類型和映射
     DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
-                          "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+                          "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {
         "mode": 0,

src/fc_interfaces/CMakeLists.txt

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

src/fc_interfaces/msg/AttitudeRaw.msg

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

src/fc_interfaces/package.xml

@@ -15,6 +15,9 @@
   <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>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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 = "v0.61"
+PROJECT_VER = "v0.70"
 
 class PanelState:
     def __init__(self):
@@ -45,7 +45,7 @@ class PanelState:
 
         # 關於創建通道時的暫存資訊
         self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊
-        self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊
+        self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": True} # 暫存 Serial 設定資訊
 
         # 關於顯示通道資訊
         self.socket_info_single = {
@@ -426,7 +426,7 @@ class ControlPanel:
                         menu_stack.pop()
                         idx_stack.pop()
                     elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
-                        state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)"
+                        state.serial_info_temp["CommunicationType"] = "TELEMETRY"
                         menu_stack.pop()
                         idx_stack.pop()
 
@@ -767,7 +767,7 @@ 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("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
+                        MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                     ]),
                     MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
                     MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[
@@ -1058,11 +1058,15 @@ class ControlPanel:
 
             # MAVLink 訊息名稱對應（縮寫版本）
             MSG_NAMES = {
-                0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU",
+                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"
+                74: "VFR_HUD", 147: "BATT_ST", 136: "TERRAIN", 241: "VIBRAT",
+                125: "POW_STA",
             }
 
+            # ardupilot mega 
+            # 178: "AHRS2", 163: "AHRS", 193: "EKF_STAT", 
+
             # 顯示前 12 個最常見的訊息類型（兩列各 6 個）
             msg_items = list(msg_counts.items())[:12]
             line = 13
@@ -1134,6 +1138,7 @@ class Orchestrator:
         self.bridge = mo.mavlink_bridge()
 
         # === 3) serial_manager 部分的準備 ===
+        sm.rx_module_ack.clear()
         self.plumber = sm.serial_manager()
 
         # === 4) ros 部分的準備 ===
@@ -1243,7 +1248,7 @@ class Orchestrator:
                             if serial_obj:
                                 self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port
                                 self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate
-                                self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name
+                                self.panelState.serial_info_single["receiver_type"] = serial_obj.serial_mode.name
                                 self.panelState.serial_info_single["target_port"] = serial_obj.target_port
                                 self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好
                         elif action == "REMOVE_LINKED_SERIAL":
@@ -1516,8 +1521,8 @@ class Orchestrator:
 
         # 定義通訊類型映射表
         COMM_TYPE_MAP = {
-            "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT,
-            # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY,  # TODO: 之後再弄
+            "XBee(API-AT)": sm.SerialMode.XBEEAPI2AT,
+            "TELEMETRY": sm.SerialMode.STRAIGHT,
             # 新增區
         }
 
@@ -1541,7 +1546,7 @@ class Orchestrator:
             serial_port=self.panelState.serial_info_temp['Port'],
             baudrate=self.panelState.serial_info_temp['Baud'],
             target_port=udp_port_tmp,
-            receiver_type=comm_type_tmp,
+            serial_mode=comm_type_tmp,
         )
 
         if not ret:
@@ -1572,7 +1577,7 @@ def main():
     if mvv.MODULE_VER != "1.00":
         print("Module Version Error! : mavlinkVehicleView")
         version_check = False
-    if sm.MODULE_VER != "0.60":
+    if sm.MODULE_VER != "0.80":
         print("Module Version Error! : serialManager")
         version_check = False
     if version_check == False:

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -230,6 +230,7 @@ class mavlink_bridge:
             'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
         }
 
+
     def _handle_global_position(self, vehicle, component, msg, timestamp):
         """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
         component.status.position.latitude = msg.lat / 1e7  # 轉換為度

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -32,6 +32,7 @@ import mavros_msgs.msg
 
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
+import fc_interfaces.msg as fcmsg
 from .ackEnum import serviceAckResult
 
 # 自定義 imports
@@ -210,7 +211,9 @@ class VehicleStatusPublisher(Node):
         if pos.latitude is None or pos.longitude is None:
             return
         
-        publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix)
+        publisher = self._get_or_create_publisher(
+            sysid, 'position', sensor_msgs.msg.NavSatFix
+            )
         
         # 檢查是否有訂閱者
         if publisher.get_subscription_count() == 0:
@@ -229,7 +232,7 @@ class VehicleStatusPublisher(Node):
         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
 
@@ -243,7 +246,7 @@ class VehicleStatusPublisher(Node):
 
         publisher = self._get_or_create_publisher(
             sysid, 'position_ned', nav_msgs.msg.Odometry
-        )
+            )
 
         if publisher.get_subscription_count() == 0:
             return
@@ -265,34 +268,28 @@ class VehicleStatusPublisher(Node):
         publisher.publish(msg)
     
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
-        """發布姿態（IMU）"""
+        """發布原始姿態（roll/pitch/yaw 與角速度）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
             return
         
         att = status.attitude
-        if att.roll is None:
+        if att.roll is None or att.pitch is None or att.yaw is None:
             return
-        publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
+        publisher = self._get_or_create_publisher(
+            sysid, 'attitude', fcmsg.AttitudeRaw
+        )
         
         if publisher.get_subscription_count() == 0:
             return
         
-        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
+        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
         
         publisher.publish(msg)
     
@@ -1135,7 +1132,8 @@ ros2_manager = fc_ros_manager()
 2026.04.07
 1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
 2. 優化 response.ack_result 回傳值的資訊
-
+3. 新增 _publish_position_ned() 發布剛體座標的訊息
+4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
 
 TODO 
 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -16,29 +16,55 @@ import threading
 import struct
 from enum import Enum, auto
 from abc import ABC, abstractmethod
+from dataclasses import dataclass
 
 # # XBee 模組
 # from xbee.frame import APIFrame
 
 # 自定義的 import
-from .utils import setup_logger
+from .utils import RingBuffer, setup_logger
 
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
-MODULE_VER = "0.60"
+MODULE_VER = "0.80"
 
-# ====================== 分割線 =====================
+rx_module_ack = RingBuffer(capacity=64, buffer_id=253)
+
+# ====================== State DEFINITION =====================
 
 # 定義 serial 連線的模式
 class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
     XBEEAPI2AT = auto()    # XBee API 模式
+    XBEEAPI_POLL = auto()
     NOT_USE = auto()       # 不使用
 
 
-# ====================== Frame Processor 基類與實現 =====================
+# ====================== AT Frame Data Classes =====================
+
+@dataclass
+class ATRequest:
+    """送往 dongle 的 AT 指令"""
+    command: bytes      # 例如 b'DB'
+    parameter: bytes    # 寫入用指令的參數，讀取型通常為空
+    frame_id: int       # XBee frame id；0x00 表示不要求回應
+
+
+@dataclass
+class ATResponse:
+    """dongle 回傳的 AT Response (0x88)"""
+    frame_id: int
+    command: bytes
+    status: int
+    data: bytes
+
+    @property
+    def is_ok(self) -> bool:
+        return self.status == 0x00
+
+# ====================== Frame Processor Base and Implementation =====================
 
 class FrameProcessor(ABC):
     """協議處理器基類"""
@@ -47,10 +73,10 @@ class FrameProcessor(ABC):
         self.buffer = bytearray()
     
     @abstractmethod
-    def process_incoming(self, data: bytes):
+    def process_incoming(self, data: bytes) -> bytes:
         """
         處理接收到的數據
-        返回：已完整解析的 payload 列表
+        返回：已完整解析的 payload 列表 後續要丟到 UDP 去
         """
         pass
     
@@ -66,7 +92,7 @@ class FrameProcessor(ABC):
 class RawFrameProcessor(FrameProcessor):
     """原始數據直通處理器"""
     
-    def process_incoming(self, data: bytes):
+    def process_incoming(self, data: bytes) -> bytes:
         """直接返回原始數據，不進行緩衝"""
         return [data] if data else []
     
@@ -76,110 +102,108 @@ class RawFrameProcessor(FrameProcessor):
 
 
 class XBeeFrameProcessor(FrameProcessor):
-    """XBee API 協議處理器"""
-    
-    def process_incoming(self, data: bytes):
+    """
+    XBee API 協議處理器
+
+    職責：
+    - XBee API frame 的拆幀 / 組幀
+    - 0x90 (RX Packet)   -> 解出 payload 回傳
+    - 0x88 (AT Response) -> 轉交 at_handler 處理（若有注入）
+    - 0x8B (TX Status)   -> 目前忽略
+    - 其他 frame type    -> 記 warning 忽略
+
+    若未來要做變化型 XBee（例如 API2 escape mode、不同 addressing），
+    繼承此類並覆寫 _encapsulate / _decapsulate / _try_extract_frame 即可。
+    """
+
+    # XBee API frame type
+    FRAME_TYPE_TX_REQUEST  = 0x10
+    FRAME_TYPE_AT_RESPONSE = 0x88
+    FRAME_TYPE_TX_STATUS   = 0x8B
+    FRAME_TYPE_RX_PACKET   = 0x90
+
+    def __init__(self, at_handler: "ATCommandHandler" = None):
+        super().__init__()
+        self.at_handler = at_handler
+
+    # ---- 對外契約 ----
+    def process_incoming(self, data: bytes) -> bytes:
         """處理 XBee API 幀並提取 payload"""
         self.buffer.extend(data)
         payloads = []
-        
+
+        while True:
+            frame = self._try_extract_frame()
+            if frame is None:
+                break
+            payload = self._dispatch_frame(frame)
+            if payload:
+                payloads.append(payload)
+
+        return payloads
+
+    def process_outgoing(self, data: bytes) -> bytes:
+        """將數據封裝為 XBee API 傳輸幀"""
+        return self._encapsulate(data)
+
+    # ---- 內部：拆幀與分派 ----
+    def _try_extract_frame(self) -> bytes:
+        """
+        從 buffer 嘗試抓一個完整 frame
+        - 對齊幀頭 0x7E
+        - 長度不足時等待下次進 buffer
+        - 成功則從 buffer 切掉並回傳整個 frame bytes
+        """
         while len(self.buffer) >= 3:
-            # 尋找幀頭
             if self.buffer[0] != 0x7E:
                 self.buffer.pop(0)
                 continue
-            
-            # 讀取 payload 長度
+
             length = (self.buffer[1] << 8) | self.buffer[2]
             full_length = 3 + length + 1  # 起始符(1) + 長度(2) + payload + 校驗和(1)
-            
-            # 等待完整幀
+
             if len(self.buffer) < full_length:
-                break
-            
-            # 提取完整 frame 並從緩衝區移除
+                return None
+
+            # TODO: 驗證 checksum：(sum(frame[3:]) & 0xFF) 應為 0xFF；不通過則丟棄此 frame 並從 buffer pop 1 byte 再重新對齊
+
             frame = bytes(self.buffer[:full_length])
             del self.buffer[:full_length]
-            
-            # 判斷 frame 類型並處理
-            frame_type = frame[3]
-            
-            if frame_type == 0x90:  # RX Packet
-                payload = XBeeFrameHandler.decapsulate_data(frame)
-                if payload:
-                    payloads.append(payload)
-            elif frame_type == 0x88:  # AT Response
-                # 可以在這裡處理 AT 指令回應
-                # response = XBeeFrameHandler.parse_at_command_response(frame)
-                # 目前忽略
-                pass
-            elif frame_type == 0x8B:  # Transmit Status
-                # 傳輸狀態，目前忽略
-                pass
-            else:
-                logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
-        
-        return payloads
-    
-    def process_outgoing(self, data: bytes) -> bytes:
-        """將數據封裝為 XBee API 傳輸幀"""
-        return XBeeFrameProcessor.encapsulate_data(data)
+            return frame
 
-
-# ====================== XBee Frame Handler =====================
-
-class XBeeFrameHandler:
-    """XBee API Frame 處理器"""
-    
-    @staticmethod
-    def parse_at_command_response(frame: bytes) -> dict:
-        """解析 AT Command Response (0x88)"""
-        if len(frame) < 8:
-            return None
-        
-        frame_type = frame[3]
-        if frame_type != 0x88:
-            return None
-        
-        frame_id = frame[4]
-        at_command = frame[5:7]
-        status = frame[7]
-        data = frame[8:] if len(frame) > 8 else b''
-        
-        return {
-            'frame_id': frame_id,
-            'command': at_command,
-            'status': status,
-            'data': data,
-            'is_ok': status == 0x00
-        }
-    
-    @staticmethod
-    def parse_receive_packet(frame: bytes) -> dict:
-        """解析 RX Packet (0x90) - 未來擴展用"""
-        # if len(frame) < 15 or frame[3] != 0x90:
-        #     return None
-        
-        # return {
-        #     'source_addr': frame[4:12],
-        #     'reserved': frame[12:14],
-        #     'options': frame[14],
-        #     'data': frame[15:-1]
-        # }
-        pass
         return None
 
+    def _dispatch_frame(self, frame: bytes) -> bytes:
+        """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
+        frame_type = frame[3]
+
+        if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
+            return self._decapsulate(frame)
+
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
+            if self.at_handler is not None:
+                self.at_handler.handle_frame(frame)
+            return None
+
+        if frame_type == self.FRAME_TYPE_TX_STATUS:
+            return None
+
+        logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+        return None
+
+    # ---- 內部：編解碼 ----
     @staticmethod
-    def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes:
+    def _encapsulate(
+        data: bytes,
+        dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF',
+        frame_id: int = 0x01,
+    ) -> bytes:
         """
-        將數據封裝為 XBee API 傳輸幀
-        
-        使用 XBee API 格式封裝數據:
-        - 傳輸請求幀 (0x10)
+        將 payload 包成 XBee TX Request (0x10)
         - 使用廣播地址
         - 添加適當的頭部和校驗和
         """
-        frame_type = 0x10
+        frame_type = XBeeFrameProcessor.FRAME_TYPE_TX_REQUEST
         dest_addr16 = b'\xFF\xFE'
         broadcast_radius = 0x00
         options = 0x00
@@ -191,89 +215,178 @@ class XBeeFrameHandler:
         return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
 
     @staticmethod
-    def decapsulate_data(data: bytes):
-        
-        # 獲取數據長度 (不包括校驗和)
-        length = (data[1] << 8) | data[2]
-
+    def _decapsulate(frame: bytes) -> bytes:
+        """從 RX Packet (0x90) 取出 payload"""
+        length = (frame[1] << 8) | frame[2]
         rf_data_start = 3 + 12
-        return data[rf_data_start:3 + length]
+        return frame[rf_data_start:3 + length]
 
 
+# ====================== Dongle Command Handler =====================
+
 class ATCommandHandler:
-    """AT 指令回應處理器"""
-    
+    """
+    XBee AT 指令處理器
+
+    職責：
+    - 組 AT Command Request frame (0x08) 並透過注入的 writer 送出
+    - 解析 AT Response frame (0x88)
+    - 依 AT 指令分派到對應的 handler
+
+    writer 由 SerialHandler 在連線建立後 (connection_made) 注入，
+    型別為 Callable[[bytes], None]（通常就是 serial transport.write）。
+    """
+
+    FRAME_TYPE_AT_COMMAND = 0x08
+
     def __init__(self, serial_port: str):
         self.serial_port = serial_port
+        self.writer = None
         self.handlers = {
             b'DB': self._handle_rssi,
             b'SH': self._handle_serial_high,
             b'SL': self._handle_serial_low,
             # 可擴展其他 AT 指令
         }
-    
-    def handle_response(self, response: dict):
-        """根據 AT 指令類型分派處理"""
-        if not response or not response['is_ok']:
-            if response:
-                logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+
+    # ---- 發送端 ----
+    def set_writer(self, writer):
+        """由 SerialHandler 注入實際寫入 serial 的 callable"""
+        self.writer = writer
+
+    def send_command(self, request: ATRequest):
+        """發送一筆 AT 指令給 dongle"""
+        if self.writer is None:
+            logger.warning(
+                f"[{self.serial_port}] AT writer 尚未就緒，指令 {request.command!r} 丟棄"
+            )
             return
-        
-        command = response['command']
-        handler = self.handlers.get(command)
-        
+
+        self.writer(self._build_at_request(request))
+        # logger.debug(
+        #     f"[{self.serial_port}] send AT {request.command.decode(errors='replace')} "
+        #     f"(frame_id=0x{request.frame_id:02X})"
+        # ) # dev
+
+    @staticmethod
+    def _build_at_request(request: ATRequest) -> bytes:
+        """將 ATRequest 組成 XBee API AT Command Request frame (0x08) bytes"""
+        frame_type = ATCommandHandler.FRAME_TYPE_AT_COMMAND
+
+        frame = struct.pack(">B", frame_type) + struct.pack(">B", request.frame_id)
+        frame += request.command + request.parameter
+        checksum = 0xFF - (sum(frame) & 0xFF)
+        return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+    # ---- 接收端 ----
+    def handle_frame(self, frame: bytes) -> None:
+        """
+        接收一整個 AT Response frame：
+        1. 解析成 ATResponse
+        2. 推進 rx_module_ack 供其他模組消費
+        3. 本地 dispatch 給對應的 _handle_xxx
+        """
+        parsed = self._parse(frame)
+        if parsed is None:
+            return
+
+        if not rx_module_ack.put(parsed):
+            logger.warning(
+                f"[{self.serial_port}] rx_module_ack overflow, drop {parsed.command!r}"
+            )
+
+        self._dispatch(parsed)
+
+    @staticmethod
+    def _parse(frame: bytes) -> ATResponse:
+        """解析 AT Command Response (0x88)；不符格式回傳 None"""
+        if len(frame) < 8:
+            return None
+
+        if frame[3] != 0x88:
+            return None
+
+        return ATResponse(
+            frame_id=frame[4],
+            command=frame[5:7],
+            status=frame[7],
+            data=frame[8:] if len(frame) > 8 else b'',
+        )
+
+    def _dispatch(self, response: ATResponse) -> None:
+        """根據 AT 指令類型分派處理"""
+        # print(f"[{self.serial_port}] AT Response: {response}")  # dev
+
+        if not response.is_ok:
+            logger.warning(
+                f"[{self.serial_port}] AT {response.command.decode()} "
+                f"失敗，狀態碼: {response.status}"
+            )
+            return
+
+        handler = self.handlers.get(response.command)
         if handler:
-            handler(response['data'])
+            handler(response.data)
         else:
-            logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
-    
+            logger.debug(
+                f"[{self.serial_port}] 未處理的 AT 指令: "
+                f"{response.command.decode()}"
+            )
+
     def _handle_rssi(self, data: bytes):
-        """處理 DB (RSSI) 回應"""
-        # 未來可實現 RSSI 處理邏輯
+        """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
         pass
-    
+        if data:
+            print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+            # logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
+            pass
+        
     def _handle_serial_high(self, data: bytes):
         """處理 SH (Serial Number High)"""
         pass
-    
+
     def _handle_serial_low(self, data: bytes):
         """處理 SL (Serial Number Low)"""
         pass
 
 
-# ====================== Serial Handler =====================
-
+# ================ Serial UDP Socket Object ==============
 class SerialHandler(asyncio.Protocol):
     """asyncio.Protocol 用於處理 Serial 收發"""
-    
+
     def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode):
         self.udp_handler = udp_handler  # UDP 的傳輸物件
         self.serial_port_str = serial_port_str
         self.serial_mode = serial_mode
         self.transport = None  # Serial 自己的傳輸物件
-        
-        # 根據模式創建對應的 processor
-        self.processor = self._create_processor(serial_mode)
-        
-        # AT 指令處理器（僅 XBee 模式使用）
-        if serial_mode == SerialMode.XBEEAPI2AT:
-            self.at_handler = ATCommandHandler(serial_port_str)
-        else:
-            self.at_handler = None
 
-    def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor:
-        """工廠方法：根據模式創建處理器"""
-        if serial_mode == SerialMode.STRAIGHT:
-            return RawFrameProcessor()
-        elif serial_mode == SerialMode.XBEEAPI2AT:
-            return XBeeFrameProcessor()
-        else:
-            logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
+        # 根據模式建立 processor（需要 AT handler 時一併在工廠內建好注入）
+        self.processor = self._create_processor()
+
+    def _create_processor(self) -> FrameProcessor:
+        """工廠方法：根據 self.serial_mode 建立對應的 processor，必要時一併注入 AT handler"""
+        if self.serial_mode == SerialMode.STRAIGHT:
             return RawFrameProcessor()
 
+        if self.serial_mode == SerialMode.XBEEAPI2AT:
+            at_handler = ATCommandHandler(self.serial_port_str)
+            return XBeeFrameProcessor(at_handler=at_handler)
+
+        # if self.serial_mode == SerialMode.XBEEAPI_POLL:
+        #     at_handler = ATCommandHandler_new(self.serial_port_str)
+        #     return XBeeFrameProcessor(at_handler=at_handler)
+
+        logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
+        return RawFrameProcessor()
+
     def connection_made(self, transport):
         """連接建立時的回調"""
         self.transport = transport
+
+        # XBee 模式下，將 transport.write 注入給 AT handler，讓它能送指令
+        if self.serial_mode == SerialMode.XBEEAPI2AT:
+            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")
@@ -291,8 +404,6 @@ class SerialHandler(asyncio.Protocol):
             )
 
 
-# ====================== UDP Handler =====================
-
 class UDPHandler(asyncio.DatagramProtocol):
     """asyncio.DatagramProtocol 用於處理 UDP 收發"""
     
@@ -561,6 +672,33 @@ class serial_manager:
             ret[key] = obj.serial_port
         return ret
 
+    def send_at_command(self, serial_id, request: ATRequest) -> bool:
+        """
+        對指定 serial_id 的 XBee dongle 發送一筆 AT 指令（thread-safe）
+        - serial_id: create_serial_link 取得的編號
+        - request: ATRequest 物件，攜帶 command / parameter / frame_id
+        回傳是否成功排進事件圈
+        """
+        if not self.running or not self.loop:
+            logger.error("Event loop not running, cannot send AT command")
+            return False
+
+        if serial_id not in self.serial_objects:
+            logger.error(f"Serial object {serial_id} not found")
+            return False
+
+        serial_obj = self.serial_objects[serial_id]
+        if serial_obj.serial_mode != SerialMode.XBEEAPI2AT:
+            logger.error(
+                f"Serial {serial_id} mode is {serial_obj.serial_mode.name}, "
+                f"AT command only supported in XBEEAPI2AT mode"
+            )
+            return False
+
+        at_handler = serial_obj.serial_handler.processor.at_handler
+        self.loop.call_soon_threadsafe(at_handler.send_command, request)
+        return True
+
     @staticmethod
     def check_serial_port(serial_port, baudrate):
         """檢查串口是否存在與可用"""
@@ -598,20 +736,42 @@ if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
 
-    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
-    SERIAL_BAUDRATE = 115200
-    UDP_REMOTE_PORT = 14571
-    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-
     # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
     # SERIAL_BAUDRATE = 115200
     # UDP_REMOTE_PORT = 14571
     # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
+    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
+    SERIAL_BAUDRATE = 115200
+    UDP_REMOTE_PORT = 14561
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
+
     linked_serial = sm.get_serial_link()
     print(linked_serial)
-    time.sleep(60)
+
+    # 等 connection_made 完成 writer 注入，再發一筆 AT 指令測試
+    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(3)
     sm.shutdown()
+
+
+
+'''
+================= 改版記錄 ============================
+2026.4.20
+1. XBeeFrameHandler 結構移除
+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 (未實作)
+
+'''
+

src/fc_network_adapter/package.xml

@@ -7,6 +7,13 @@
   <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/unitdev02/unitdev02/lifecycle_node.py

@@ -1,45 +0,0 @@
-
-
-
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-import time
-
-# import mavros_msgs.srv
-
-class TalkerNode(Node):
-    def __init__(self):
-        start_time = time.time()
-        super().__init__('talker_node')
-        end_time = time.time()
-        print(f"Node initialization took {end_time - start_time:.2f} seconds")
-
-        self.publisher_ = self.create_publisher(String, 'hahatest/_1', 10)
-        self.timer = self.create_timer(1.0, self.timer_callback)  # 每秒執行一次
-        self.get_logger().info('TalkerNode has been started.')
-
-    def timer_callback(self):
-        msg = String()
-        msg.data = 'Hello, ROS 2!'
-        self.publisher_.publish(msg)
-        self.get_logger().info(f'Published: "{msg.data}"')
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = TalkerNode()
-
-    print("Before sleep")
-    time.sleep(5)  # 等待 5 秒鐘
-    print("After sleep")
-    try:
-        start_time = time.time()
-        while time.time() - start_time < 10:  # 持續 10 秒鐘
-            rclpy.spin_once(node)
-            time.sleep(1)  # 每秒執行一次
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/unitdev02/unitdev02/ping_test.py

@@ -0,0 +1,188 @@
+import time
+from pymavlink import mavutil
+
+# # 強制 source_system=255
+# master = mavutil.mavlink_connection('udpout:localhost:14561', source_system=255)
+
+# print("Starting Force-Ping Test...")
+
+# while True:
+#     # 1. 發送 Heartbeat (證明連線活著)
+#     master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, 
+#                              mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
+    
+#     # 2. 強制發送 PING (target 1, 1)
+#     # 我們加一個計數器，方便在 tcpdump 裡辨識
+#     ping_seq = int(time.time()) % 100
+#     master.mav.ping_send(int(time.time() * 1e6), ping_seq, 1, 1)
+    
+#     print(f"Sent Heartbeat and PING (seq {ping_seq})")
+    
+#     # 3. 非阻塞讀取，避免卡死
+#     msg = master.recv_match(blocking=False)
+#     if msg:
+#         print(f"--> Received something: {msg.get_type()}")
+        
+#     time.sleep(1) # 每秒噴一次
+
+# -------------------------------------------------------------------
+
+# # 改成 udpin，這會讓你的程式守在 14561 等模擬器送資料進來
+# # 這樣你就能收到模擬器的 Heartbeat，建立連線後再發 PING
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# print("Waiting for simulator heartbeat...")
+# # 這一行很重要：先收到 Heartbeat，pymavlink 才會設定好 target 地址
+# master.wait_heartbeat()
+# print(f"Heartbeat from system {master.target_system} component {master.target_component}")
+
+# while True:
+#     # 既然收到了，現在發 PING 就會順著原路回去
+#     master.mav.ping_send(int(time.time() * 1e6), 1, master.target_system, master.target_component)
+#     print("PING sent!")
+    
+#     msg = master.recv_match(type='PING', blocking=True, timeout=2)
+#     if msg:
+#         print(f"Got PING response: {msg}")
+#     time.sleep(1)
+
+
+# -------------------------------------------------------------------
+
+# import time
+# from pymavlink import mavutil
+
+# # 核心修改：因為 MAVProxy 正在往 14561 噴資料，我們必須「接住」它
+# # 使用 udpin 會讓你的程式在 14561 監聽
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=254)
+
+# print("正在等待 MAVProxy 轉發的 Heartbeat...")
+
+# # 只要這行跑過，代表你跟 MAVProxy 握手成功了
+# master.wait_heartbeat()
+# print(f"成功連線到系統 {master.target_system}！")
+
+# while True:
+#     # 1. 務必發送 Heartbeat，讓 MAVProxy 知道要把回應送回這個 Port
+#     master.mav.heartbeat_send(
+#         mavutil.mavlink.MAV_TYPE_GCS, 
+#         mavutil.mavlink.MAV_AUTOPILOT_INVALID, 
+#         0, 0, 0
+#     )
+
+#     # 2. 修改 target_component 為 1 (飛控核心)
+#     # 注意：這裡刻意將 seq 設為動態，方便你在 watch PING 裡觀察
+#     p_seq = int(time.time()) % 255
+#     master.mav.ping_send(
+#         int(time.time() * 1e6), 
+#         p_seq, 
+#         1, 
+#         1  # <--- 改成 1 試試看！
+#     )
+#     print(f"Sent PING seq {p_seq} to 1/1")
+
+#     # 3. 讀取所有訊息，看看有沒有 PING 回來
+#     # 有時候回應的 target_system 可能是 255 (你的 ID)
+#     msg = master.recv_match(type='PING', blocking=True, timeout=1.0)
+#     if msg:
+#         print(f"🎉 成功！收到回應: {msg}")
+    
+#     time.sleep(1)
+
+
+
+# -------------------------------------------------------------------
+
+
+# import time
+# from pymavlink import mavutil
+
+# # 建議一樣用 udpin 監聽 MAVProxy 的輸出
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# def get_time_ns():
+#     return int(time.time() * 1e9) # 轉為奈秒
+
+# print("開始測試 TIMESYNC 響應...")
+
+# while True:
+#     now_ns = get_time_ns()
+    
+#     # 發送 TIMESYNC 請求
+#     # tc1=0 代表這是請求，ts1=當前時間
+#     master.mav.timesync_send(0, now_ns)
+    
+#     # 等待回應
+#     msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+    
+#     if msg and msg.tc1 != 0:
+#         # 收到回應，msg.tc1 應該等於我們剛才送出的 now_ns
+#         rtt_ms = (get_time_ns() - msg.tc1) / 1e6
+#         print(f"🕒 TIMESYNC RTT: {rtt_ms:.3f} ms")
+#         break # 測試一次就好
+#     else:
+#         print("等待 TIMESYNC 回應中...")
+        
+#     time.sleep(1)
+
+# -------------------------------------------------------------------
+
+import time
+import statistics
+from pymavlink import mavutil
+
+# 連接到 MAVProxy 轉發的端口
+master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# 測試設定
+TARGET_SYS = 2  # 你想測試的載具 ID
+SAMPLES = 20    # 取 20 次樣本來算平均和抖動
+rtt_history = []
+
+print(f"開始測試對 System {TARGET_SYS} 的通道品質...")
+
+def get_time_ns():
+    return int(time.time() * 1e9)
+
+try:
+    while len(rtt_history) < SAMPLES:
+        ts1_send = get_time_ns()
+        
+        # 發送 TIMESYNC (V1/V2 格式相容)
+        # 針對特定系統發送
+        master.mav.timesync_send(0, ts1_send)
+        
+        # 等待回應 (過濾目標系統的訊息)
+        msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+        
+        if msg and msg.get_srcSystem() == TARGET_SYS and msg.tc1 != 0:
+            ts1_recv = get_time_ns()
+            rtt_ns = ts1_recv - msg.tc1
+            rtt_ms = rtt_ns / 1e6
+            rtt_history.append(rtt_ms)
+            print(f"Sample {len(rtt_history)}: RTT = {rtt_ms:.3f} ms")
+        else:
+            print("Request timeout or mismatched ID...")
+            
+        time.sleep(0.1) # 快速採樣
+
+    # 4. 計算統計數據
+    avg_latency = statistics.mean(rtt_history)
+    std_dev = statistics.stdev(rtt_history) # 這就是 Jitter 的一種表現
+    max_rtt = max(rtt_history)
+    min_rtt = min(rtt_history)
+
+    print("\n--- 物理通道品質分析報告 ---")
+    print(f"目標載具  : System {TARGET_SYS}")
+    print(f"平均延遲 (Avg) : {avg_latency:.3f} ms")
+    print(f"最小延遲 (Min) : {min_rtt:.3f} ms")
+    print(f"最大延遲 (Max) : {max_rtt:.3f} ms")
+    print(f"抖動幅度 (Jitter): {std_dev:.3f} ms (標準差)")
+    
+    if std_dev > 10:
+        print("警告：網路抖動過高，可能影響控制穩定性！")
+    else:
+        print("通道品質：良好")
+
+except KeyboardInterrupt:
+    print("測試終止。")

src/unitdev02/unitdev02/pymav_nullbuffer.py

@@ -0,0 +1,46 @@
+from pymavlink.dialects.v20 import common as mavlink
+
+class NullBuffer:
+    def write(self, data): pass
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+class CapturingBuffer:
+    def __init__(self):
+        self.last_data = b''
+    def write(self, data):
+        self.last_data = bytes(data)
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+# 1. 初始化（僅作為編碼器）
+# file 參數可以是任何擁有 write() 方法的物件，這裡用 BytesIO 模擬
+# 初始化方法一：使用 BytesIO
+# import io
+# out_buf = io.BytesIO()
+# mav = mavlink.MAVLink(out_buf, srcSystem=1, srcComponent=191)
+
+# 初始化方法二：使用自定義的 NullBuffer
+mav = mavlink.MAVLink(NullBuffer(), srcSystem=1, srcComponent=191)
+mav.seq = 254
+
+# 2. 建立心跳包並取得二進制數據
+# 連續產出「不同」的訊息物件
+for i in range(3):
+    msg = mav.heartbeat_encode(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = msg.pack(mav)
+    mav.seq = (mav.seq + 1) & 0xFF
+    # MAVLink 2 的序列號在第 3 個 Byte (Index 2)
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")
+
+
+
+print("分隔線")
+
+buf = CapturingBuffer()
+mav_buf2 = mavlink.MAVLink(buf, srcSystem=1, srcComponent=191)
+
+for i in range(3):
+    mav_buf2.heartbeat_send(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = buf.last_data
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")

src/unitdev02/unitdev02/serial_udp_ad.py

@@ -0,0 +1,71 @@
+import asyncio
+import serial_asyncio
+
+SERIAL_PORT = '/dev/ttyUSB0'   # 修改為你的 serial port
+SERIAL_BAUDRATE = 115200       # 修改為你的 baudrate
+UDP_REMOTE_IP = '192.168.1.100' # 修改為目標 IP
+UDP_REMOTE_PORT = 5005         # 修改為目標 port
+UDP_LOCAL_PORT = 5006          # 本地 UDP 監聽 port
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_transport):
+        self.udp_transport = udp_transport
+
+    def connection_made(self, transport):
+        self.transport = transport
+
+    def data_received(self, data):
+        # Serial 收到資料，轉發到 UDP
+        self.udp_transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def write_to_serial(self, data):
+        self.transport.write(data)
+
+class UDPToSerial(asyncio.DatagramProtocol):
+    def __init__(self, serial_proto):
+        self.serial_proto = serial_proto
+
+    def datagram_received(self, data, addr):
+        # UDP 收到資料，轉發到 Serial
+        self.serial_proto.write_to_serial(data)
+
+async def main():
+    loop = asyncio.get_running_loop()
+
+    # 定義協議工廠函數
+    def create_empty_protocol():
+        return asyncio.DatagramProtocol()
+
+    # 建立 UDP1 傳輸
+    udp_transport, _ = await loop.create_datagram_endpoint(
+        create_empty_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 建立 Serial 傳輸
+    serial_proto = SerialToUDP(udp_transport)
+    def get_serial_protocol():
+        return serial_proto
+
+    _, serial_transport = await serial_asyncio.create_serial_connection(
+        loop, get_serial_protocol, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+    )
+
+    # 建立 UDP2 監聽
+    udp_proto = UDPToSerial(serial_proto)
+    def get_udp_protocol():
+        return udp_proto
+
+    udp_listen_transport, _ = await loop.create_datagram_endpoint(
+        get_udp_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 保持運行
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+if __name__ == '__main__':
+    asyncio.run(main())

src/unitdev02/unitdev02/sslChech.sh

@@ -1,27 +0,0 @@
-#!/bin/bash
-
-# 網站清單
-DOMAINS=("google.com" "smarter.nchu.edu.tw")
-
-echo "網站 SSL 憑證剩餘天數："
-echo "---------------------------"
-
-for domain in "${DOMAINS[@]}"; do
-    end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null |
-        openssl x509 -noout -enddate | cut -d= -f2)
-
-    end_timestamp=$(date -d "$end_date" +%s)
-    now_timestamp=$(date +%s)
-
-    remaining_days=$(( (end_timestamp - now_timestamp) / 86400 ))
-
-    if [ $remaining_days -lt 0 ]; then
-        status="已過期 ❌"
-    elif [ $remaining_days -lt 15 ]; then
-        status="即將到期 ⚠️"
-    else
-        status="正常 ✅"
-    fi
-
-    printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status"
-done

src/unitdev02/unitdev02/udp_unix_ad.py

@@ -0,0 +1,40 @@
+import socket
+import sys
+import select
+
+# 設定來源 IP 與 Port
+SRC_IP = '127.0.0.1'    # 監聽所有介面
+SRC_PORT = 16661      # 請自行修改
+
+# 建立 UDP 監聽 socket
+src_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+src_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+src_sock.bind((SRC_IP, SRC_PORT))
+print(f"Listening for UDP on {SRC_IP}:{SRC_PORT}")
+
+# 設定目標 Unix socket 路徑
+UNIX_SOCKET_PATH = '/tmp/unix_socket_mavlink.sock'  # 請自行修改
+
+# 建立 Unix socket 連線
+unix_sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+try:
+    unix_sock.connect(UNIX_SOCKET_PATH)
+except Exception as e:
+    print(f"Failed to connect to unix socket: {e}")
+    sys.exit(1)
+
+while True:
+    # 使用 select 監聽兩個 socket
+    readable, _, _ = select.select([src_sock, unix_sock], [], [])
+    for sock in readable:
+        if sock is src_sock:
+            data, addr = src_sock.recvfrom(4096)
+            if data:
+                unix_sock.sendall(data)
+                # print(f"Received UDP data from {addr}: {data}") # debug
+                # break # debug
+        elif sock is unix_sock:
+            data = unix_sock.recv(4096)
+            if data:
+                # 回送到最近收到資料的 UDP client
+                src_sock.sendto(data, addr)

src/unitdev04/esp32-final.py

@@ -0,0 +1,151 @@
+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/test01.py

@@ -0,0 +1,229 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+def mavlink_analyzer_simple(count = 500):
+    # rclpy.init()
+    # node = rclpy.create_node('mavlink_analyzer_simple')
+ 
+    print('Inintializing connection')
+
+    connection_string="udp:127.0.0.1:14550"
+    connection = mavutil.mavlink_connection(connection_string)
+
+    print('Catch messages')
+    msg_count = {}
+
+    start_time = time.time()
+
+    for _ in range(count):
+        msg = connection.recv_msg()
+        # msg = connection.recv_match(blocking=True)
+        if msg:
+            msg_type = msg.get_type()
+            if msg_type in msg_count:
+                msg_count[msg_type] += 1
+            else:
+                msg_count[msg_type] = 1
+
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                print(msg)
+                pass
+
+        else:
+            print('No message yet, 1 second for data to fill')
+            sleep(1)
+
+    print("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
+    print('Messages Type received:')
+    print(msg_count)
+
+    end_time = time.time()
+    print('Time elapsed: ', end_time - start_time)
+
+    print('Closing connection')
+    connection.close()
+
+
+def fdm_parser_example(data=None):
+    if data is None:
+        data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+    # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+    parse_format = 'HHI16H'
+
+    if len(data) != struct.calcsize(parse_format):
+        print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+        exit(1)
+
+    decoded = struct.unpack(parse_format,data)
+
+    magic = decoded[0]
+    frame_rate_hz = decoded[1]
+    frame_count = decoded[2]
+    pwm = decoded[3:]
+
+
+    print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    沒有轉換數據格式
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9002))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+            packet_count += 1
+        except socket.timeout:
+            pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            # print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack : {data_g2s}')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
+    '''
+    # read info from SITL via MAVLink
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection = mavutil.mavlink_connection(connection_string)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    data_queue = queue.Queue()
+    servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+    print('Start sending data to Gazebo')
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
+
+print('Start')
+# fdm_switch_example_two()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+mavlink_btye_generator_test()

src/unitdev04/udptest4.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/unitdev04/udptest8-2.py

@@ -0,0 +1,500 @@
+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()

src/unitdev04/udptest8.py

@@ -0,0 +1,384 @@
+# === import 保持不變 ===
+import asyncio
+import serial_asyncio
+import struct
+import serial
+import traceback
+import time
+from collections import deque, defaultdict
+from pymavlink import mavutil
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import threading
+
+# === 多組設備設定 ===
+CONFIGS = [
+    {"serial_port": "/dev/ttyUSB2", "udp_port": 14551},
+    {"serial_port": "COM43", "udp_port": 14552},
+    {"serial_port": "COM15", "udp_port": 14553},
+]
+
+SERIAL_BAUDRATE = 115200
+UDP_REMOTE_IP = '127.0.0.1'
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
+
+# === 數據存儲 ===
+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)  # {sysid: {compid: {'last_seq': x, 'total_packets': y, 'lost_packets': z}}}
+packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0})
+
+serial_to_sysid = {}
+serial_last_mavlink_time = {}  # 優化 1：追蹤各 serial port 最近的 MAVLink 時間
+
+def calculate_packet_loss(sysid, compid, current_seq):
+    """計算丟包率"""
+    global mavlink_sequence_tracker, packet_loss_stats
+    
+    tracker = mavlink_sequence_tracker[sysid]
+    if compid not in tracker:
+        # 第一次收到這個component的消息
+        tracker[compid] = {
+            'last_seq': current_seq,
+            'total_packets': 1,
+            'lost_packets': 0,
+            'last_update': time.time()
+        }
+        return 0.0
+    
+    comp_tracker = tracker[compid]
+    last_seq = comp_tracker['last_seq']
+    
+    # 計算序列號差異（處理255溢出）
+    if current_seq > last_seq:
+        expected_packets = current_seq - last_seq
+    elif current_seq < last_seq:
+        # 序列號溢出（0-255循環）
+        expected_packets = (255 - last_seq) + current_seq + 1
+    else:
+        # 重複的序列號，忽略
+        return comp_tracker.get('loss_rate', 0.0)
+    
+    # 更新統計
+    comp_tracker['total_packets'] += expected_packets
+    lost_packets = expected_packets - 1  # 實際收到1個，應該收到expected_packets個
+    comp_tracker['lost_packets'] += max(0, lost_packets)
+    comp_tracker['last_seq'] = current_seq
+    comp_tracker['last_update'] = time.time()
+    
+    # 計算丟包率
+    total_expected = comp_tracker['total_packets']
+    total_lost = comp_tracker['lost_packets']
+    loss_rate = (total_lost / total_expected) * 100.0 if total_expected > 0 else 0.0
+    comp_tracker['loss_rate'] = loss_rate
+    
+    # 更新全局統計（按sysid匯總所有component）
+    total_received = 0
+    total_lost_all = 0
+    total_expected_all = 0
+    
+    for comp_id, comp_data in tracker.items():
+        total_expected_all += comp_data['total_packets']
+        total_lost_all += comp_data['lost_packets']
+        total_received += comp_data['total_packets'] - comp_data['lost_packets']
+    
+    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_received,
+        'total_lost': total_lost_all
+    }
+    
+    # 記錄到歷史數據
+    now = time.time()
+    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=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)
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol, serial_port):
+        self.udp_protocol = udp_protocol
+        self.serial_port = serial_port
+        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"[{self.serial_port}] Serial connection established.")
+
+    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))
+
+            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()
+
+                    # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
+                    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)
+                            # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
+                        else:
+                            print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+                    else:
+                        print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+                else:
+                    print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[{self.serial_port} TX Error] 無法封裝或傳送資料: {e}")
+
+    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 as e:
+            print(f"[{self.serial_port}] 發送 DB 指令失敗: {e}")
+
+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
+        print(f"[UDP:{self.udp_port}] 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:
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, self.udp_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"[UDP:{self.udp_port} 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            # 逐字節解析MAVLink
+            for byte in 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:
+                        serial_to_sysid[self.serial_transport.serial_port] = sysid
+                        serial_last_mavlink_time[self.serial_transport.serial_port] = time.time()
+                        self.serial_transport.send_at_command_db()
+                    
+                    # === 新增：計算丟包率 ===
+                    try:
+                        loss_rate = calculate_packet_loss(sysid, compid, seq)
+                        # print(f"[SYSID:{sysid}] 丟包率: {loss_rate:.1f}%, SEQ: {seq}")
+                    except Exception as e:
+                        print(f"[UDP:{self.udp_port}] 丟包率計算錯誤: {e}")
+                        
+        except Exception as e:
+            print(f"[UDP:{self.udp_port} MAVLink Decode Error] {e}")
+
+def start_plotting():
+    # 創建子圖：上方RSSI，下方丟包率
+    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
+
+    def update(frame):
+        ax1.clear()
+        ax2.clear()
+        
+        # === RSSI 圖 ===
+        ax1.set_title("RSSI", fontsize=14)
+        ax1.set_xlabel("Time (s ago)")
+        ax1.set_ylabel("RSSI (dBm)")
+        ax1.set_xlim(30, 0)  # 顯示最近10秒
+        ax1.set_ylim(-100, -10)
+        ax1.grid(True, alpha=0.3)
+
+        # === 丟包率圖 ===
+        ax2.set_title("Packet Loss Rate", fontsize=14)
+        ax2.set_xlabel("Time (s ago)")
+        ax2.set_ylabel("Loss Rate (%)")
+        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', 'brown', 'pink', 'gray']
+        
+        for i, sysid in enumerate(sorted(rssi_history.keys())):
+            color = colors[i % len(colors)]
+            
+            # === 繪製 RSSI ===
+            rssi_recent_indices = [
+                idx for idx, ts in enumerate(time_history[sysid]) if now - ts <= 30
+            ]
+            if rssi_recent_indices:
+                rssi_t = [now - time_history[sysid][idx] for idx in rssi_recent_indices]
+                rssi_r = [rssi_history[sysid][idx] for idx in rssi_recent_indices]
+                if rssi_t and rssi_r:
+                    ax1.plot(rssi_t, rssi_r, label=f"SYSID:{sysid}", color=color, linewidth=2)
+                    
+                    # 在RSSI圖上顯示當前數值
+                    if rssi_r:
+                        latest_rssi = rssi_r[-1]
+                        ax1.text(2, latest_rssi, f'{latest_rssi:.0f}dBm', 
+                                bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
+                                fontsize=10, fontweight='bold', color='white')
+
+            # === 繪製丟包率 ===
+            if sysid in packet_loss_history:
+                loss_recent_indices = [
+                    idx for idx, ts in enumerate(packet_loss_time_history[sysid]) if now - ts <= 10
+                ]
+                if loss_recent_indices:
+                    loss_t = [now - packet_loss_time_history[sysid][idx] for idx in loss_recent_indices]
+                    loss_r = [packet_loss_history[sysid][idx] for idx in loss_recent_indices]
+                    if loss_t and loss_r:
+                        ax2.plot(loss_t, loss_r, label=f"SYSID:{sysid}", color=color, linewidth=2, marker='o', markersize=3)
+                        
+                        # 在丟包率圖上顯示當前數值和統計
+                        if loss_r:
+                            latest_loss = loss_r[-1]
+                            stats = packet_loss_stats[sysid]
+                            
+                            # 顯示丟包率百分比
+                            ax2.text(2, latest_loss + (i * 8), f'{latest_loss:.1f}%', 
+                                    bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
+                                    fontsize=10, fontweight='bold', color='white')
+                            
+                            # 在圖表左側顯示詳細統計
+                            stats_text = f"SYSID {sysid}:\nRecieved: {stats['total_received']}\nLoss: {stats['total_lost']}\nLoss Rate: {latest_loss:.1f}%"
+                            ax2.text(28, 85 - (i * 20), stats_text,
+                                    bbox=dict(boxstyle="round,pad=0.5", facecolor=color, alpha=0.8),
+                                    fontsize=9, color='white', verticalalignment='top')
+
+        # 設置圖例
+        ax1.legend(loc="upper right", fontsize=10)
+        ax2.legend(loc="upper right", fontsize=10)
+        
+    ani = animation.FuncAnimation(fig, update, interval=1000)
+    plt.tight_layout()
+    plt.show()
+
+async def setup_bridge(config):
+    serial_port = config["serial_port"]
+    udp_port = config["udp_port"]
+    try:
+        ser = serial.Serial(serial_port, SERIAL_BAUDRATE)
+        ser.close()
+    except Exception as e:
+        print(f"[{serial_port}] 串口打開失敗: {e}")
+        return
+    loop = asyncio.get_running_loop()
+    udp_handler = UDPHandler(udp_port)
+    try:
+        await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"[{serial_port}] 無法創建 UDP: {e}")
+        return
+    try:
+        serial_proto = SerialToUDP(udp_handler, serial_port)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, serial_port, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"[{serial_port}] 無法建立串口連接：{e}")
+        traceback.print_exc()
+        return
+    print(f"[{serial_port}] Serial <--> UDP:{udp_port} bridge ready.")
+
+async def main():
+    tasks = [setup_bridge(cfg) for cfg in CONFIGS]
+    await asyncio.gather(*tasks)
+    await asyncio.Future()
+
+if __name__ == '__main__':
+    try:
+        threading.Thread(target=lambda: asyncio.run(main()), daemon=True).start()
+        start_plotting()
+    except KeyboardInterrupt:
+        print("使用者中斷程式")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()