學長建議修改內容

src/GUI/comm_panel.py

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

src/GUI/command_sender.py

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

src/GUI/communication.py

@@ -20,11 +20,6 @@ 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:
@@ -35,9 +30,10 @@ try:
     from fc_network_apps.longCommand import CommandLongClient
 except ImportError as e:
     import traceback
-    _log("WARN", "無法導入 CommandLongClient")
-    _log("ERROR", f"錯誤: {e}")
-    _log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
+    print(f"⚠️  警告: 無法導入 CommandLongClient")
+    print(f"   错误: {e}")
+    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+    print(f"   完整堆栈跟踪:")
     traceback.print_exc()
     CommandLongClient = None
 
@@ -46,18 +42,11 @@ try:
     from fc_network_apps.navigation import PositionTargetGlobalIntClient
 except ImportError as e:
     import traceback
-    _log("WARN", "無法導入 PositionTargetGlobalIntClient")
-    _log("ERROR", f"錯誤: {e}")
+    print(f"⚠️  警告: 無法導入 PositionTargetGlobalIntClient")
+    print(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)
 
@@ -539,25 +528,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
-                    _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})")
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
                     
                     # 將新 client 添加到主執行器（這樣它的回調才能被處理）
                     if self.executor:
                         self.executor.add_node(client)
-                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
                     
                 except TypeError:
                     # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
                     client = CommandLongClient()
                     self.command_long_clients[drone_id] = client
-                    _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
                     
                     if self.executor:
                         self.executor.add_node(client)
-                        _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
                         
                 except Exception as e:
-                    _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
+                    print(f"⚠️  無法為 {drone_id} 創建 CommandLongClient: {e}")
                     return None
             return self.command_long_clients[drone_id]
 
@@ -572,12 +561,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
-                    _log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})")
+                    print(f"   ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
                     if self.executor:
                         self.executor.add_node(client)
-                        _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
+                        print(f"   ✓ 將 {drone_id} 的 position client 添加到主執行器")
                 except Exception as e:
-                    _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
+                    print(f"⚠️  無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
                     return None
             return self.position_target_clients[drone_id]
 
@@ -604,7 +593,7 @@ class DroneMonitor(Node):
             if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
             else:
-                # 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
+                # 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
                 subs = getattr(self, subs_attr, {})
                 if isinstance(subs, dict) and 'position_ned' not in subs:
                     base_topic = f'/fc_network/vehicle/{sys_id}'
@@ -619,19 +608,6 @@ 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, ...
@@ -686,14 +662,6 @@ class DroneMonitor(Node):
                 10
             )
         }
-
-        if AttitudeRaw is not None:
-            subs['attitude'] = self.create_subscription(
-                AttitudeRaw,
-                f'{base_topic}/attitude',
-                lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
-                10
-            )
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
@@ -731,22 +699,22 @@ class DroneMonitor(Node):
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
             if len(parts) < 2:
-                _log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                print(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:
-                _log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
+                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
                 return False
             
-            _log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
             
             # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
             client = self.get_or_create_client(drone_id)
             if not client:
-                _log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
+                print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -759,13 +727,13 @@ class DroneMonitor(Node):
             )
             
             if result and result.success:
-                _log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
+                print(f"✅ [SET_MODE] 模式切換成功")
                 return True
             else:
-                _log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
-            _log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
             traceback.print_exc()
             return False
 
@@ -775,17 +743,17 @@ class DroneMonitor(Node):
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
             if len(parts) < 2:
-                _log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
+                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
                 return False
             sysid = int(parts[-1])
             
             action_name = "解鎖" if arm else "上鎖"
-            _log("INFO", f"[ARM] {drone_id} -> {action_name}")
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
             # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
             client = self.get_or_create_client(drone_id)
             if not client:
-                _log("ERROR", "[ARM] CommandLongClient 無法初始化")
+                print(f"❌ [ARM] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -797,13 +765,13 @@ class DroneMonitor(Node):
             )
             
             if result and result.success:
-                _log("INFO", f"[ARM] {drone_id} {action_name}成功")
+                print(f"✅ [ARM] {action_name}成功")
                 return True
             else:
-                _log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})")
+                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
-            _log("ERROR", f"[ARM] 例外錯誤: {e}")
+            print(f"❌ [ARM] 例外錯誤: {e}")
             traceback.print_exc()
             return False
 
@@ -813,16 +781,16 @@ class DroneMonitor(Node):
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
             if len(parts) < 2:
-                _log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
                 return False
             sysid = int(parts[-1])
             
-            _log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
             
             # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
             client = self.get_or_create_client(drone_id)
             if not client:
-                _log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
+                print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -834,13 +802,13 @@ class DroneMonitor(Node):
             )
             
             if result and result.success:
-                _log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
+                print(f"✅ [TAKEOFF] 起飛成功")
                 return True
             else:
-                _log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
-            _log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
             traceback.print_exc()
             return False
 
@@ -872,42 +840,17 @@ class DroneMonitor(Node):
         return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
 
     # callbacks
-    def attitude_callback(self, sys_id, msg):
-        """處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。"""
-        actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
-        if actual_drone_id is None:
-            return
-
-        try:
-            if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'):
-                data = {
-                    'roll': math.degrees(msg.roll),
-                    'pitch': math.degrees(msg.pitch),
-                    'yaw': math.degrees(msg.yaw),
-                    'rates': (
-                        getattr(msg, 'rollspeed', 0.0),
-                        getattr(msg, 'pitchspeed', 0.0),
-                        getattr(msg, 'yawspeed', 0.0),
-                    )
-                }
-            elif hasattr(msg, 'orientation'):
-                roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
-                data = {
-                    'roll': roll,
-                    'pitch': pitch,
-                    'yaw': yaw,
-                    'rates': (
-                        msg.angular_velocity.x,
-                        msg.angular_velocity.y,
-                        msg.angular_velocity.z,
-                    )
-                }
-            else:
-                return
-
-            self.latest_data[(actual_drone_id, 'attitude')] = data
-        except Exception as e:
-            print(f"Error parsing attitude msg for {sys_id}: {e}")
+    def attitude_callback(self, drone_id, msg):
+        if hasattr(msg, 'orientation'):
+            roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
+            self.latest_data[(drone_id, 'attitude')] = {
+                'roll': roll,
+                'pitch': pitch,
+                'yaw': yaw,
+                'rates': (msg.angular_velocity.x, 
+                         msg.angular_velocity.y,
+                         msg.angular_velocity.z)
+            }
 
     def battery_callback(self, sys_id, msg):
         # 使用映射獲取實際的 drone_id
@@ -960,7 +903,7 @@ class DroneMonitor(Node):
                 sys_num = sys_id.replace('sys', '')
                 actual_drone_id = f's{assigned_socket_id}_{sys_num}'
                 self.sys_to_actual_id[sys_id] = actual_drone_id
-                _log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
+                print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
             
             # 先發送連接類型資訊
             self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -1060,9 +1003,6 @@ 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')] = {
@@ -1075,13 +1015,6 @@ class DroneMonitor(Node):
                 'y': y,
                 'z': z
             }
-
-            # 儲存速度資訊，供總覽頁「XY速度」欄位顯示
-            self.latest_data[(actual_drone_id, 'velocity')] = {
-                'vx': vx,
-                'vy': vy,
-                'vz': vz
-            }
             
             # 發送信號給 GUI 更新高度顯示
             self.signals.update_signal.emit('altitude', actual_drone_id, {
@@ -1106,5 +1039,5 @@ class DroneMonitor(Node):
         receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
         receiver.start()
         self.serial_receivers.append(receiver)
-        _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
-        return receiver
+        print(f"Started serial connection on {port} at {baudrate} baud")
+        return receiver

src/GUI/drone_panel.py

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

src/GUI/map_layout.py

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

src/GUI/mission_executor.py

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

src/GUI/mission_group.py

@@ -23,18 +23,6 @@ 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:
     """單一任務群組的資料"""
@@ -169,13 +157,10 @@ class GroupPanel(QWidget):
         QPushButton:disabled {{ background-color: #444; color: #666; }}
     """
 
-    def __init__(self, group: MissionGroup, parent=None, default_params=None):
+    def __init__(self, group: MissionGroup, parent=None):
         super().__init__(parent)
         self.group = group
         self.all_btn_ref = None  # 保存全選按鈕的參考
-        self.default_params = dict(DEFAULT_MISSION_PARAM_VALUES)
-        if default_params:
-            self.default_params.update(default_params)
         self._build_ui()
 
     def _make_sep(self):
@@ -387,23 +372,23 @@ class GroupPanel(QWidget):
         # 每種任務類型的參數定義: (key, label, default_value)
         self._param_defs = {
             'M_FORMATION': [
-                ('spacing', '間距 (m)', self.default_params['spacing']),
-                ('base_altitude', '基準高度 (m)', self.default_params['base_altitude']),
-                ('altitude_diff', '高低差 (m)', self.default_params['altitude_diff']),
+                ('spacing', '間距 (m)', '5.0'),
+                ('base_altitude', '基準高度 (m)', '10.0'),
+                ('altitude_diff', '高低差 (m)', '2.0'),
             ],
             'CIRCLE_FORMATION': [
-                ('radius', '半徑 (m)', self.default_params['radius']),
-                ('altitude', '高度 (m)', self.default_params['altitude']),
-                ('start_angle', '起始角 (°)', self.default_params['start_angle']),
+                ('radius', '半徑 (m)', '10.0'),
+                ('altitude', '高度 (m)', '10.0'),
+                ('start_angle', '起始角 (°)', '0'),
             ],
             'LEADER_FOLLOWER': [
-                ('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
-                ('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
-                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('lateral_offset', '橫向偏移 (m)', '3.0'),
+                ('longitudinal_spacing', '縱向間距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
             ],
             'GRID_SWEEP': [
-                ('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
-                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('line_spacing', '掃描線距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
             ],
         }
 
@@ -547,13 +532,6 @@ 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;"
@@ -575,3 +553,4 @@ class GroupPanel(QWidget):
         except ValueError:
             alt = 10.0
         self.takeoff_requested.emit(self.group.group_id, alt)
+

src/fc_interfaces/CMakeLists.txt

@@ -8,14 +8,11 @@ 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

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

src/fc_interfaces/package.xml

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

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -32,7 +32,6 @@ import mavros_msgs.msg
 
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
-import fc_interfaces.msg as fcmsg
 from .ackEnum import serviceAckResult
 
 # 自定義 imports
@@ -232,7 +231,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
 
@@ -268,28 +267,34 @@ class VehicleStatusPublisher(Node):
         publisher.publish(msg)
     
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
-        """發布原始姿態（roll/pitch/yaw 與角速度）"""
+        """發布姿態（IMU）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
             return
         
         att = status.attitude
-        if att.roll is None or att.pitch is None or att.yaw is None:
+        if att.roll is None:
             return
-        publisher = self._get_or_create_publisher(
-            sysid, 'attitude', fcmsg.AttitudeRaw
-        )
+        publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
         
         if publisher.get_subscription_count() == 0:
             return
         
-        msg = fcmsg.AttitudeRaw()
-        msg.stamp = self.get_clock().now().to_msg()
-        msg.roll = float(att.roll)
-        msg.pitch = float(att.pitch)
-        msg.yaw = float(att.yaw)
-        msg.rollspeed = float(att.rollspeed) if att.rollspeed is not None else 0.0
-        msg.pitchspeed = float(att.pitchspeed) if att.pitchspeed is not None else 0.0
-        msg.yawspeed = float(att.yawspeed) if att.yawspeed is not None else 0.0
+        msg = sensor_msgs.msg.Imu()
+        
+        # 歐拉角轉四元數
+        qx, qy, qz, qw = self._euler_to_quaternion(
+            att.roll, att.pitch, att.yaw
+        )
+        msg.orientation.x = qx
+        msg.orientation.y = qy
+        msg.orientation.z = qz
+        msg.orientation.w = qw
+        
+        # 角速度
+        if att.rollspeed is not None:
+            msg.angular_velocity.x = att.rollspeed
+            msg.angular_velocity.y = att.pitchspeed
+            msg.angular_velocity.z = att.yawspeed
         
         publisher.publish(msg)
     
@@ -1132,8 +1137,7 @@ 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,7 +38,6 @@ class SerialMode(Enum):
     """連接類型"""
     STRAIGHT = auto()      # 原始數據直通
     XBEEAPI2AT = auto()    # XBee API 模式
-    XBEEAPI_POLL = auto()
     NOT_USE = auto()       # 不使用
 
 
@@ -177,10 +176,10 @@ class XBeeFrameProcessor(FrameProcessor):
         """根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
         frame_type = frame[3]
 
-        if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
+        if frame_type == self.FRAME_TYPE_RX_PACKET:
             return self._decapsulate(frame)
 
-        if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
+        if frame_type == self.FRAME_TYPE_AT_RESPONSE:
             if self.at_handler is not None:
                 self.at_handler.handle_frame(frame)
             return None
@@ -336,10 +335,9 @@ 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)"""
@@ -372,10 +370,6 @@ 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,13 +7,6 @@
   <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
   <license>TODO: License declaration</license>
 
-  <exec_depend>fc_interfaces</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>mavros_msgs</exec_depend>
-
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

src/unitdev04/esp32-final.py

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

src/unitdev04/esp32.py

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

src/unitdev04/udptest8-2.py

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