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

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

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

src/GUI/comm_panel.py

@@ -1,10 +1,78 @@
 #!/usr/bin/env python3
 from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
-                            QPushButton, QLineEdit, QComboBox)
+                            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;
@@ -685,3 +764,11 @@ class CommPanel(QWidget):
         """從列表中移除 Serial 連接"""
         if conn in self.serial_connections:
             self.serial_connections.remove(conn)
+
+    def apply_font_scale(self):
+        """重新套用目前字體倍率到通訊面板。"""
+        _apply_scaled_font(self)
+        _reapply_scaled_stylesheet(self)
+        for child in self.findChildren(QWidget):
+            _apply_scaled_font(child)
+            _reapply_scaled_stylesheet(child)

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

@@ -20,6 +20,11 @@ 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:
@@ -30,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
 
@@ -42,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)
 
@@ -528,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]
 
@@ -561,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]
 
@@ -593,7 +604,7 @@ class DroneMonitor(Node):
             if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
             else:
-                # 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
+                # 檢查既有訂閱是否包含 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}'
@@ -608,6 +619,19 @@ class DroneMonitor(Node):
                         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, ...
@@ -663,6 +687,14 @@ class DroneMonitor(Node):
             )
         }
 
+        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)
 
     # ================================================================================
@@ -699,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 方法，無需線程池（避免嵌套執行器衝突）
@@ -727,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
 
@@ -743,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 方法，無需線程池（避免嵌套執行器衝突）
@@ -765,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
 
@@ -781,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 方法，無需線程池（避免嵌套執行器衝突）
@@ -802,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
 
@@ -840,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'):
+    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)
-            self.latest_data[(drone_id, 'attitude')] = {
+                data = {
                     'roll': roll,
                     'pitch': pitch,
                     'yaw': yaw,
-                'rates': (msg.angular_velocity.x, 
+                    'rates': (
+                        msg.angular_velocity.x,
                         msg.angular_velocity.y,
-                         msg.angular_velocity.z)
+                        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
@@ -903,7 +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
-                print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
+                _log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
             
             # 先發送連接類型資訊
             self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -1003,6 +1060,9 @@ class DroneMonitor(Node):
             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')] = {
@@ -1016,6 +1076,13 @@ class DroneMonitor(Node):
                 '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
@@ -1039,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")
+        _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)
@@ -321,6 +389,16 @@ class DronePanel(QWidget):
         """獲取勾選狀態"""
         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):
     # 定義信號
     group_selection_changed = pyqtSignal(str, int)  # socket_id, state
@@ -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)

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>',
@@ -618,6 +625,10 @@ class DroneMap:
                 }
                 // ================================================================================
 
+                function setFontScale(scale) {
+                    document.documentElement.style.setProperty('--ui-font-scale', scale);
+                }
+                
                 // 開始任務
                 function startMission() {
                     if (!centerPosition || !targetPosition) {
@@ -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):
         """更新無人機位置（加入待處理隊列）"""
@@ -905,6 +917,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)} 架無人機, "
+        _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}")
+            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,6 +23,18 @@ 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:
     """單一任務群組的資料"""
@@ -157,10 +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.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):
@@ -372,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']),
             ],
         }
 
@@ -532,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;"
@@ -553,4 +575,3 @@ class GroupPanel(QWidget):
         except ValueError:
             alt = 10.0
         self.takeoff_requested.emit(self.group.group_id, alt)
-

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/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
@@ -231,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
 
@@ -267,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)
     
@@ -1137,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

@@ -38,6 +38,7 @@ class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
     XBEEAPI2AT = auto()    # XBee API 模式
+    XBEEAPI_POLL = auto()
     NOT_USE = auto()       # 不使用
 
 
@@ -176,10 +177,10 @@ class XBeeFrameProcessor(FrameProcessor):
         """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
         frame_type = frame[3]
 
-        if frame_type == self.FRAME_TYPE_RX_PACKET:
+        if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
             return self._decapsulate(frame)
 
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
+        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
@@ -335,9 +336,10 @@ class ATCommandHandler:
     def _handle_rssi(self, data: bytes):
         """處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
         pass
-        # if data:
-            # print(f"[{self.serial_port}] RSSI = -{data[0]} dBm")  # dev
+        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)"""
@@ -370,6 +372,10 @@ class SerialHandler(asyncio.Protocol):
             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()
 

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/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/esp32.py

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

src/unitdev04/udptest8-2.py

@@ -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()