Merge remote-tracking branch 'origin/ken' into wenchun

src/GUI/comm_panel.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python3
 from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
-                            QPushButton, QLineEdit)
+                            QPushButton, QLineEdit, QComboBox)
 from PyQt6.QtCore import pyqtSignal
+import glob
+import os
 
 class CommPanel(QWidget):
     """通讯设置面板类"""
@@ -13,12 +15,16 @@ class CommPanel(QWidget):
     ws_connection_added = pyqtSignal(str)  # url
     ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
     ws_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    serial_connection_added = pyqtSignal(str, int)  # port, baudrate
+    serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    serial_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
     status_message = pyqtSignal(str, int)  # message, timeout
     
     def __init__(self, parent=None):
         super().__init__(parent)
         self.udp_connections = []
         self.ws_connections = []
+        self.serial_connections = []
         self._init_ui()
     
     def _init_ui(self):
@@ -65,7 +71,7 @@ class CommPanel(QWidget):
         """)
         
         self.udp_port_input = QLineEdit()
-        self.udp_port_input.setText("14540")
+        self.udp_port_input.setText("14550")
         self.udp_port_input.setPlaceholderText("Port")
         self.udp_port_input.setFixedWidth(80)
         self.udp_port_input.setStyleSheet("""
@@ -105,6 +111,118 @@ class CommPanel(QWidget):
         separator.setFixedHeight(20)
         layout.addWidget(separator)
         
+        # ========== Serial 區域 ==========
+        serial_title = QLabel("Serial")
+        serial_title.setStyleSheet("""
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(serial_title)
+        
+        # Serial 連接列表容器
+        self.serial_list_widget = QWidget()
+        self.serial_list_layout = QVBoxLayout(self.serial_list_widget)
+        self.serial_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.serial_list_layout.setSpacing(5)
+        layout.addWidget(self.serial_list_widget)
+        
+        # Serial 添加新連接區域
+        add_serial_widget = QWidget()
+        add_serial_layout = QHBoxLayout(add_serial_widget)
+        add_serial_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.serial_port_combo = QComboBox()
+        self.serial_port_combo.setStyleSheet("""
+            QComboBox {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+            QComboBox::drop-down {
+                border: none;
+            }
+            QComboBox::down-arrow {
+                image: none;
+                border-left: 5px solid transparent;
+                border-right: 5px solid transparent;
+                border-top: 5px solid #DDD;
+            }
+        """)
+        self._refresh_serial_ports()
+        
+        refresh_ports_btn = QPushButton("↻")
+        refresh_ports_btn.setFixedWidth(35)
+        refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
+        refresh_ports_btn.setToolTip("重新掃描串口")
+        refresh_ports_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 6px;
+                border-radius: 4px;
+                font-size: 16px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        self.serial_baudrate_combo = QComboBox()
+        self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
+        self.serial_baudrate_combo.setCurrentText('57600')
+        self.serial_baudrate_combo.setFixedWidth(100)
+        self.serial_baudrate_combo.setStyleSheet("""
+            QComboBox {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+            QComboBox::drop-down {
+                border: none;
+            }
+            QComboBox::down-arrow {
+                image: none;
+                border-left: 5px solid transparent;
+                border-right: 5px solid transparent;
+                border-top: 5px solid #DDD;
+            }
+        """)
+        
+        add_serial_btn = QPushButton("添加")
+        add_serial_btn.clicked.connect(self._handle_add_serial)
+        add_serial_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+        add_serial_layout.addWidget(self.serial_port_combo)
+        add_serial_layout.addWidget(refresh_ports_btn)
+        add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;"))
+        add_serial_layout.addWidget(self.serial_baudrate_combo)
+        add_serial_layout.addWidget(add_serial_btn)
+        
+        layout.addWidget(add_serial_widget)
+        
+        # 分隔線
+        separator = QWidget()
+        separator.setFixedHeight(20)
+        layout.addWidget(separator)
+        
         # ========== WebSocket 區域 ==========
         ws_title = QLabel("WebSocket")
         ws_title.setStyleSheet("""
@@ -229,6 +347,94 @@ class CommPanel(QWidget):
         # 清空輸入框
         self.ws_url_input.clear()
     
+    def _refresh_serial_ports(self):
+        """重新掃描可用的串口"""
+        self.serial_port_combo.clear()
+        
+        # 掃描 Linux 下的串口設備
+        ports = []
+        
+        # 掃描 USB 串口
+        usb_ports = glob.glob('/dev/ttyUSB*')
+        ports.extend(usb_ports)
+        
+        # 掃描 ACM 串口
+        acm_ports = glob.glob('/dev/ttyACM*')
+        ports.extend(acm_ports)
+        
+        # 排序
+        ports.sort()
+        
+        if ports:
+            self.serial_port_combo.addItems(ports)
+        else:
+            self.serial_port_combo.addItem("沒有找到串口")
+            self.serial_port_combo.setEnabled(False)
+            return
+        
+        self.serial_port_combo.setEnabled(True)
+    
+    def _handle_add_serial(self):
+        """處理添加 Serial 連接"""
+        port = self.serial_port_combo.currentText()
+        baudrate_text = self.serial_baudrate_combo.currentText()
+        
+        if not port or port == "沒有找到串口":
+            self.status_message.emit("請選擇有效的串口", 3000)
+            return
+        
+        try:
+            baudrate = int(baudrate_text)
+        except ValueError:
+            self.status_message.emit("波特率格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.serial_connections:
+            if conn['port'] == port:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.serial_connection_added.emit(port, baudrate)
+    
+    def _handle_add_ws(self):
+        """處理添加 WebSocket 連接"""
+        input_url = self.ws_url_input.text().strip()
+        
+        if not input_url:
+            self.status_message.emit("請輸入 WebSocket URL", 3000)
+            return
+        
+        # 自動添加 ws:// 前綴
+        if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+            url = f'ws://{input_url}'
+        else:
+            url = input_url
+        
+        # 基本 URL 格式驗證
+        try:
+            if '://' in url:
+                parts = url.split('://', 1)
+                if len(parts) == 2 and ':' not in parts[1]:
+                    self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+                    return
+        except:
+            self.status_message.emit("URL 格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.ws_connections:
+            if conn['url'] == url:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.ws_connection_added.emit(url)
+        
+        # 清空輸入框
+        self.ws_url_input.clear()
+    
     def create_udp_connection_panel(self, conn):
         """創建 UDP 連接面板"""
         panel = QWidget()
@@ -395,4 +601,88 @@ class CommPanel(QWidget):
     def remove_ws_connection_from_list(self, conn):
         """從列表中移除 WebSocket 連接"""
         if conn in self.ws_connections:
-            self.ws_connections.remove(conn)
+            self.ws_connections.remove(conn)
+    
+    def create_serial_connection_panel(self, conn):
+        """創建 Serial 連接面板"""
+        panel = QWidget()
+        panel.setStyleSheet("""
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            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))
+        toggle_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
+        remove_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def add_serial_panel(self, conn):
+        """添加 Serial 連接面板到列表"""
+        panel = self.create_serial_connection_panel(conn)
+        self.serial_list_layout.addWidget(panel)
+        self.serial_connections.append(conn)
+        return panel
+    
+    def remove_serial_connection_from_list(self, conn):
+        """從列表中移除 Serial 連接"""
+        if conn in self.serial_connections:
+            self.serial_connections.remove(conn)

src/GUI/communication.py

@@ -11,7 +11,7 @@ import socket
 from pymavlink import mavutil
 from geometry_msgs.msg import Point, Vector3
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
-from std_msgs.msg import Float64
+from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
 from mavros_msgs.srv import CommandBool, CommandTOL
 
@@ -20,12 +20,15 @@ class DroneSignals(QObject):
 
 class UDPMavlinkReceiver(threading.Thread):
     """UDP MAVLink 接收器"""
-    def __init__(self, ip, port, signals, connection_name):
+    def __init__(self, ip, port, signals, connection_name, monitor=None):
         super().__init__(daemon=True)
         self.ip = ip
         self.port = port
         self.signals = signals
         self.connection_name = connection_name
+        self.monitor = monitor  # 保存 monitor 引用
+        # UDP 使用原始 socket_id = 8
+        self.socket_id = monitor.get_or_assign_socket_id('8') if monitor else 0
         self.running = False
         self.sock = None
         
@@ -61,7 +64,7 @@ class UDPMavlinkReceiver(threading.Thread):
         try:
             msg_type = msg.get_type()
             system_id = msg.get_srcSystem()
-            drone_id = f"s8_{system_id}"  # 使用 s8_ 前綴表示 UDP 來源
+            drone_id = f"s{self.socket_id}_{system_id}"
             
             if msg_type == "HEARTBEAT":
                 mode = mavutil.mode_string_v10(msg)
@@ -116,11 +119,13 @@ class UDPMavlinkReceiver(threading.Thread):
                 })
 
             elif msg_type == "VFR_HUD":
-                groundspeed = msg.groundspeed
-                heading = msg.heading
                 self.signals.update_signal.emit('hud', drone_id, {
-                    'heading': heading,
-                    'groundspeed': groundspeed
+                    'airspeed': msg.airspeed,
+                    'groundspeed': msg.groundspeed,
+                    'heading': msg.heading,
+                    'throttle': msg.throttle,
+                    'alt': msg.alt,
+                    'climb': msg.climb
                 })
                 
         except Exception as e:
@@ -132,12 +137,15 @@ class UDPMavlinkReceiver(threading.Thread):
 
 class SerialMavlinkReceiver(threading.Thread):
     """串口 MAVLink 接收器"""
-    def __init__(self, port, baudrate, signals, connection_name):
+    def __init__(self, port, baudrate, signals, connection_name, monitor=None):
         super().__init__(daemon=True)
         self.port = port
         self.baudrate = baudrate
         self.signals = signals
         self.connection_name = connection_name
+        self.monitor = monitor  # 保存 monitor 引用
+        # Serial 使用原始 socket_id = 5
+        self.socket_id = monitor.get_or_assign_socket_id('5') if monitor else 0
         self.running = False
         self.mav = None
         
@@ -184,7 +192,7 @@ class SerialMavlinkReceiver(threading.Thread):
         try:
             msg_type = msg.get_type()
             system_id = msg.get_srcSystem()
-            drone_id = f"s5_{system_id}"  # 使用 serial_ 前綴表示串口來源
+            drone_id = f"s{self.socket_id}_{system_id}"
             
             if msg_type == "HEARTBEAT":
                 mode = mavutil.mode_string_v10(msg)
@@ -239,11 +247,13 @@ class SerialMavlinkReceiver(threading.Thread):
                 })
 
             elif msg_type == "VFR_HUD":
-                groundspeed = msg.groundspeed
-                heading = msg.heading
                 self.signals.update_signal.emit('hud', drone_id, {
-                    'heading': heading,
-                    'groundspeed': groundspeed
+                    'airspeed': msg.airspeed,
+                    'groundspeed': msg.groundspeed,
+                    'heading': msg.heading,
+                    'throttle': msg.throttle,
+                    'alt': msg.alt,
+                    'climb': msg.climb
                 })
                 
         except Exception as e:
@@ -255,12 +265,12 @@ class SerialMavlinkReceiver(threading.Thread):
 
 class WebSocketMavlinkReceiver(threading.Thread):
     """WebSocket MAVLink 接收器"""
-    def __init__(self, url, signals, connection_name):
+    def __init__(self, url, signals, connection_name, monitor=None):
         super().__init__(daemon=True)
         self.url = url
         self.signals = signals
         self.connection_name = connection_name
-        self.running = False
+        self.monitor = monitor  # 保存 monitor 引用        self.socket_id = monitor.get_next_socket_id() if monitor else 0  # 一次性分配 socket_id        self.running = False
         self.max_retries = 5
         self.base_delay = 1.0
         
@@ -319,10 +329,10 @@ class WebSocketMavlinkReceiver(threading.Thread):
     def process_websocket_message(self, data):
         """處理 WebSocket 訊息"""
         try:
-            drone_id = data.get('system_id')
-            drone_id = f"s9_{drone_id}" if drone_id else None
-            if not drone_id:
+            system_id = data.get('system_id')
+            if not system_id:
                 return
+            drone_id = f"s{self.socket_id}_{system_id}"
 
             # 模式
             if 'mode' in data:
@@ -371,6 +381,7 @@ class WebSocketMavlinkReceiver(threading.Thread):
         self.running = False
 
 class DroneMonitor(Node):
+    # Subscribe to drone ROS2 topics
     def __init__(self):
         super().__init__('drone_monitor')
         self.signals = DroneSignals()
@@ -396,111 +407,112 @@ class DroneMonitor(Node):
         # ================================================================================
         self.drone_gps = {}  # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
         # ================================================================================
+        
+        # ================================================================================
+        # 【新增】Socket ID 重新分配機制 (從 0 開始)
+        # ================================================================================
+        self.socket_id_mapping = {}  # {原始socket_id: 重新分配的socket_id}
+        self.socket_id_counter = 0   # 當前分配到的最大socket_id
+        self.socket_id_lock = Lock()  # 線程安全鎖
+        # ================================================================================
+        
+        # ================================================================================
+        # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取)
+        # ================================================================================
+        self.sys_to_actual_id = {}  # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'}
+        self.sys_to_socket_id = {}  # {sys_id: assigned_socket_id} e.g. {'sys11': 0}
+        # ================================================================================
         self.serial_receivers = []
         
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
+    
+    def get_or_assign_socket_id(self, original_socket_id):
+        """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配）
+        同一個原始 socket_id 會得到同一個分配的 ID
+        """
+        original_socket_id = str(original_socket_id)
+        
+        with self.socket_id_lock:
+            if original_socket_id not in self.socket_id_mapping:
+                # 分配新的 socket_id
+                self.socket_id_mapping[original_socket_id] = self.socket_id_counter
+                print(f"🆕 Socket ID 映射: 原始 {original_socket_id} -> 分配 {self.socket_id_counter}")
+                self.socket_id_counter += 1
+            
+            return self.socket_id_mapping[original_socket_id]
        
     def scan_topics(self):
         topics = self.get_topic_names_and_types()
-        drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
+        drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
         
         found_drones = set()
         for topic_name, _ in topics:
             if match := drone_pattern.match(topic_name):
-                drone_id, topic_type = match.groups()
-                found_drones.add(drone_id)
+                sys_id, topic_type = match.groups()
+                found_drones.add(sys_id)
                 with self.lock:
-                    self.drone_topics.setdefault(drone_id, set()).add(topic_type)
+                    self.drone_topics.setdefault(sys_id, set()).add(topic_type)
         
-        for drone_id in found_drones:
-            if not hasattr(self, f'drone_{drone_id}_subs'):
-                self.setup_drone(drone_id)
+        for sys_id in found_drones:
+            # 為每個 sys_id 分配 socket_id（如果還沒有分配）
+            # 注意：如果後續 summary 提供了 socket_id，會使用 summary 的映射覆蓋
+            if sys_id not in self.sys_to_socket_id:
+                # 暫時所有 ROS2 topic 共享同一個原始 socket_id，等 summary 提供實際的 socket_id
+                self.sys_to_socket_id[sys_id] = self.get_or_assign_socket_id('0')
+            
+            if not hasattr(self, f'drone_{sys_id}_subs'):
+                self.setup_drone(sys_id)
     
-    def setup_drone(self, drone_id):
-        base_topic = f'/MavLinkBus/{drone_id}'
+    def setup_drone(self, sys_id):
+        # sys_id 格式： sys11, sys12, ...
+        base_topic = f'/fc_network/vehicle/{sys_id}'
         
-        # Add service clients
-        self.arm_clients[drone_id] = self.create_client(
+        # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
+        self.arm_clients[sys_id] = self.create_client(
             CommandBool,
             f'{base_topic}/cmd/arming'
         )
-        self.takeoff_clients[drone_id] = self.create_client(
+        self.takeoff_clients[sys_id] = self.create_client(
             CommandTOL,
             f'{base_topic}/cmd/takeoff'
         )
 
         # Add setpoint publisher
-        self.setpoint_pubs[drone_id] = self.create_publisher(
+        self.setpoint_pubs[sys_id] = self.create_publisher(
             Point,
             f'{base_topic}/setpoint_position/local',
             10
         )
 
         subs = {
-            'attitude': self.create_subscription(
-                Imu,
-                f'{base_topic}/attitude',
-                lambda msg, did=drone_id: self.attitude_callback(did, msg),
-                10
-            ),
             'battery': self.create_subscription(
                 BatteryState,
                 f'{base_topic}/battery',
-                lambda msg, did=drone_id: self.battery_callback(did, msg),
+                lambda msg, sid=sys_id: self.battery_callback(sid, msg),
                 10
             ),
-            'global': self.create_subscription(
+            'position': self.create_subscription(
                 NavSatFix,
-                f'{base_topic}/global_position/global',
-                lambda msg, did=drone_id: self.gps_callback(did, msg),
+                f'{base_topic}/position',
+                lambda msg, sid=sys_id: self.gps_callback(sid, msg),
                 10
             ),
-            'rel_alt': self.create_subscription(
-                Float64,
-                f'{base_topic}/global_position/rel_alt',
-                lambda msg, did=drone_id: self.altitude_callback(did, msg),
-                10
-            ),
-            'local_pose': self.create_subscription(
-                Point,
-                f'{base_topic}/local_position/pose',
-                lambda msg, did=drone_id: self.local_pose_callback(did, msg),
-                10
-            ),
-            'local_vel': self.create_subscription(
-                Vector3,
-                f'{base_topic}/local_position/velocity',
-                lambda msg, did=drone_id: self.local_vel_callback(did, msg),
-                10
-            ),
-            'loss_rate': self.create_subscription(
-                Float64,
-                f'{base_topic}/packet_loss_rate',
-                lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
-                10
-            ),
-            'state': self.create_subscription(
-                State,
-                f'{base_topic}/state',
-                lambda msg, did=drone_id: self.state_callback(did, msg),
-                10
-            ),
-            'ping': self.create_subscription(
-                Float64,
-                f'{base_topic}/ping',
-                lambda msg, did=drone_id: self.ping_callback(did, msg),
+            'summary': self.create_subscription(
+                String,
+                f'{base_topic}/summary',
+                lambda msg, sid=sys_id: self.summary_callback(sid, msg),
                 10
             ),
             'vfr_hud': self.create_subscription(
                 VfrHud,
                 f'{base_topic}/vfr_hud',
-                lambda msg, did=drone_id: self.hud_callback(did, msg),
+                lambda msg, sid=sys_id: self.hud_callback(sid, msg),
                 10
             )
         }
         
-        setattr(self, f'drone_{drone_id}_subs', subs)
+        setattr(self, f'drone_{sys_id}_subs', subs)
 
     async def arm_drone(self, drone_id, arm):
         if drone_id not in self.arm_clients:
@@ -582,8 +594,14 @@ class DroneMonitor(Node):
                          msg.angular_velocity.z)
             }
 
-    def battery_callback(self, drone_id, msg):
-        self.latest_data[(drone_id, 'battery')] = {
+    def battery_callback(self, sys_id, msg):
+        # 使用映射獲取實際的 drone_id
+        actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+        # 如果還沒有從 summary 獲取到映射，則不處理
+        if actual_drone_id is None:
+            return
+        
+        self.latest_data[(actual_drone_id, 'battery')] = {
             'voltage': msg.voltage
         }
 
@@ -596,8 +614,62 @@ class DroneMonitor(Node):
             'armed': msg.armed
         }
 
-    def gps_callback(self, drone_id, msg):
-        self.latest_data[(drone_id, 'gps')] = {
+    def summary_callback(self, sys_id, msg):
+        """處理 summary topic (JSON 格式)"""
+        try:
+            data = json.loads(msg.data)
+            mode = data.get('mode_name', 'UNKNOWN')
+            if mode in self.filtered_modes:
+                return
+            
+            # 從 summary 獲取原始 socket_id，並映射到分配的 socket_id
+            original_socket_id = data.get('socket_id')
+            if original_socket_id is not None:
+                # 使用原始 socket_id 獲取或分配統一的 socket_id
+                assigned_socket_id = self.get_or_assign_socket_id(original_socket_id)
+            else:
+                # 如果沒有 socket_id，使用 sys_to_socket_id 映射
+                assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0)
+            
+            sysid = data.get('sysid')
+            if sysid is not None:
+                actual_drone_id = f's{assigned_socket_id}_{sysid}'
+                
+                # ================================================================================
+                # 【關鍵】保存 sys_id 到 actual_drone_id 的映射
+                # ================================================================================
+                self.sys_to_actual_id[sys_id] = actual_drone_id
+                # ================================================================================
+            else:
+                # 如果沒有 sysid，使用 sys_id 中的數字
+                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
+            
+            self.latest_data[(actual_drone_id, 'state')] = {
+                'mode': mode,
+                'armed': data.get('armed', False),
+                'socket_id': original_socket_id,
+                'sysid': sysid,
+                'vehicle_type': data.get('vehicle_type'),
+                'autopilot': data.get('autopilot'),
+                'gps_fix': data.get('gps_fix'),
+                'gps_fix_type': data.get('gps_fix'),
+                'connected': data.get('connected')
+            }
+        except json.JSONDecodeError as e:
+            print(f"Error parsing summary JSON for {sys_id}: {e}")
+        except Exception as e:
+            print(f"Error in summary_callback for {sys_id}: {e}")
+
+    def gps_callback(self, sys_id, msg):
+        # 使用映射獲取實際的 drone_id
+        actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+        # 如果還沒有從 summary 獲取到映射，則不處理
+        if actual_drone_id is None:
+            return
+        
+        self.latest_data[(actual_drone_id, 'gps')] = {
             'lat': msg.latitude,
             'lon': msg.longitude,
             'alt': msg.altitude
@@ -606,7 +678,7 @@ class DroneMonitor(Node):
         # ================================================================================
         # 【新增】儲存 GPS 資料到 drone_gps 字典
         # ================================================================================
-        self.drone_gps[drone_id] = {
+        self.drone_gps[actual_drone_id] = {
             'lat': msg.latitude,
             'lon': msg.longitude,
             'alt': msg.altitude
@@ -632,8 +704,14 @@ class DroneMonitor(Node):
             'z': msg.z
         }
 
-    def hud_callback(self, drone_id, msg):
-        self.latest_data[(drone_id, 'hud')] = {
+    def hud_callback(self, sys_id, msg):
+        # 使用映射獲取實際的 drone_id
+        actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+        # 如果還沒有從 summary 獲取到映射，則不處理
+        if actual_drone_id is None:
+            return
+        
+        self.latest_data[(actual_drone_id, 'hud')] = {
             'airspeed': msg.airspeed,
             'groundspeed': msg.groundspeed,
             'heading': msg.heading,
@@ -655,7 +733,7 @@ class DroneMonitor(Node):
     def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
         """啟動串口 MAVLink 連接"""
         connection_name = f"Serial_{port.replace('/', '_')}"
-        receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name)
+        receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
         receiver.start()
         self.serial_receivers.append(receiver)
         print(f"Started serial connection on {port} at {baudrate} baud")

src/GUI/gui.py

@@ -13,6 +13,7 @@ from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkRece
 from map_layout import DroneMap
 from drone_panel import DronePanel, SocketGroupPanel
 from comm_panel import CommPanel
+from overview_table import OverviewTable
 
 # ================================================================================
 # 【新增】導入任務規劃器
@@ -43,23 +44,6 @@ class ControlStationUI(QMainWindow):
         # 初始化UI
         self.drones = {}
         self.socket_groups = {}
-        self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向",
-                           "Roll", "Pitch", "Yaw", "丟包", "延遲"]
-        self.info_type_map = {
-            "mode": 0,
-            "armed": 1,
-            "battery": 2,
-            "altitude": 3,
-            "local": 4,
-            "velocity": 5,
-            "groundspeed": 6,
-            "heading": 7,
-            "roll": 8,
-            "pitch": 9,
-            "yaw": 10,
-            "loss_rate": 11,
-            "ping": 12
-        }
         
         self.socket_colors = {
             '0': '#00BFFF',  # 天藍色 (DeepSkyBlue)
@@ -83,7 +67,8 @@ class ControlStationUI(QMainWindow):
         self.udp_receivers = []
         self.udp_connections = []
         self.ws_connections = []
-        self.monitor.start_serial_connection('/dev/ttyUSB0', 57600)
+        self.serial_receivers = []
+        self.serial_connections = []
         
         # ================================================================================
         # 【新增】初始化任務規劃器
@@ -119,29 +104,20 @@ class ControlStationUI(QMainWindow):
         self.left_tab.addTab(scroll, "無人載具")
         
         # — 分頁 2：Overview Table
-        self.overview_table = QTableWidget()
-        self.overview_table.setColumnCount(1)
-        self.overview_table.setRowCount(len(self.info_types))
-        self.overview_table.setHorizontalHeaderLabels(["資訊"])
-        header = self.overview_table.horizontalHeader()
-        header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents)
-        self.overview_table.verticalHeader().setVisible(False)
-        for i, txt in enumerate(self.info_types):
-            item = QTableWidgetItem(txt)
-            item.setFlags(Qt.ItemFlag.ItemIsEnabled)
-            item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)  
-            self.overview_table.setItem(i, 0, item)
-            
+        self.overview_table = OverviewTable()
         self.left_tab.addTab(self.overview_table, "總覽")
 
         # — 分頁 3：通訊設定
         self.comm_panel = CommPanel()
         self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added)
         self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added)
+        self.comm_panel.serial_connection_added.connect(self.handle_serial_connection_added)
         self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection)
         self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection)
+        self.comm_panel.serial_connection_toggled.connect(self.toggle_serial_connection)
         self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection)
         self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection)
+        self.comm_panel.serial_connection_removed.connect(self.remove_serial_connection)
         self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout))
 
         self.left_tab.addTab(self.comm_panel, "通訊")
@@ -278,6 +254,7 @@ class ControlStationUI(QMainWindow):
         # 添加地圖
         right_layout.addWidget(self.drone_map.get_widget())
         self.drone_map.get_gps_signal().connect(self.handle_map_click)
+        self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked)
 
             
         # Add widgets to splitter
@@ -299,7 +276,7 @@ class ControlStationUI(QMainWindow):
         }
         
         # 启动接收器
-        receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'])
+        receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'], self.monitor)
         receiver.start()
         self.udp_receivers.append(receiver)
         new_conn['receiver'] = receiver
@@ -322,7 +299,7 @@ class ControlStationUI(QMainWindow):
         }
         
         # 启动接收器
-        receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'])
+        receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'], self.monitor)
         receiver.start()
         self.monitor.ws_receivers.append(receiver)
         new_conn['receiver'] = receiver
@@ -436,6 +413,69 @@ class ControlStationUI(QMainWindow):
         
         self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000)
 
+    def handle_serial_connection_added(self, port, baudrate):
+        """處理添加 Serial 連接"""
+        conn = {
+            'name': f'Serial',
+            'port': port,
+            'baudrate': baudrate,
+            'enabled': False,
+            'receiver': None
+        }
+        
+        # 添加到列表
+        self.serial_connections.append(conn)
+        
+        # 在 UI 中顯示
+        panel = self.comm_panel.add_serial_panel(conn)
+        
+        self.statusBar().showMessage(f"已添加 Serial 连接: {port} @ {baudrate}", 3000)
+    
+    def toggle_serial_connection(self, conn, btn, status_label):
+        """切換 Serial 連接狀態"""
+        if conn.get('enabled', False):
+            # 停止連接
+            if conn.get('receiver'):
+                conn['receiver'].stop()
+                conn['receiver'] = None
+            
+            conn['enabled'] = False
+            btn.setText("啟動")
+            status_label.setStyleSheet("color: #888; font-size: 16px;")
+            status_label.setToolTip("已停止")
+            self.statusBar().showMessage(f"已停止 Serial 連接: {conn['port']}", 3000)
+        else:
+            # 啟動連接
+            try:
+                receiver = self.monitor.start_serial_connection(conn['port'], conn['baudrate'])
+                conn['receiver'] = receiver
+                conn['enabled'] = True
+                btn.setText("停止")
+                status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+                status_label.setToolTip("運行中")
+                self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
+            except Exception as e:
+                self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
+    
+    def remove_serial_connection(self, conn, panel):
+        """移除 Serial 連接"""
+        # 停止連接
+        if conn.get('enabled', False) and conn.get('receiver'):
+            conn['receiver'].stop()
+        
+        # 从列表移除
+        if conn in self.serial_connections:
+            self.serial_connections.remove(conn)
+        
+        # 从 comm_panel 列表移除
+        self.comm_panel.remove_serial_connection_from_list(conn)
+        
+        # 从 UI 移除
+        panel.setParent(None)
+        panel.deleteLater()
+        
+        self.statusBar().showMessage(f"已移除 Serial 连接: {conn['port']}", 3000)
+
     def create_socket_group_panel(self, socket_id):
         """創建 socket 分組面板"""
         color = self.socket_colors.get(socket_id, self.socket_colors['default'])
@@ -540,7 +580,10 @@ class ControlStationUI(QMainWindow):
             cells = round(voltage / 3.95)
 
             # 計算電量百分比
-            percentage = (voltage / cells - 3.7) / 0.5 * 100
+            if cells == 0:
+                percentage = 0
+            else:
+                percentage = (voltage / cells - 3.7) / 0.5 * 100
             
             # 根據百分比設置顏色
             if percentage < 20:
@@ -562,9 +605,12 @@ class ControlStationUI(QMainWindow):
         elif msg_type == 'gps':
             lat, lon = data.get('lat', 0), data.get('lon', 0)
             self.drone_positions[drone_id] = (lat, lon)
-            text = f"{lat:.6f}°, {lon:.6f}°"
-            self.update_field(panel, drone_id, 'gps', text)
-            self.update_overview_table(drone_id, 'gps', text)
+            lon_text = f"{lon:.6f}°"
+            lat_text = f"{lat:.6f}°"
+            self.update_field(panel, drone_id, 'longitude', lon_text)
+            self.update_field(panel, drone_id, 'latitude', lat_text)
+            self.update_overview_table(drone_id, 'longitude', lon_text)
+            self.update_overview_table(drone_id, 'latitude', lat_text)
 
             # ================================================================================
             # 【新增】同時儲存到 monitor.drone_gps（處理 UDP/WebSocket 的 GPS 資料）
@@ -578,6 +624,10 @@ class ControlStationUI(QMainWindow):
                 'alt': alt
             }
             # ================================================================================
+            
+            # 更新地圖上的無人機位置
+            heading = self.drone_headings.get(drone_id, 0)  # 如果沒有航向，使用 0
+            self.drone_map.update_drone_position(drone_id, lat, lon, heading)
 
         elif msg_type == 'altitude':
             altitude = data.get('altitude', 0)
@@ -594,15 +644,50 @@ class ControlStationUI(QMainWindow):
             heading = data.get('heading')
             self.drone_headings[drone_id] = heading
             groundspeed = data.get('groundspeed')
+            airspeed = data.get('airspeed')
+            throttle = data.get('throttle')
+            hud_alt = data.get('alt')
+            climb = data.get('climb')
+            
             heading_text = f"{heading:.1f}°"
             if isinstance(groundspeed, (int, float)):
                 groundspeed_text = f"{groundspeed:.1f} m/s"
             else:
                 groundspeed_text = "--"
+            
+            if isinstance(airspeed, (int, float)):
+                airspeed_text = f"{airspeed:.1f} m/s"
+            else:
+                airspeed_text = "--"
+            
+            if isinstance(throttle, (int, float)):
+                throttle_text = f"{throttle:.0f}%"
+            else:
+                throttle_text = "--"
+            
+            if isinstance(hud_alt, (int, float)):
+                hud_alt_text = f"{hud_alt:.1f} m"
+            else:
+                hud_alt_text = "--"
+            
+            if isinstance(climb, (int, float)):
+                climb_text = f"{climb:.1f} m/s"
+            else:
+                climb_text = "--"
+            
             self.update_field(panel, drone_id, 'heading', heading_text)
             self.update_field(panel, drone_id, 'groundspeed', groundspeed_text)
             self.update_overview_table(drone_id, 'heading', heading_text)
             self.update_overview_table(drone_id, 'groundspeed', groundspeed_text)
+            self.update_overview_table(drone_id, 'airspeed', airspeed_text)
+            self.update_overview_table(drone_id, 'throttle', throttle_text)
+            self.update_overview_table(drone_id, 'hud_alt', hud_alt_text)
+            self.update_overview_table(drone_id, 'climb', climb_text)
+            
+            # 如果有位置資訊，也更新地圖上的航向
+            if drone_id in self.drone_positions:
+                lat, lon = self.drone_positions[drone_id]
+                self.drone_map.update_drone_position(drone_id, lat, lon, heading)
 
         elif msg_type == 'loss_rate':
             loss_rate = data.get('loss_rate', 0)
@@ -631,12 +716,6 @@ class ControlStationUI(QMainWindow):
             self.update_overview_table(drone_id, 'pitch', pitch_text)
             self.update_overview_table(drone_id, 'yaw', yaw_text)
 
-        # 更新地圖上的無人機位置
-        if drone_id in self.drone_positions and drone_id in self.drone_headings:
-            lat, lon = self.drone_positions[drone_id]
-            heading = self.drone_headings[drone_id]
-            self.drone_map.update_drone_position(drone_id, lat, lon, heading)
-
     # 新增處理分組勾選的方法
     def handle_group_selection(self, socket_id, state):
         """處理 socket 分組勾選狀態變化"""
@@ -756,47 +835,23 @@ class ControlStationUI(QMainWindow):
             panel.update_field(field, text, color)
 
     def update_overview_table(self, drone_id=None, field=None, value=None):
-        # Ensure the widget is available
+        """更新總覽表格"""
         if not hasattr(self, 'overview_table') or self.overview_table is None:
             return
-
-        # Update a specific cell in the overview table
-        if drone_id and field and value:
-            col = 1 + list(self.drones.keys()).index(drone_id)
-            row = self.info_type_map.get(field, -1)  # Get row from field mapping
-
-            if row == -1:
-                return  # Invalid field, exit
-
-            item = self.overview_table.item(row, col)
-            if not item:
-                item = QTableWidgetItem()
-                self.overview_table.setItem(row, col, item)
-
-            item.setText(value)  # Update the cell's text
-            item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)  # Center the text
-
-        # If no specific update, refresh entire table
-        if drone_id is None:
-            cols = 1 + len(self.drones)
-            self.overview_table.setColumnCount(cols)
-            headers = ["資訊"] + list(self.drones.keys())
-            self.overview_table.setHorizontalHeaderLabels(headers)
-
-            for col, did in enumerate(self.drones, start=1):
-                panel = self.drones[did]
-                for field, row in self.info_type_map.items():
-                    lbl = panel.findChild(QLabel, f"{did}_{field}")
-                    val = lbl.text() if lbl else "--"
-                    val_item = QTableWidgetItem(val)
-                    val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
-                    self.overview_table.setItem(row, col, val_item)
+        
+        # 更新無人機引用
+        self.overview_table.set_drones(self.drones)
+        # 委託給 OverviewTable 處理
+        self.overview_table.update_table(drone_id, field, value)
     
     def get_socket_id(self, drone_id):
-        """從 drone_id 提取 socket_id (例如 's9_1' -> '9')"""
+        """從 drone_id 提取 socket_id (例如 's0_1' -> '0')"""
         import re
-        match = re.match(r's(\d+)_\d+', drone_id)
-        return match.group(1) if match else 'unknown'
+        match = re.match(r's(\d+)_(\d+)', drone_id)
+        if not match:
+            return 'unknown'
+        
+        return match.group(1)
 
     def add_drone(self, drone_id):
         if drone_id in self.drones:
@@ -978,6 +1033,21 @@ class ControlStationUI(QMainWindow):
             import traceback
             traceback.print_exc()
     
+    def handle_drone_clicked(self, drone_id):
+        """
+        處理地圖上無人機被點擊事件，切換該無人機的選取狀態
+        
+        Args:
+            drone_id: 無人機 ID
+        """
+        print(f"地圖上點擊無人機: {drone_id}")
+        
+        if drone_id in self.drones:
+            panel = self.drones[drone_id]
+            checkbox = panel.get_checkbox()
+            # 切換勾選狀態
+            checkbox.setChecked(not checkbox.isChecked())
+    
     def show_planned_waypoints(self):
         """
         顯示規劃的航點（在終端輸出）

src/GUI/map_layout.py

@@ -32,24 +32,120 @@ class DroneMap:
             <script src="qrc:///qtwebchannel/qwebchannel.js"></script>
             <style>
                 html, body, #map { height: 100%; margin: 0; }
+                #map {
+                    user-select: none;
+                    -webkit-user-select: none;
+                    -moz-user-select: none;
+                    -ms-user-select: none;
+                }
                 .map-controls {
                     position: absolute;
                     top: 10px;
                     right: 10px;
                     z-index: 1000;
                 }
+                .map-controls-bottom {
+                    position: absolute;
+                    bottom: 10px;
+                    left: 10px;
+                    z-index: 1000;
+                }
                 .control-button {
                     padding: 8px 12px;
-                    background-color: #f44336;
+                    background-color: #F44336;
                     color: white;
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 12px;
+                    font-size: 13px;
+                    font-weight: bold;
                     box-shadow: 0 2px 5px rgba(0,0,0,0.2);
                 }
                 .control-button:hover {
-                    background-color: #d32f2f;
+                    background-color: #D32F2F;
+                }
+                .map-toggle-button {
+                    padding: 8px 12px;
+                    background-color: #2E7D32;
+                    color: white;
+                    border: none;
+                    border-radius: 4px;
+                    cursor: pointer;
+                    font-size: 13px;
+                    font-weight: bold;
+                    box-shadow: 0 2px 5px rgba(0,0,0,0.2);
+                }
+                .map-toggle-button:hover {
+                    background-color: #1B5E20;
+                }
+                .mission-info-panel {
+                    position: absolute;
+                    bottom: 10px;
+                    right: 10px;
+                    z-index: 1000;
+                    background-color: rgba(255, 255, 255, 0.95);
+                    padding: 12px;
+                    border-radius: 6px;
+                    box-shadow: 0 2px 8px rgba(0,0,0,0.3);
+                    min-width: 200px;
+                }
+                .mission-info-row {
+                    margin-bottom: 8px;
+                    font-size: 12px;
+                    color: #333;
+                }
+                .mission-info-label {
+                    font-weight: bold;
+                    color: #555;
+                }
+                .mission-info-value {
+                    color: #2196F3;
+                    font-family: monospace;
+                }
+                .mission-start-button {
+                    width: 100%;
+                    padding: 10px;
+                    background-color: #7FBA82;
+                    color: white;
+                    border: none;
+                    border-radius: 4px;
+                    cursor: pointer;
+                    font-size: 13px;
+                    font-weight: bold;
+                    margin-top: 8px;
+                }
+                .mission-start-button:hover {
+                    background-color: #6FA872;
+                }
+                .mission-start-button:disabled {
+                    background-color: #ccc;
+                    cursor: not-allowed;
+                }
+                .selection-buttons {
+                    display: flex;
+                    flex-direction: column;
+                    gap: 5px;
+                    margin-bottom: 10px;
+                    padding-bottom: 10px;
+                    border-bottom: 1px solid #ddd;
+                }
+                .selection-button {
+                    width: 100%;
+                    padding: 8px;
+                    background-color: #64B5F6;
+                    color: white;
+                    border: none;
+                    border-radius: 4px;
+                    cursor: pointer;
+                    font-size: 13px;
+                    font-weight: bold;
+                    transition: background-color 0.2s;
+                }
+                .selection-button:hover {
+                    background-color: #42A5F5;
+                }
+                .selection-button.active {
+                    background-color: #1976D2;
                 }
             </style>
             </head>
@@ -58,6 +154,25 @@ class DroneMap:
             <div class="map-controls">
                 <button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
             </div>
+            <div class="map-controls-bottom">
+                <button class="map-toggle-button" onclick="toggleMapLayer()">地圖：預設</button>
+            </div>
+            <div class="mission-info-panel">
+                <div class="selection-buttons">
+                    <button class="selection-button" id="select-drones-btn" onclick="toggleDroneSelection()">框選無人機</button>
+                    <button class="selection-button" id="select-rect-btn" onclick="toggleRectSelection()">框選方形區域</button>
+                    <button class="selection-button" id="select-polygon-btn" onclick="togglePolygonSelection()">多點選擇區域</button>
+                </div>
+                <div class="mission-info-row">
+                    <span class="mission-info-label">中心點: </span>
+                    <span class="mission-info-value" id="center-position">未設定</span>
+                </div>
+                <div class="mission-info-row">
+                    <span class="mission-info-label">目標點: </span>
+                    <span class="mission-info-value" id="target-position">未設定</span>
+                </div>
+                <button class="mission-start-button" id="start-mission-btn" onclick="startMission()" disabled>開始任務</button>
+            </div>
 
             <script>
                 var bridge;
@@ -66,24 +181,106 @@ class DroneMap:
                 });
 
                 var map = L.map('map').setView([0, 0], 19); 
-                L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
+                
+                // 創建不同的地圖圖層
+                var streetLayer = L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
                     maxZoom: 19,
                     attribution: '© OpenStreetMap contributors'
-                }).addTo(map);
+                });
+                
+                var satelliteLayer = L.tileLayer('https://server.arcgisonline.com/ArcGIS/rest/services/World_Imagery/MapServer/tile/{z}/{y}/{x}', {
+                    maxZoom: 19,
+                    attribution: 'Tiles © Esri'
+                });
+                
+                // 默認使用街道圖
+                var currentLayer = streetLayer;
+                currentLayer.addTo(map);
+                var isSatellite = false;
+                
+                // 切換地圖圖層函數
+                function toggleMapLayer() {
+                    if (isSatellite) {
+                        map.removeLayer(satelliteLayer);
+                        streetLayer.addTo(map);
+                        isSatellite = false;
+                        document.querySelector('.map-toggle-button').textContent = '地圖：預設';
+                    } else {
+                        map.removeLayer(streetLayer);
+                        satelliteLayer.addTo(map);
+                        isSatellite = true;
+                        document.querySelector('.map-toggle-button').textContent = '地圖：衛星';
+                    }
+                }
 
                 // 地圖點擊事件
                 map.on('click', function(e) {
-                    if (bridge) {
-                        bridge.emitGpsSignal(e.latlng.lat, e.latlng.lng);
-                        console.log('點擊位置:', e.latlng.lat, e.latlng.lng);
+                    if (selectionMode === 'polygon') {
+                        // 多點選擇模式：添加點
+                        addPolygonPoint(e.latlng.lat, e.latlng.lng);
+                    } else if (!selectionMode) {
+                        // 正常模式：發送GPS信號
+                        if (bridge) {
+                            bridge.emitGpsSignal(e.latlng.lat, e.latlng.lng);
+                            console.log('點擊位置:', e.latlng.lat, e.latlng.lng);
+                        }
+                    }
+                });
+                
+                // 地圖拖曳事件（用於矩形選擇）
+                map.on('mousedown', function(e) {
+                    if (selectionMode === 'rect' || selectionMode === 'drones') {
+                        isDrawing = true;
+                        drawStartPoint = e.latlng;
+                        L.DomEvent.preventDefault(e.originalEvent);
+                        L.DomEvent.stopPropagation(e);
+                    }
+                });
+                
+                map.on('mousemove', function(e) {
+                    if (isDrawing && (selectionMode === 'rect' || selectionMode === 'drones') && drawStartPoint) {
+                        // 更新臨時矩形
+                        if (tempRectangle) {
+                            selectionLayer.removeLayer(tempRectangle);
+                        }
+                        var bounds = L.latLngBounds(drawStartPoint, e.latlng);
+                        tempRectangle = L.rectangle(bounds, {
+                            color: selectionMode === 'drones' ? '#9C27B0' : '#FF6B6B',
+                            weight: 2,
+                            fillOpacity: selectionMode === 'drones' ? 0 : 0.2,
+                            dashArray: selectionMode === 'drones' ? '5, 5' : null
+                        }).addTo(selectionLayer);
+                    }
+                });
+                
+                map.on('mouseup', function(e) {
+                    if (isDrawing && (selectionMode === 'rect' || selectionMode === 'drones') && drawStartPoint) {
+                        isDrawing = false;
+                        var bounds = L.latLngBounds(drawStartPoint, e.latlng);
+                        finishRectSelection(bounds);
+                        drawStartPoint = null;
                     }
                 });
 
-                var arrowIcon = L.icon({
-                    iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png',
-                    iconSize: [40, 40],
-                    iconAnchor: [20, 20]
+                function createArrowIcon(color) {
+                return L.divIcon({
+                    className: 'drone-arrow',
+                    html: `
+                    <div style="
+                        width: 30px; height: 30px;
+                        display: flex;
+                        align-items: center;
+                        justify-content: center;
+                    ">
+                        <svg width="30" height="30" viewBox="0 0 30 30">
+                            <polygon points="15,0 30,30  15,25  0,30" fill="${color || '#FF0000'}" />
+                        </svg>
+                    </div>
+                    `,
+                    iconSize: [30, 30],
+                    iconAnchor: [15, 15]   // ★ 箭頭往左上移，使 ID 顯示在右下
                 });
+                }
 
                 function getColorBySocketId(id) {
                     if (id.startsWith("s0_")) return "#00BFFF";  // 天藍色
@@ -100,27 +297,25 @@ class DroneMap:
                 }
 
                 function createIdIcon(id) {
-                    const color = getColorBySocketId(id);
-                    const sysid = id.split('_')[1];
-                    return L.divIcon({
-                        className: 'drone-id',
-                        html: `<div style="
-                            position: relative;
-                            left: 2px;
-                            background-color: ${color};
-                            color: black;
-                            width: 16px;
-                            height: 16px;
-                            border-radius: 50%;
-                            display: flex;
-                            align-items: center;
-                            justify-content: center;
-                            font-weight: bold;
-                            font-size: 10px;
-                            ">${sysid}</div>`,
-                        iconSize: [20, 20],
-                        iconAnchor: [10, 10]
-                    });
+                const sysid = id.split('_')[1];
+                return L.divIcon({
+                    className: 'drone-id',
+                    html: `<div style="
+                    background-color: transparent;
+                    color: black;
+                    width: 16px;
+                    height: 16px;
+                    border-radius: 50%;
+                    display: flex;
+                    align-items: center;
+                    justify-content: center;
+                    font-weight: bold;
+                    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],
+                    iconAnchor: [8, 6]   // ★ icon 中心 = 經緯度點
+                });
                 }
 
                 var markers = {};
@@ -138,6 +333,251 @@ class DroneMap:
                 var centerMarker = null;     // 中心點標記
                 var targetMarker = null;     // 目標點標記
                 var missionLine = null;      // 中心點到目標點的虛線
+                var centerPosition = null;   // 中心點經緯度
+                var targetPosition = null;   // 目標點經緯度
+                
+                // 選擇工具變量
+                var selectionMode = null;  // 'drones', 'rect', 'polygon', null
+                var selectionLayer = L.layerGroup().addTo(map);
+                var polygonPoints = [];  // 多點選擇的點
+                var polygonMarkers = [];  // 多點選擇的標記
+                var tempRectangle = null;  // 臨時矩形
+                var isDrawing = false;  // 是否正在繪製
+                var drawStartPoint = null;  // 繪製起點
+                // ================================================================================
+                
+                // 更新任務信息面板
+                function updateMissionInfo() {
+                    const centerElem = document.getElementById('center-position');
+                    const targetElem = document.getElementById('target-position');
+                    const startBtn = document.getElementById('start-mission-btn');
+                    
+                    if (centerPosition) {
+                        centerElem.textContent = `${centerPosition.lat.toFixed(6)}°, ${centerPosition.lng.toFixed(6)}°`;
+                    } else {
+                        centerElem.textContent = '未設定';
+                    }
+                    
+                    if (targetPosition) {
+                        targetElem.textContent = `${targetPosition.lat.toFixed(6)}°, ${targetPosition.lng.toFixed(6)}°`;
+                    } else {
+                        targetElem.textContent = '未設定';
+                    }
+                    
+                    // 只有當中心點和目標點都設定時才啟用按鈕
+                    if (centerPosition && targetPosition) {
+                        startBtn.disabled = false;
+                    } else {
+                        startBtn.disabled = true;
+                    }
+                }
+                
+                // ================================================================================
+                // 選擇工具函數
+                // ================================================================================
+                function toggleDroneSelection() {
+                    clearSelectionMode();
+                    if (selectionMode === 'drones') {
+                        selectionMode = null;
+                        document.getElementById('select-drones-btn').classList.remove('active');
+                        map.dragging.enable();
+                    } else {
+                        selectionMode = 'drones';
+                        document.getElementById('select-drones-btn').classList.add('active');
+                        map.dragging.disable();
+                    }
+                }
+                
+                function toggleRectSelection() {
+                    clearSelectionMode();
+                    if (selectionMode === 'rect') {
+                        selectionMode = null;
+                        document.getElementById('select-rect-btn').classList.remove('active');
+                        map.dragging.enable();
+                    } else {
+                        selectionMode = 'rect';
+                        document.getElementById('select-rect-btn').classList.add('active');
+                        map.dragging.disable();
+                    }
+                }
+                
+                function togglePolygonSelection() {
+                    if (selectionMode === 'polygon') {
+                        // 第二次點擊：完成選擇
+                        if (polygonPoints.length >= 3) {
+                            finishPolygonSelection();
+                        } else {
+                            alert('至少需要3個點來形成區域');
+                            // 清除並重置
+                            clearSelectionMode();
+                            clearPolygonPoints();
+                            selectionMode = null;
+                            document.getElementById('select-polygon-btn').classList.remove('active');
+                        }
+                    } else {
+                        // 第一次點擊：清除上次的點位並啟動模式
+                        clearSelectionMode();
+                        clearPolygonPoints();
+                        selectionMode = 'polygon';
+                        document.getElementById('select-polygon-btn').classList.add('active');
+                        map.dragging.disable();
+                    }
+                }
+                
+                function clearSelectionMode() {
+                    // 清除所有按鈕的激活狀態
+                    document.getElementById('select-drones-btn').classList.remove('active');
+                    document.getElementById('select-rect-btn').classList.remove('active');
+                    document.getElementById('select-polygon-btn').classList.remove('active');
+                    
+                    // 清除選擇圖層
+                    selectionLayer.clearLayers();
+                    tempRectangle = null;
+                    
+                    // 重新啟用地圖拖曳
+                    map.dragging.enable();
+                }
+                
+                function addPolygonPoint(lat, lng) {
+                    polygonPoints.push([lat, lng]);
+                    
+                    // 添加邊界點標記
+                    var marker = L.circleMarker([lat, lng], {
+                        radius: 5,
+                        color: '#FF6B6B',
+                        fillColor: '#FF6B6B',
+                        fillOpacity: 0.8
+                    }).addTo(selectionLayer);
+                    polygonMarkers.push(marker);
+                    
+                    // 如果有多個點，繪製連線
+                    if (polygonPoints.length > 1) {
+                        L.polyline(polygonPoints, {
+                            color: '#FF6B6B',
+                            weight: 2,
+                            dashArray: '5, 5'
+                        }).addTo(selectionLayer);
+                    }
+                    
+                    console.log('添加邊界點:', lat, lng, '總共:', polygonPoints.length);
+                }
+                
+                function clearPolygonPoints() {
+                    polygonPoints = [];
+                    polygonMarkers.forEach(m => selectionLayer.removeLayer(m));
+                    polygonMarkers = [];
+                    selectionLayer.clearLayers();
+                }
+                
+                function finishPolygonSelection() {
+                    if (polygonPoints.length < 3) {
+                        alert('至少需要3個點來形成區域');
+                        return;
+                    }
+                    
+                    // 繪製最終多邊形
+                    var polygon = L.polygon(polygonPoints, {
+                        color: '#FF6B6B',
+                        fillColor: '#FF6B6B',
+                        fillOpacity: 0.2,
+                        weight: 2
+                    }).addTo(selectionLayer);
+                    
+                    // 發送多邊形數據到Python
+                    if (bridge) {
+                        var pointsStr = JSON.stringify(polygonPoints);
+                        bridge.polygonSelected(pointsStr);
+                        console.log('多邊形選擇完成:', polygonPoints);
+                    }
+                    
+                    // 重置狀態
+                    selectionMode = null;
+                    document.getElementById('select-polygon-btn').classList.remove('active');
+                    map.dragging.enable();
+                }
+                
+                function finishRectSelection(bounds) {
+                    var selectedDrones = [];
+                    
+                    // 清除臨時矩形
+                    if (tempRectangle) {
+                        selectionLayer.removeLayer(tempRectangle);
+                        tempRectangle = null;
+                    }
+                    
+                    if (selectionMode === 'drones') {
+                        // 框選無人機模式
+                        Object.keys(markers).forEach(droneId => {
+                            var pos = markers[droneId].getLatLng();
+                            if (bounds.contains(pos)) {
+                                selectedDrones.push(droneId);
+                                // 觸發無人機點擊信號
+                                if (bridge) {
+                                    bridge.emitDroneClicked(droneId);
+                                }
+                            }
+                        });
+                        console.log('框選無人機:', selectedDrones);
+                        // 不保留選擇框，直接完成
+                        
+                    } else if (selectionMode === 'rect') {
+                        // 框選區域模式
+                        var rectPoints = [
+                            [bounds.getNorth(), bounds.getWest()],
+                            [bounds.getNorth(), bounds.getEast()],
+                            [bounds.getSouth(), bounds.getEast()],
+                            [bounds.getSouth(), bounds.getWest()]
+                        ];
+                        
+                        // 繪製最終矩形
+                        L.rectangle(bounds, {
+                            color: '#FF6B6B',
+                            fillColor: '#FF6B6B',
+                            fillOpacity: 0.2,
+                            weight: 2
+                        }).addTo(selectionLayer);
+                        
+                        // 添加四個角的標記
+                        rectPoints.forEach(point => {
+                            L.circleMarker(point, {
+                                radius: 5,
+                                color: '#FF6B6B',
+                                fillColor: '#FF6B6B',
+                                fillOpacity: 0.8
+                            }).addTo(selectionLayer);
+                        });
+                        
+                        // 發送矩形數據到Python
+                        if (bridge) {
+                            var pointsStr = JSON.stringify(rectPoints);
+                            bridge.rectangleSelected(pointsStr);
+                            console.log('矩形選擇完成:', rectPoints);
+                        }
+                    }
+                    
+                    // 重置狀態
+                    selectionMode = null;
+                    document.getElementById('select-drones-btn').classList.remove('active');
+                    document.getElementById('select-rect-btn').classList.remove('active');
+                    map.dragging.enable();
+                }
+                // ================================================================================
+                
+                // 開始任務
+                function startMission() {
+                    if (!centerPosition || !targetPosition) {
+                        alert('請先設定中心點和目標點');
+                        return;
+                    }
+                    
+                    if (bridge) {
+                        bridge.startMissionSignal(
+                            centerPosition.lat, centerPosition.lng,
+                            targetPosition.lat, targetPosition.lng
+                        );
+                        console.log('開始任務:', centerPosition, targetPosition);
+                    }
+                }
                 // ================================================================================
 
                 function initTrajectory(id) {
@@ -195,12 +635,16 @@ class DroneMap:
                         initTrajectory(id);
                         addTrajectoryPoint(id, lat, lon);
                         
+                        const color = getColorBySocketId(id);
                         markers[id] = L.marker([lat, lon], {
-                            icon: arrowIcon,
+                            icon: createArrowIcon(color),
                             rotationAngle: heading,
                             rotationOrigin: 'center'
                         })
                         .on('click', function () {
+                            if (bridge) {
+                                bridge.emitDroneClicked(id);
+                            }
                             focusOn(id);
                         })
                         .addTo(map);
@@ -210,6 +654,9 @@ class DroneMap:
                             zIndexOffset: 1000
                         })
                         .on('click', function() {
+                            if (bridge) {
+                                bridge.emitDroneClicked(id);
+                            }
                             focusOn(id);
                         })
                         .addTo(map);
@@ -237,6 +684,11 @@ class DroneMap:
                     // 清除舊的任務規劃標記
                     clearMissionPlan();
                     
+                    // 保存中心點和目標點位置
+                    centerPosition = {lat: centerLat, lng: centerLon};
+                    targetPosition = {lat: targetLat, lng: targetLon};
+                    updateMissionInfo();
+                    
                     // 繪製中心點標記 "C"（縮小到 20px）
                     var centerIcon = L.divIcon({
                         className: 'mission-center',
@@ -323,6 +775,11 @@ class DroneMap:
                         missionLine = null;
                     }
                     
+                    // 清除位置資訊
+                    centerPosition = null;
+                    targetPosition = null;
+                    updateMissionInfo();
+                    
                     console.log('任務規劃已清除');
                 }
                 // ================================================================================
@@ -410,9 +867,29 @@ class DroneMap:
         """獲取 GPS 信號"""
         return self.bridge.gps_signal
     
+    def get_drone_clicked_signal(self):
+        """獲取無人機點擊信號"""
+        return self.bridge.drone_clicked
+    
+    def get_start_mission_signal(self):
+        """獲取開始任務信號"""
+        return self.bridge.start_mission_signal
+    
+    def get_rectangle_selected_signal(self):
+        """獲取矩形選擇信號"""
+        return self.bridge.rectangle_selected
+    
+    def get_polygon_selected_signal(self):
+        """獲取多邊形選擇信號"""
+        return self.bridge.polygon_selected
+    
 class MapBridge(QObject):
     """JavaScript 和 Python 之間的橋接類"""
     gps_signal = pyqtSignal(float, float)  # lat, lon
+    drone_clicked = pyqtSignal(str)  # drone_id
+    start_mission_signal = pyqtSignal(float, float, float, float)  # center_lat, center_lon, target_lat, target_lon
+    rectangle_selected = pyqtSignal(str)  # JSON string of rectangle points
+    polygon_selected = pyqtSignal(str)  # JSON string of polygon points
     
     def __init__(self):
         super().__init__()
@@ -420,4 +897,27 @@ class MapBridge(QObject):
     @pyqtSlot(float, float)
     def emitGpsSignal(self, lat, lon):
         """供 JavaScript 調用的方法"""
-        self.gps_signal.emit(lat, lon)
+        self.gps_signal.emit(lat, lon)
+    
+    @pyqtSlot(str)
+    def emitDroneClicked(self, drone_id):
+        """供 JavaScript 調用的方法 - 當無人機被點擊時"""
+        self.drone_clicked.emit(drone_id)
+    
+    @pyqtSlot(float, float, float, float)
+    def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
+        """供 JavaScript 調用的方法 - 開始任務"""
+        self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon)
+        print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
+    
+    @pyqtSlot(str)
+    def rectangleSelected(self, points_json):
+        """供 JavaScript 調用的方法 - 矩形選擇完成"""
+        self.rectangle_selected.emit(points_json)
+        print(f"📦 矩形區域已選擇: {points_json}")
+    
+    @pyqtSlot(str)
+    def polygonSelected(self, points_json):
+        """供 JavaScript 調用的方法 - 多邊形選擇完成"""
+        self.polygon_selected.emit(points_json)
+        print(f"🔷 多邊形區域已選擇: {points_json}")

src/GUI/overview_table.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import QTableWidget, QTableWidgetItem, QHeaderView, QLabel
+from PyQt6.QtCore import Qt
+
+class OverviewTable(QTableWidget):
+    """總覽表格，顯示所有無人機的狀態資訊"""
+    
+    # 默認的資訊類型和映射
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向",
+                          "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+    
+    DEFAULT_INFO_TYPE_MAP = {
+        "mode": 0,
+        "armed": 1,
+        "battery": 2,
+        "longitude": 3,
+        "latitude": 4,
+        "altitude": 5,
+        "local": 6,
+        "velocity": 7,
+        "groundspeed": 8,
+        "heading": 9,
+        "airspeed": 10,
+        "throttle": 11,
+        "hud_alt": 12,
+        "climb": 13,
+        "roll": 14,
+        "pitch": 15,
+        "yaw": 16,
+        "loss_rate": 17,
+        "ping": 18
+    }
+    
+    def __init__(self, info_types=None, info_type_map=None, parent=None):
+        super().__init__(parent)
+        
+        # 使用提供的或默認的資訊類型
+        self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES
+        self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP
+        self.drones = {}  # 存儲無人機面板的引用
+        
+        # 初始化表格
+        self.setColumnCount(1)
+        self.setRowCount(len(self.info_types))
+        self.setHorizontalHeaderLabels(["資訊"])
+        header = self.horizontalHeader()
+        header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents)
+        self.verticalHeader().setVisible(False)
+        
+        # 設置第一列的資訊類型
+        for i, txt in enumerate(self.info_types):
+            item = QTableWidgetItem(txt)
+            item.setFlags(Qt.ItemFlag.ItemIsEnabled)
+            item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+            self.setItem(i, 0, item)
+    
+    def set_drones(self, drones):
+        """設置無人機面板字典的引用"""
+        self.drones = drones
+    
+    def update_table(self, drone_id=None, field=None, value=None):
+        """更新總覽表格
+        
+        Args:
+            drone_id: 無人機 ID
+            field: 欄位名稱 (如 'mode', 'altitude' 等)
+            value: 要更新的值
+        """
+        # 更新特定儲存格
+        if drone_id and field and value:
+            if drone_id not in self.drones:
+                return
+                
+            col = 1 + list(self.drones.keys()).index(drone_id)
+            row = self.info_type_map.get(field, -1)
+
+            if row == -1:
+                return  # 無效的欄位
+
+            item = self.item(row, col)
+            if not item:
+                item = QTableWidgetItem()
+                self.setItem(row, col, item)
+
+            item.setText(value)
+            item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+
+        # 如果沒有指定更新，刷新整個表格
+        if drone_id is None:
+            self.refresh_all()
+    
+    def refresh_all(self):
+        """刷新整個表格"""
+        cols = 1 + len(self.drones)
+        self.setColumnCount(cols)
+        headers = ["資訊"] + list(self.drones.keys())
+        self.setHorizontalHeaderLabels(headers)
+
+        for col, did in enumerate(self.drones, start=1):
+            panel = self.drones[did]
+            for field, row in self.info_type_map.items():
+                lbl = panel.findChild(QLabel, f"{did}_{field}")
+                val = lbl.text() if lbl else "--"
+                val_item = QTableWidgetItem(val)
+                val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+                self.setItem(row, col, val_item)
+    
+    def add_drone_column(self, drone_id):
+        """當新增無人機時，添加一列"""
+        if drone_id in self.drones:
+            self.refresh_all()
+    
+    def remove_drone_column(self, drone_id):
+        """當移除無人機時，刷新表格"""
+        if drone_id in self.drones:
+            self.refresh_all()

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -0,0 +1,190 @@
+
+這個檔案整理 此專案下 程式代碼的流程與思路
+只會挑出重要的變數與方法描述
+以利後續開發使用
+
+# 開發此專案的注意事項
+- 預設 autopilot 的 component id = 1
+- 不允許 system id 重複
+- 增加一個固定數值監控然後要到 ros2 topic 
+  - mavlinkROS2Node.py 檔案內 
+    - PublishRateController.topic_intervals 建立
+    - VehicleStatusPublisher._publish_vehicle_status 登記
+    - VehicleStatusPublisher._publish_XXX 實作
+  - mavlinkObject.py 檔案內 
+    - mavlink_bridge.message_handlers 登記
+    - mavlink_bridge._handle_XXX 實作
+    - mavlink_object.bridge_msg_types 登記 (這個可以用介面調)
+  - mavlinkVehicleView.py 檔案內 
+    - 注意對應的資料存放區
+
+
+---
+# 檔案結構
+
+特別注意:
+1. 有標註 [async method] 都是不該被直接呼叫的內部方法
+
+- *valuable* 這個是變數 **沒有括號**
+- *method (parameters...)* 這個是方法 **有括號**
+
+## mainOrchestrator.py : 程式進入點
+
+### **[Class]** Orchestrator 
+    最上層的發配資源與啟動終端機面板的調配者
+- *self.manager* 存放 async_io_manager 實例 
+- *self.bridge* 存放 mavlink_bridge 實例 
+- *self.plumber* 存放 serial_manager 實例
+- *self.vehicle_registry* 存放 vehicle_registry 實例
+
+- *self.panel_thread* 面板的執行緒
+- *self.panelState* 暫存面板與調配者互動的資料流動區
+  - 面板運行狀態 
+  - 面板操作結果
+  - 其他模組的運行狀態
+---
+- *mainLoop()* 核心方法 
+  - 更新個模組狀態到 *self.panelState*
+  - 對應面板來的操作指令
+---
+    對於 async_io_manager 控制實現
+- *create_udp_object()*
+- *delete_udp_object()*
+- *add_target_to_object()*
+- *remove_target_from_object()*
+---
+    關於載具管理與檢視
+- *_update_vehicles_list()*
+- *_prepare_vehicle_info()*
+---
+    關於 serial_manager 控制實現
+- *create_serial_port_object()*
+
+
+### **[Class]** ControlPanel 
+    面板的核心運行物件
+    把自己的變數 獨立出來都放到 PanelState 去
+- *panel_thread()* 核心方法 
+  - 主選單的引入
+  - 主選單下所有的按鍵操作
+  - 定義所有人為操作後續面板執行緒行為
+- *menu_tree()* 基礎選單的定義檔
+---
+    關於 udp object 的操作
+- *create_object_list_menu()* object 選單的定義檔
+- *show_object_info()* 顯示 object 資訊
+- *select_target_socket()* object 對於轉拋功能的操作
+---
+    關於 serial 的操作
+- *create_serial_port_menu()*
+- *create_linked_serial_menu()*
+- *show_linked_serial_info()*
+---
+    關於載具檢視與操作
+- *create_vehicles_list_menu()*
+- *show_vehicle_info()*
+
+### **[Class]** PanelState
+    作為面板執行緒(ControlPanel)與調配者(Orchestrator)溝通的管道
+    不包含具體實作方法 是 ControlPanel 的延伸
+- *self.panel_info_msg_list* 顯示在面板上的資訊訊息
+
+## mavlinkObject.py
+
+### 全域變數
+- *stream_bridge_ring* 
+- *return_packet_ring*
+
+### **[Class]** mavlink_bridge
+    唯一實例
+    實際去解析 mavlink 封包的地方
+    接收 stream_bridge_ring 與 return_packet_ring 的資料
+    這邊是比較偏自動化 不會被操作的
+- *self.thread* 自己的執行緒
+---
+- *_run_thread()* 核心方法
+- *_handle_XXXXX()* 每一種單項 mavlink 封包的解析
+- *send_message()* 是 _send_to_socket() 的高階包裝 跟 ros2 介面做互動的方法
+- *_send_to_socket()* 把要傳送的封包 丟給 mavlink 去處理
+
+### **[Class]** async_io_manager
+    唯一實例
+    異步 event loop 
+    沒有核心方法
+    這邊主要是管理 mavlink_object 的地方 (但不會對於某個 mavlink_object 內部需求做操作)
+    
+- *self.thread* 自己的執行緒
+- *self.managed_objects* 資料結構  socket_id: mavlink_object
+---
+- *add_mavlink_object(mavlink_object)* [call method] 把一個 mavlink_object 物件加入管理
+- *_async_add_mavlink_object(mavlink_object)* [async method] 對應上面的內部方法 不該直接使用
+- *remove_mavlink_object(socket_id)* [call method] 從管理區把指定 mavlink_object 移除
+
+### **[Class]** mavlink_object
+    儲存 mavlink socket
+    處理 mavlink 封包分流的地方
+- *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) }
+- *self.mavlink_socket* 從 pymavlink 繼承的socket物件
+- *self.state* 描述這個 socket 物件的狀態
+---
+- *process_data()* [async method] 核心方法
+- *remove_target_socket()* *add_target_socket()*
+- *message_put_queue()* 把要傳送的封包放到自己這個物件的暫存區 會由 process_data() 依照異步流程被實際丟出
+
+## serialManager.py
+    看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件
+
+### **[Class]** serial_manager
+    異步 event loop 
+    管理 mavlink_object 的地方
+- *self.thread* 自己的執行緒
+- *self.loop* 自己的事件迴圈
+---
+- *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通
+- *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序
+- *remove_serial_link()* [call method] 關閉指定的 serial 端口
+- *_async_remove_serial_link()* [async method]
+
+### **[Class]** serial_object
+    被塞在 serial_manager 裡面
+    只是一個變數物件
+    用來被儲存 serial 的資訊
+- *self.transport*
+- *self.protocol*
+- *self.udp_handler* UDP 端口物件
+- *self.serial_handler* Serial 端口物件
+
+### **[Class]** UDPHandler
+    處理 UDP 收發的端口 作為一個端口物件
+    作為 UDP OutBound 使用 所以不會佔用系統監聽資源
+- *self.transport* 自己的傳輸物件
+---
+- *datagram_received()* 先加碼成 Xbee 再呼叫 Serial 端口物件送出
+
+### **[Class]** SerialHandler
+    處理 Serial 收發的端口 作為一個端口物件
+- *self.transport* 自己的傳輸物件
+---
+- *data_received()* 先組合 Serial 封包 再解碼 再呼叫 UDP 端口物件送出
+
+## mavlinkVehicleView.py
+    這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用
+
+# 開發記錄
+
+## 已實現功能
+1. mavlink 分流解析
+2. mavlink socket 建立
+3. mavlink socket 轉拋 proxy
+4. 建立 Serial 轉 UDP 連結 並管理
+5. 建立 serial 連線
+6. 各單元模組化
+7. 終端機介面控制
+8. 基礎載具流量觀測
+9. 載具狀態收集與彙整
+10. a. ros2 topic 應用開發介面
+
+### 待開發功能
+5-1. 建立 serial 連線 並可以對接收器下達AT指令
+5-2. 模組化 serial 連線機制 以利後期擴容其他模組
+10. a. ros2  應用開發介面

src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py

@@ -9,6 +9,8 @@
 publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic>
 '''
 
+import os
+
 # ROS2 的 import
 import std_msgs.msg
 import sensor_msgs.msg
@@ -18,9 +20,9 @@ import mavros_msgs.msg
 import math
 
 # 自定義的 import
-from .theLogger import setup_logger
+from .utils import setup_logger
 
-logger = setup_logger("mavlinkPublish.py")
+logger = setup_logger(os.path.basename(__file__))
 
 class mavlink_publisher():
 

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -0,0 +1,902 @@
+"""
+MAVLink ROS2 Nodes
+包含兩個獨立的 ROS2 Node：
+1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
+2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
+
+從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
+"""
+
+import os
+import time
+import math
+import threading
+from typing import Dict, Optional
+
+# ROS2 imports
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+
+# ROS2 Message imports
+import std_msgs.msg
+import sensor_msgs.msg
+import geometry_msgs.msg
+import mavros_msgs.msg
+
+# 自定義 imports
+from . import mavlinkVehicleView as mvv
+from .utils import setup_logger
+
+logger = setup_logger(os.path.basename(__file__))
+
+
+# ============================================================================
+#                          頻率控制器
+# ============================================================================
+
+class PublishRateController:
+    """發布頻率控制器 - 按時間間隔控制發布頻率"""
+    
+    def __init__(self):
+        # ═══════════════════════════════════════════════════════════════
+        # 【新增 Topic 位置 1/4】
+        # 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔
+        # 例如：'ekf_status': 1.0,  # EKF 狀態
+        # ═══════════════════════════════════════════════════════════════
+        # 各 topic 的發布間隔（秒）
+        self.topic_intervals = {
+            'position': 0.5,       # GPS位置
+            'attitude': 0.5,       # 姿態
+            'velocity': 0.5,       # 速度
+            'battery': 1.0,        # 電池
+            'vfr_hud': 0.5,        # VFR HUD
+            'mode': 1.0,           # 飛行模式
+            'summary': 1.0,        # 載具摘要
+            # 在這裡新增更多 topics...
+        }
+        # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
+        self.last_publish_time: Dict[tuple, float] = {}
+    
+    def should_publish(self, sysid: int, topic: str) -> bool:
+        """
+        檢查是否應該發布此 topic
+        
+        Args:
+            sysid: 系統ID
+            topic: topic 名稱
+            
+        Returns:
+            bool: True 表示應該發布
+        """
+        key = (sysid, topic)
+        now = time.time()
+        
+        # 當間隔設定為0或負數時 關閉該 topic 的發布
+        interval = self.topic_intervals.get(topic, 0)
+        if interval <= 0:
+            return False 
+
+        # 首次發布
+        if key not in self.last_publish_time:
+            self.last_publish_time[key] = now
+            return True
+        
+        # 檢查時間間隔
+        if now - self.last_publish_time[key] >= interval:
+            self.last_publish_time[key] = now
+            return True
+        
+        return False
+    
+    def reset(self):
+        """重置所有計時器"""
+        self.last_publish_time.clear()
+
+
+# ============================================================================
+#                       VehicleStatusPublisher Node
+# ============================================================================
+
+class VehicleStatusPublisher(Node):
+    """
+    載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
+    
+    職責:
+    - 定期從 vehicle_registry 讀取載具狀態
+    - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
+    - 發布標準 ROS2 消息類型
+    - 檢測訂閱者，按需發布
+    """
+    topicString_prefix = f'/fc_network/vehicle'
+    
+    def __init__(self):
+        super().__init__('vehicle_status_publisher')
+        
+        # 頻率控制器
+        self.rate_controller = PublishRateController()
+        
+        # fc_publishers 字典 {(sysid, topic_name): publisher}
+        self.fc_publishers: Dict[tuple, any] = {}
+        
+        # 定時器：以較高頻率檢查 vehicle_registry 並發布
+        # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
+        self.timer_period = 0.1  # 100ms
+        self.timer = self.create_timer(self.timer_period, self.timer_callback)
+        
+        # 狀態標誌
+        self.running = True
+        
+        # logger.info("VehicleStatusPublisher initialized")
+    
+    def timer_callback(self):
+        """定時器回調 - 檢查所有載具並發布狀態"""
+        if not self.running:
+            return
+
+        # 從 vehicle_registry 獲取所有載具
+        all_vehicles = mvv.vehicle_registry.get_all()
+        
+        for sysid, vehicle in all_vehicles.items():
+            self._publish_vehicle_status(vehicle)
+    
+    def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
+        """
+        發布單個載具的所有狀態
+        
+        Args:
+            vehicle: VehicleView 實例
+        """
+        sysid = vehicle.sysid
+        
+        # 假設只有一個 autopilot component (component_id=1)
+        component = vehicle.get_component(1)
+        if not component:
+            return
+        
+        status = component.status
+        
+        # ═══════════════════════════════════════════════════════════════
+        # 【新增 Topic 位置 2/4】
+        # 若要新增 topic，請在此處調用對應的發布方法
+        # 例如：self._publish_ekf_status(sysid, status)
+        # ═══════════════════════════════════════════════════════════════
+        # 發布各種狀態（通過頻率控制器判斷是否發布）
+        self._publish_position(sysid, status)
+        self._publish_attitude(sysid, status)
+        self._publish_velocity(sysid, status)
+        self._publish_battery(sysid, status)
+        self._publish_vfr_hud(sysid, status)
+        self._publish_mode(sysid, status)
+        self._publish_summary(vehicle)
+        # 在這裡新增更多 publish 方法調用...
+    
+    def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1):
+        """
+        獲取或創建 publisher
+        
+        Args:
+            sysid: 系統ID
+            topic: topic 名稱
+            msg_type: ROS2 消息類型
+            qos: QoS 設置
+            
+        Returns:
+            publisher 對象
+        """
+        key = (sysid, topic)
+        if key not in self.fc_publishers:
+            topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
+            publisher = self.create_publisher(msg_type, topic_name, qos)
+            self.fc_publishers[key] = publisher
+            logger.info(f"Created publisher: {topic_name}")
+        return self.fc_publishers[key]
+    
+    def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 GPS 位置"""
+        if not self.rate_controller.should_publish(sysid, 'position'):
+            return
+        
+        pos = status.position
+        if pos.latitude is None or pos.longitude is None:
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix)
+        
+        # 檢查是否有訂閱者
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = sensor_msgs.msg.NavSatFix()
+        msg.latitude = pos.latitude
+        msg.longitude = pos.longitude
+        msg.altitude = pos.altitude if pos.altitude is not None else 0.0
+        
+        # GPS 狀態資訊
+        gps = status.gps
+        if gps.fix_type is not None:
+            msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
+        
+        publisher.publish(msg)
+    
+    def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
+        """發布姿態（IMU）"""
+        if not self.rate_controller.should_publish(sysid, 'attitude'):
+            return
+        
+        att = status.attitude
+        if att.roll is None:
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = sensor_msgs.msg.Imu()
+        
+        # 歐拉角轉四元數
+        qx, qy, qz, qw = self._euler_to_quaternion(
+            att.roll, att.pitch, att.yaw
+        )
+        msg.orientation.x = qx
+        msg.orientation.y = qy
+        msg.orientation.z = qz
+        msg.orientation.w = qw
+        
+        # 角速度
+        if att.rollspeed is not None:
+            msg.angular_velocity.x = att.rollspeed
+            msg.angular_velocity.y = att.pitchspeed
+            msg.angular_velocity.z = att.yawspeed
+        
+        publisher.publish(msg)
+    
+    def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus):
+        """發布速度"""
+        if not self.rate_controller.should_publish(sysid, 'velocity'):
+            return
+        
+        vfr = status.vfr
+        if vfr.groundspeed is None:
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped)
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = geometry_msgs.msg.TwistStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        
+        # 使用 VFR HUD 的地速和航向計算速度分量
+        if vfr.heading is not None:
+            heading_rad = math.radians(vfr.heading)
+            msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad)
+            msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad)
+        
+        # 爬升率作為 z 軸速度
+        if vfr.climb is not None:
+            msg.twist.linear.z = vfr.climb
+        
+        publisher.publish(msg)
+    
+    def _publish_battery(self, sysid: int, status: mvv.ComponentStatus):
+        """發布電池狀態"""
+        if not self.rate_controller.should_publish(sysid, 'battery'):
+            return
+        
+        bat = status.battery
+        if bat.voltage is None:
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState)
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = sensor_msgs.msg.BatteryState()
+        msg.voltage = bat.voltage
+        
+        if bat.current is not None:
+            msg.current = bat.current
+        
+        if bat.remaining is not None:
+            msg.percentage = bat.remaining / 100.0
+        
+        if bat.temperature is not None:
+            msg.temperature = bat.temperature
+        
+        publisher.publish(msg)
+    
+    def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 VFR HUD"""
+        if not self.rate_controller.should_publish(sysid, 'vfr_hud'):
+            return
+        
+        vfr = status.vfr
+        if vfr.airspeed is None:
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud)
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = mavros_msgs.msg.VfrHud()
+        msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0
+        msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0
+        msg.heading = vfr.heading if vfr.heading is not None else 0
+        msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0
+        msg.climb = vfr.climb if vfr.climb is not None else 0.0
+        
+        # altitude 需要從 position 獲取
+        if status.position.altitude is not None:
+            msg.altitude = status.position.altitude
+        
+        publisher.publish(msg)
+    
+    def _publish_mode(self, sysid: int, status: mvv.ComponentStatus):
+        """發布飛行模式"""
+        if not self.rate_controller.should_publish(sysid, 'mode'):
+            return
+        
+        mode = status.mode
+        if mode.mode_name is None:
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String)
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        msg = std_msgs.msg.String()
+        msg.data = mode.mode_name
+        publisher.publish(msg)
+    
+    def _publish_summary(self, vehicle: mvv.VehicleView):
+        """
+        發布載具摘要資訊（自定義格式，使用 String 暫時代替）
+        TODO: 未來可以定義專門的 VehicleSummary.msg
+        """
+        sysid = vehicle.sysid
+        
+        if not self.rate_controller.should_publish(sysid, 'summary'):
+            return
+        
+        publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String)
+        
+        if publisher.get_subscription_count() == 0:
+            return
+        
+        # 獲取 autopilot component
+        component = vehicle.get_component(1)
+        if not component:
+            return
+        
+        status = component.status
+        
+        # 構建摘要資訊（JSON 格式字串）
+        import json
+        summary = {
+            'sysid': sysid,
+            'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0,
+            'autopilot': component.mav_autopilot if component.mav_autopilot else 0,
+            'socket_id': vehicle.custom_meta.get('socket_id', -1),  # 重要！
+            'armed': status.armed if status.armed is not None else False,
+            # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
+            'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
+            # 'latitude': status.position.latitude if status.position.latitude else 0.0,
+            # 'longitude': status.position.longitude if status.position.longitude else 0.0,
+            # 'altitude': status.position.altitude if status.position.altitude else 0.0,
+            # 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
+            'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
+            'connection_type': vehicle.connected_via.value,
+            'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
+        }
+        
+        msg = std_msgs.msg.String()
+        msg.data = json.dumps(summary)
+        publisher.publish(msg)
+    
+    # ═══════════════════════════════════════════════════════════════
+    # 【新增 Topic 位置 3/4】
+    # 若要新增 topic，請在此處實作對應的發布方法
+    # 方法命名規則：def _publish_<topic_name>(self, sysid: int, status: mvv.ComponentStatus):
+    # 例如：
+    # def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus):
+    #     """發布 EKF 狀態"""
+    #     if not self.rate_controller.should_publish(sysid, 'ekf_status'):
+    #         return
+    #     
+    #     ekf = status.ekf
+    #     if ekf.flags is None:
+    #         return
+    #     
+    #     publisher = self._get_or_create_publisher(sysid, 'ekf_status', ...
+    #     # ... 實作發布邏輯
+    # ═══════════════════════════════════════════════════════════════
+    
+    @staticmethod
+    def _euler_to_quaternion(roll, pitch, yaw):
+        """
+        歐拉角轉四元數
+        
+        Args:
+            roll: 橫滾角 (弧度)
+            pitch: 俯仰角 (弧度)
+            yaw: 偏航角 (弧度)
+            
+        Returns:
+            tuple: (qx, qy, qz, qw)
+        """
+        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
+             math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
+             math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
+        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
+             math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
+        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
+             math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+        return (qx, qy, qz, qw)
+    
+    def stop(self):
+        """停止發布"""
+        self.running = False
+        # logger.info("VehicleStatusPublisher stopped")
+
+
+# ============================================================================
+#                       MavlinkCommandService Node
+# ============================================================================
+
+class MavlinkCommandService(Node):
+    """
+    MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令
+    
+    職責:
+    - 作為 service server，等待 client 請求
+    - 接收請求，組裝 MAVLink 封包
+    - 調用 mavlinkObject 發送封包
+    - 處理 ACK 等待和超時（未來實現）
+    
+    設計理念：回歸 MAVLink 純粹結構
+    - 只負責將 ROS2 請求轉換為 MAVLink 封包
+    - 不預設功能（如 ARM/DISARM），保持模組化
+    - 高層應用可透過此 service 實現各種功能
+    """
+    
+    def __init__(self):
+        super().__init__('mavlink_command_service')
+        
+        # ═══════════════════════════════════════════════════════════════════
+        # ROS2 Service 架構說明：
+        # 
+        # 1. Service 定義：由 .srv 檔案定義（Request + Response）
+        #    - Request: client 發送的請求內容
+        #    - Response: server 回傳的結果
+        # 
+        # 2. Service Server 創建：
+        #    self.create_service(srv_type, service_name, callback_function)
+        #    - srv_type: service 的訊息類型（需要自定義或使用標準）
+        #    - service_name: service 的名稱（client 用此名稱呼叫）
+        #    - callback_function: 處理請求的回調函數
+        # 
+        # 3. Callback 函數：
+        #    def callback(self, request, response):
+        #        # request: 包含 client 發送的數據
+        #        # response: 需要填充並返回給 client
+        #        return response
+        # 
+        # 4. Service Client 使用方式（在其他程式中）：
+        #    client = node.create_client(srv_type, service_name)
+        #    request = srv_type.Request()
+        #    future = client.call_async(request)  # 異步調用
+        #    # 或 response = client.call(request)  # 同步調用
+        # ═══════════════════════════════════════════════════════════════════
+        
+        # 由於 ROS2 自定義 service 需要 .srv 檔案編譯
+        # 這裡先使用標準 String service 作為簡化實現
+        # TODO: 未來可創建專門的 .srv 檔案
+        from std_srvs.srv import Trigger
+        from example_interfaces.srv import SetBool
+        
+        # ═══════════════════════════════════════════════════════════════════
+        # Service 1: 發送 MAVLink Message（通用介面）
+        # 使用 Trigger 作為臨時實現，未來應使用自定義 service
+        # ═══════════════════════════════════════════════════════════════════
+        # TODO: 創建 SendMavlinkMessage.srv
+        # Request:
+        #   uint8 target_sysid
+        #   uint8 target_compid
+        #   uint16 message_id
+        #   string fields_json  # JSON 格式的字段數據
+        #   bool wait_response
+        #   uint16 response_msgid
+        #   float32 timeout
+        # Response:
+        #   bool success
+        #   string response_json
+        #   string error_message
+        
+        # 暫時使用簡化版本（僅示範架構）
+        self.srv_send_message = self.create_service(
+            Trigger,
+            '/mavlink/send_message',
+            self.handle_send_message
+        )
+        
+        # ═══════════════════════════════════════════════════════════════════
+        # Service 2: 發送 COMMAND_LONG
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_command_long = self.create_service(
+            Trigger,
+            '/mavlink/send_command_long',
+            self.handle_command_long
+        )
+        
+        # ═══════════════════════════════════════════════════════════════════
+        # Service 3: 參數請求
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_param_request = self.create_service(
+            Trigger,
+            '/mavlink/param_request',
+            self.handle_param_request
+        )
+        
+        # 狀態標記
+        self.running = True
+        
+        # mavlinkObject 的引用（將由外部設置）
+        self.mavlink_analyzer = None
+        
+        logger.info("MavlinkCommandService initialized")
+    
+    def set_mavlink_analyzer(self, mavlink_analyzer):
+        """
+        設置 mavlink_analyzer 引用
+        
+        Args:
+            mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例
+        """
+        self.mavlink_analyzer = mavlink_analyzer
+        logger.info("MavlinkCommandService: mavlink_analyzer set")
+    
+    # ═══════════════════════════════════════════════════════════════════════
+    # Service Handler: 發送 MAVLink Message
+    # ═══════════════════════════════════════════════════════════════════════
+    def handle_send_message(self, request, response):
+        """
+        處理發送 MAVLink 訊息的請求
+        
+        ROS2 Service Callback 說明：
+        - 此函數會在 client 調用 service 時被執行
+        - request: 包含 client 傳入的參數
+        - response: 需要填充結果並返回給 client
+        - 必須 return response
+        
+        Args:
+            request: Trigger.Request (暫時使用，未來改為自定義)
+            response: Trigger.Response
+            
+        Returns:
+            response: 填充後的回應
+        """
+        logger.info("Received send_message request")
+        
+        # 檢查 mavlink_analyzer 是否已設置
+        if self.mavlink_analyzer is None:
+            response.success = False
+            response.message = "Error: mavlink_analyzer not set"
+            logger.error(response.message)
+            return response
+        
+        # TODO: 實際實現
+        # 1. 從 request 解析參數（target_sysid, message_id, fields 等）
+        # 2. 使用 pymavlink 組裝 MAVLink 封包
+        # 3. 調用 mavlink_analyzer.send_message() 發送
+        # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應
+        
+        # 暫時返回成功（示範用）
+        response.success = True
+        response.message = "Message sent (placeholder implementation)"
+        return response
+    
+    # ═══════════════════════════════════════════════════════════════════════
+    # Service Handler: 發送 COMMAND_LONG
+    # ═══════════════════════════════════════════════════════════════════════
+    def handle_command_long(self, request, response):
+        """
+        處理發送 COMMAND_LONG 的請求
+        
+        COMMAND_LONG (MAVLink message ID=76):
+        - 用於發送簡單命令給載具
+        - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等
+        
+        Args:
+            request: Trigger.Request
+            response: Trigger.Response
+            
+        Returns:
+            response: 填充後的回應
+        """
+        logger.info("Received command_long request")
+        
+        if self.mavlink_analyzer is None:
+            response.success = False
+            response.message = "Error: mavlink_analyzer not set"
+            return response
+        
+        # TODO: 實際實現
+        # 1. 從 request 解析 COMMAND_LONG 參數
+        #    - target_sysid, target_compid
+        #    - command (MAV_CMD_xxx)
+        #    - param1~param7
+        # 2. 組裝 COMMAND_LONG 封包
+        # 3. 發送並等待 COMMAND_ACK (message ID=77)
+        # 4. 解析 ACK 結果（ACCEPTED/FAILED 等）
+        
+        response.success = True
+        response.message = "Command sent (placeholder implementation)"
+        return response
+    
+    # ═══════════════════════════════════════════════════════════════════════
+    # Service Handler: 參數請求
+    # ═══════════════════════════════════════════════════════════════════════
+    def handle_param_request(self, request, response):
+        """
+        處理參數讀取請求
+        
+        MAVLink 參數協議：
+        - PARAM_REQUEST_READ (ID=20): 請求讀取參數
+        - PARAM_VALUE (ID=22): 參數值回應
+        - PARAM_SET (ID=23): 設置參數值
+        
+        ═══════════════════════════════════════════════════════════════════
+        【使用 mavlinkObject 回應機制的步驟】
+        
+        1. 設置回應訊息類型：
+           self.mavlink_analyzer.set_return_message_types([22])  # PARAM_VALUE
+           
+        2. 發送請求封包：
+           message_bytes = ... # 組裝 PARAM_REQUEST_READ
+           self.mavlink_analyzer.send_message(
+               message_bytes,
+               target_sysid=1
+           )
+           
+        3. 監聽回應（在獨立線程或定時器中）：
+           from ..fc_network_adapter import mavlinkObject as mo
+           
+           # 等待回應（帶超時）
+           timeout = 3.0
+           start_time = time.time()
+           while time.time() - start_time < timeout:
+               items = mo.return_packet_ring.get_all()
+               for socket_id, timestamp, msg in items:
+                   if msg.get_type() == 'PARAM_VALUE':
+                       # 找到回應！
+                       param_id = msg.param_id
+                       param_value = msg.param_value
+                       # 處理回應...
+                       return
+               time.sleep(0.01)  # 短暫等待
+           
+           # 超時處理
+           
+        4. 清理（可選）：
+           self.mavlink_analyzer.set_return_message_types([])  # 清空
+           mo.return_packet_ring.clear()  # 清空緩衝區
+        
+        注意事項：
+        - return_packet_ring 是全域的，所有 mavlink_object 共用
+        - 需要通過 socket_id 或 sysid 來識別回應來源
+        - 實際使用時建議實現專門的回應管理器
+        ═══════════════════════════════════════════════════════════════════
+        
+        Args:
+            request: Trigger.Request
+            response: Trigger.Response
+            
+        Returns:
+            response: 填充後的回應
+        """
+        logger.info("Received param_request")
+        
+        if self.mavlink_analyzer is None:
+            response.success = False
+            response.message = "Error: mavlink_analyzer not set"
+            return response
+        
+        # TODO: 實際實現
+        # 1. 從 request 解析參數名稱或索引
+        # 2. 設置 mavlink_analyzer.set_return_message_types([22])  # PARAM_VALUE
+        # 3. 發送 PARAM_REQUEST_READ
+        # 4. 監聽 return_packet_ring，等待 PARAM_VALUE
+        # 5. 解析回應並填充到 response
+        
+        response.success = True
+        response.message = "Param request sent (placeholder implementation)"
+        return response
+    
+    # ═══════════════════════════════════════════════════════════════════════
+    # 【新增 Service 位置 4/4】
+    # 若要新增 service，請在此處添加新的 handler 方法
+    # 並在 __init__ 中創建對應的 service server
+    # ═══════════════════════════════════════════════════════════════════════
+    
+    def stop(self):
+        """停止服務"""
+        self.running = False
+        logger.info("MavlinkCommandService stopped")
+
+
+# ============================================================================
+#                         ROS2 節點管理器
+# ============================================================================
+
+class fc_ros_manager:
+    """
+    MAVLink ROS2 節點管理器
+    
+    管理兩個獨立的 ROS2 Node：
+    - VehicleStatusPublisher
+    - MavlinkCommandService
+    
+    提供統一的啟動/停止介面給 mainOrchestrator
+    """
+    
+    def __init__(self):
+        self.initialized = False
+        self.running = False
+        
+        # 两个 node 实例
+        self.status_publisher: Optional[VehicleStatusPublisher] = None
+        self.command_service: Optional[MavlinkCommandService] = None
+        
+        # Executor & Thread
+        self.spin_thread: Optional[threading.Thread] = None
+        self.executor: Optional[MultiThreadedExecutor] = None
+    
+    def initialize(self):
+        """初始化 ROS2 环境和 nodes"""
+        if self.initialized:
+            logger.warning("fc_ros_manager already initialized")
+            return False
+        
+        try:
+            # 初始化 ROS2
+            rclpy.init()
+            
+            # 創建節點 node
+            self.status_publisher = VehicleStatusPublisher()
+            self.command_service = MavlinkCommandService()
+    
+            # 創建執行者 MultiThreadedExecutor
+            self.executor = MultiThreadedExecutor()
+            self.executor.add_node(self.status_publisher)
+            self.executor.add_node(self.command_service)
+            
+            self.initialized = True
+            # logger.info("fc_ros_manager initialized")
+            return True
+            
+        except Exception as e:
+            logger.error(f"Failed to initialize fc_ros_manager: {e}")
+            return False
+    
+    def start(self):
+        """啟動 ROS2 nodes (在獨立執行緒中運行 executor) """
+        if not self.initialized:
+            logger.error("fc_ros_manager initialize failed or not called")
+            return False
+        
+        if self.running:
+            logger.warning("fc_ros_manager already running")
+            return False
+        
+        try:
+            self.running = True
+            
+            self.spin_thread = threading.Thread(
+                target=self._spin_executor,
+                daemon=True,
+                name="ROS2ExecutorThread"
+            )
+            self.spin_thread.start()
+            
+            logger.info("fc_ros_manager started <-")
+            return True
+            
+        except Exception as e:
+            logger.error(f"Failed to start fc_ros_manager: {e}")
+            self.running = False
+            return False
+    
+    def _spin_executor(self):
+        """在 thread 中運行的 executor"""
+        try:
+            # logger.info("ROS2 executor spinning...")
+            while self.running:
+                self.executor.spin_once(timeout_sec=0.1)
+        except Exception as e:
+            logger.error(f"fc_ros_manager error in spinning executor: {e}")
+    
+    def stop(self):
+        """停止 ROS2 nodes"""
+        if not self.running:
+            logger.warning("fc_ros_manager not running")
+            return False
+        
+        try:            
+            # 標記停止
+            self.running = False
+            
+            # 停止各個 node
+            if self.status_publisher:
+                self.status_publisher.stop()
+            if self.command_service:
+                self.command_service.stop()
+            
+            # 等待 spin 執行緒結束
+            if self.spin_thread and self.spin_thread.is_alive():
+                self.spin_thread.join(timeout=2.0)
+            
+            logger.info("fc_ros_manager thread END!")
+            return True
+            
+        except Exception as e:
+            logger.error(f"Error stopping fc_ros_manager: {e}")
+            return False
+    
+    def shutdown(self):
+        """完全關閉並清理資源"""
+        if self.running:
+            self.stop()
+        
+        if self.initialized:
+            try:
+                # 銷毀 nodes
+                if self.status_publisher:
+                    self.status_publisher.destroy_node()
+                if self.command_service:
+                    self.command_service.destroy_node()
+                
+                # 關閉 ROS2
+                if rclpy.ok():
+                    rclpy.shutdown()
+                
+                self.initialized = False
+                logger.info("fc_ros_manager Node END!")
+                
+            except Exception as e:
+                logger.error(f"Error during shutdown: {e}")
+    
+    def get_status(self) -> dict:
+        return {
+            'initialized': self.initialized,
+            'running': self.running,
+            'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
+            'command_service_active': self.command_service is not None,
+        }
+
+
+# ============================================================================
+#                              全域實例
+# ============================================================================
+
+# 全域管理器實例（供 mainOrchestrator 使用）
+ros2_manager = fc_ros_manager()
+
+
+'''
+================= 改版記錄 ============================
+
+2026.01.20
+1. 重構自 mavlinkPublish.py (該檔案將被棄用)
+2. 提供 fc_ros_manager 統一管理介面
+3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態
+4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
+5. 預留 MavlinkCommandService 結構（稍後實現）
+
+'''

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -0,0 +1,453 @@
+"""
+VehicleView - Pure State Container
+純粹的狀態容器，不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量
+只提供狀態存取介面，由外部手動餵資料（push state）
+"""
+
+import os
+from typing import Dict, Optional, Any
+from dataclasses import dataclass, field
+from enum import Enum
+
+from .utils import setup_logger
+
+logger = setup_logger(os.path.basename(__file__))
+
+
+# ====================== Enums =====================
+
+class ConnectionType(Enum):
+    """連接類型"""
+    SERIAL = "serial"
+    UDP = "udp"
+    TCP = "tcp"
+    OTHER = "other"
+
+
+class ComponentType(Enum):
+    """組件類型"""
+    AUTOPILOT = "autopilot"
+    GCS = "gcs"
+    CAMERA = "camera"
+    GIMBAL = "gimbal"
+    OTHER = "other"
+
+
+class RFModuleType(Enum):
+    """RF模組類型"""
+    XBEE = "xbee"
+    UDP = "udp"
+    TCP = "tcp"
+    OTHER = "other"
+
+
+# ====================== Data Classes =====================
+
+@dataclass
+class Position:
+    """位置資訊"""
+    latitude: Optional[float] = None  # 緯度 (度)
+    longitude: Optional[float] = None  # 經度 (度)
+    altitude: Optional[float] = None  # 海拔高度 (公尺)
+    relative_altitude: Optional[float] = None  # 相對高度 (公尺)
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class Attitude:
+    """姿態資訊"""
+    roll: Optional[float] = None  # 橫滾角 (弧度)
+    pitch: Optional[float] = None  # 俯仰角 (弧度)
+    yaw: Optional[float] = None  # 偏航角 (弧度)
+    rollspeed: Optional[float] = None  # 橫滾速度 (弧度/秒)
+    pitchspeed: Optional[float] = None  # 俯仰速度 (弧度/秒)
+    yawspeed: Optional[float] = None  # 偏航速度 (弧度/秒)
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class FlightMode:
+    """飛行模式資訊"""
+    base_mode: Optional[int] = None  # MAVLink base mode
+    custom_mode: Optional[int] = None  # 自定義模式
+    mode_name: Optional[str] = None  # 模式名稱 (例如: "GUIDED", "AUTO")
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class Battery:
+    """電池資訊"""
+    voltage: Optional[float] = None  # 電壓 (V)
+    current: Optional[float] = None  # 電流 (A)
+    remaining: Optional[int] = None  # 剩餘電量 (%)
+    temperature: Optional[float] = None  # 溫度 (攝氏)
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class EKF:
+    """EKF狀態資訊"""
+    flags: Optional[int] = None  # EKF 旗標
+    velocity_variance: Optional[float] = None  # 速度變異
+    pos_horiz_variance: Optional[float] = None  # 水平位置變異
+    pos_vert_variance: Optional[float] = None  # 垂直位置變異
+    compass_variance: Optional[float] = None  # 羅盤變異
+    terrain_alt_variance: Optional[float] = None  # 地形高度變異
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class GPS:
+    """GPS資訊"""
+    fix_type: Optional[int] = None  # GPS fix 類型 (0=無, 1=無fix, 2=2D, 3=3D, 4=DGPS, 5=RTK)
+    satellites_visible: Optional[int] = None  # 可見衛星數
+    eph: Optional[int] = None  # GPS HDOP 水平精度因子
+    epv: Optional[int] = None  # GPS VDOP 垂直精度因子
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class VFR:
+    """VFR HUD資訊"""
+    airspeed: Optional[float] = None  # 空速 (m/s)
+    groundspeed: Optional[float] = None  # 地速 (m/s)
+    heading: Optional[int] = None  # 航向 (度)
+    throttle: Optional[int] = None  # 油門 (%)
+    climb: Optional[float] = None  # 爬升率 (m/s)
+    timestamp: Optional[float] = None  # 時間戳記
+
+
+@dataclass
+class ComponentStatus:
+    """組件狀態容器"""
+    position: Position = field(default_factory=Position)
+    attitude: Attitude = field(default_factory=Attitude)
+    mode: FlightMode = field(default_factory=FlightMode)
+    battery: Battery = field(default_factory=Battery)
+    ekf: EKF = field(default_factory=EKF)
+    gps: GPS = field(default_factory=GPS)
+    vfr: VFR = field(default_factory=VFR)
+    
+    # 系統狀態
+    system_status: Optional[int] = None  # MAV_STATE
+    armed: Optional[bool] = None  # 解鎖狀態
+    
+    # 其他自定義狀態
+    custom_status: Dict[str, Any] = field(default_factory=dict)
+
+
+@dataclass
+class PacketStats:
+    """封包統計資訊"""
+    received_count: int = 0  # 接收封包數
+    lost_count: int = 0  # 遺失封包數
+    last_seq: Optional[int] = None  # 最後序號
+    last_msg_time: Optional[float] = None  # 最後訊息時間
+    msg_type_count: Dict[int, int] = field(default_factory=dict)  # 各類訊息計數 {msg_type: count}
+
+
+@dataclass
+class RFStatus:
+    """RF模組狀態"""
+    rssi: Optional[int] = None  # 信號強度
+    noise: Optional[int] = None  # 噪音水平
+    at_response: Optional[str] = None  # AT 命令回應
+    link_quality: Optional[int] = None  # 連接品質
+    timestamp: Optional[float] = None  # 時間戳記
+    custom_status: Dict[str, Any] = field(default_factory=dict)  # 其他自定義狀態
+
+
+@dataclass
+class SocketInfo:
+    """Socket連接資訊"""
+    ip: Optional[str] = None  # IP位址
+    port: Optional[int] = None  # 埠號
+    local_ip: Optional[str] = None  # 本地IP
+    local_port: Optional[int] = None  # 本地埠號
+    connected: bool = False  # 連接狀態
+
+
+# ====================== Component Class =====================
+
+class VehicleComponent:
+    """載具組件 - 代表一個 MAVLink component"""
+    
+    def __init__(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER):
+        self.component_id = component_id
+        self.type = comp_type
+        
+        # MAVLink 組件資訊
+        self.mav_type: Optional[int] = None  # MAV_TYPE
+        self.mav_autopilot: Optional[int] = None  # MAV_AUTOPILOT
+        
+        # 狀態容器
+        self.status = ComponentStatus()
+        
+        # 參數容器 (只存放，不主動下載)
+        self.parameters: Dict[str, Any] = {}
+        
+        # 封包統計
+        self.packet_stats = PacketStats()
+    
+    def update_packet_stats(self, seq: int, msg_type: int, timestamp: float) -> None:
+        """
+        更新封包統計
+        
+        Args:
+            seq: 訊息序號
+            msg_type: 訊息類型
+            timestamp: 時間戳記
+        """
+        stats = self.packet_stats
+        
+        # 檢查遺失封包
+        if stats.last_seq is not None:
+            expected_seq = (stats.last_seq + 1) % 256
+            diff = seq - expected_seq
+            if diff < 0:
+                diff += 256
+            stats.lost_count += diff
+        
+        # 更新統計資訊
+        stats.received_count += 1
+        stats.last_seq = seq
+        stats.last_msg_time = timestamp
+        
+        # 更新訊息類型計數
+        if msg_type in stats.msg_type_count:
+            stats.msg_type_count[msg_type] += 1
+        else:
+            stats.msg_type_count[msg_type] = 1
+    
+    def reset_packet_stats(self) -> None:
+        """重置封包統計"""
+        self.packet_stats = PacketStats()
+
+    def set_parameter(self, param_name: str, param_value: Any) -> None:
+        """設定參數 (手動餵入)"""
+        self.parameters[param_name] = param_value
+
+    def get_parameter(self, param_name: str, default: Any = None) -> Any:
+        """取得參數"""
+        return self.parameters.get(param_name, default)
+
+    def __str__(self) -> str:
+        return (f"Component(id={self.component_id}, type={self.type.value}, "
+                f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, "
+                f"lost={self.packet_stats.lost_count})")
+
+
+# ====================== RF Module Class =====================
+
+class RFModule:
+    """RF模組資訊容器"""
+    
+    def __init__(self, rf_type: RFModuleType = RFModuleType.OTHER):
+        self.type = rf_type
+        self.status = RFStatus()
+        self.socket_info = SocketInfo()
+    
+    def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
+        """更新RSSI"""
+        self.status.rssi = rssi
+        if timestamp:
+            self.status.timestamp = timestamp
+    
+    def update_at_response(self, response: str, timestamp: Optional[float] = None) -> None:
+        """更新AT命令回應"""
+        self.status.at_response = response
+        if timestamp:
+            self.status.timestamp = timestamp
+    
+    def update_socket_info(self, ip: str = None, port: int = None, 
+                          local_ip: str = None, local_port: int = None,
+                          connected: bool = None) -> None:
+        """更新Socket資訊"""
+        if ip is not None:
+            self.socket_info.ip = ip
+        if port is not None:
+            self.socket_info.port = port
+        if local_ip is not None:
+            self.socket_info.local_ip = local_ip
+        if local_port is not None:
+            self.socket_info.local_port = local_port
+        if connected is not None:
+            self.socket_info.connected = connected
+    
+    def __str__(self) -> str:
+        return (f"RFModule(type={self.type.value}, rssi={self.status.rssi}, "
+                f"connected={self.socket_info.connected})")
+
+
+# ====================== Main VehicleView Class =====================
+
+class VehicleView:
+    """
+    載具視圖 - 純狀態容器
+    
+    特點:
+    1. 只有狀態容器，沒有任何主動通訊功能
+    2. 不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量
+    3. 可以手動餵資料 (push state)
+    4. 可擴充 (支援 RF 模組狀態)
+    """
+    
+    # TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了
+
+    def __init__(self, sysid: int):
+        # Meta 資訊
+        self.sysid = sysid
+        self.kind: Optional[str] = None  # 載具種類描述 (例如: "Copter", "Plane")
+        self.vehicle_type: Optional[int] = None  # MAV_TYPE
+        self.connected_via: ConnectionType = ConnectionType.OTHER
+        
+        # 組件容器 {component_id: VehicleComponent}
+        self.components: Dict[int, VehicleComponent] = {}
+        
+        # RF模組
+        self.rf_module: Optional[RFModule] = None
+        
+        # 其他自定義meta資訊
+        self.custom_meta: Dict[str, Any] = {}
+    
+    def add_component(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER) -> VehicleComponent:
+        """
+        新增組件
+        
+        Args:
+            component_id: 組件ID
+            comp_type: 組件類型
+            
+        Returns:
+            VehicleComponent: 新增的組件
+        """
+        if component_id not in self.components:
+            self.components[component_id] = VehicleComponent(component_id, comp_type)
+            # logger.debug(f"Added component {component_id} to system {self.sysid}")
+        return self.components[component_id]
+    
+    def get_component(self, component_id: int) -> Optional[VehicleComponent]:
+        """取得組件"""
+        return self.components.get(component_id)
+    
+    def remove_component(self, component_id: int) -> bool:
+        """移除組件"""
+        if component_id in self.components:
+            del self.components[component_id]
+            # logger.debug(f"Removed component {component_id} from system {self.sysid}")
+            return True
+        return False
+
+    def reset_component_stats(self, component_id: int) -> None:
+        """重置指定組件的封包統計"""
+        component = self.get_component(component_id)
+        if component:
+            component.reset_packet_stats()
+            # logger.info(f"Reset packet stats for component {component_id} in system {self.sysid}")
+
+    def set_rf_module(self, rf_type: RFModuleType) -> RFModule:
+        """設定RF模組"""
+        self.rf_module = RFModule(rf_type)
+        return self.rf_module
+    
+    def get_rf_module(self) -> Optional[RFModule]:
+        """取得RF模組"""
+        return self.rf_module
+    
+    def __str__(self) -> str:
+        comp_list = ", ".join([str(cid) for cid in self.components.keys()])
+        return (f"VehicleView(sysid={self.sysid}, kind={self.kind}, "
+                f"connected_via={self.connected_via.value}, "
+                f"components=[{comp_list}], "
+                f"rf_module={self.rf_module is not None})")
+    
+    def to_dict(self) -> Dict[str, Any]:
+        """轉換為字典格式 (方便序列化/除錯)"""
+        return {
+            'meta': {
+                'sysid': self.sysid,
+                'kind': self.kind,
+                'vehicle_type': self.vehicle_type,
+                'connected_via': self.connected_via.value,
+                'custom_meta': self.custom_meta
+            },
+            'components': {
+                cid: {
+                    'component_id': comp.component_id,
+                    'type': comp.type.value,
+                    'mav_type': comp.mav_type,
+                    'mav_autopilot': comp.mav_autopilot,
+                    'packet_stats': {
+                        'received': comp.packet_stats.received_count,
+                        'lost': comp.packet_stats.lost_count,
+                        'last_seq': comp.packet_stats.last_seq,
+                        'last_msg_time': comp.packet_stats.last_msg_time
+                    },
+                    'parameters_count': len(comp.parameters)
+                }
+                for cid, comp in self.components.items()
+            },
+            'rf_module': {
+                'type': self.rf_module.type.value,
+                'rssi': self.rf_module.status.rssi,
+                'socket_connected': self.rf_module.socket_info.connected
+            } if self.rf_module else None
+        }
+
+
+# ====================== Registry =====================
+
+class VehicleViewRegistry:
+    """載具視圖註冊表 - 管理所有的 VehicleView"""
+    
+    def __init__(self):
+        self._vehicles: Dict[int, VehicleView] = {}
+    
+    def register(self, sysid: int) -> VehicleView:
+        """註冊新的載具視圖"""
+        if sysid not in self._vehicles:
+            self._vehicles[sysid] = VehicleView(sysid)
+            logger.info(f"Registered new VehicleView for system {sysid}")
+        return self._vehicles[sysid]
+    
+    def get(self, sysid: int) -> Optional[VehicleView]:
+        """取得載具視圖"""
+        return self._vehicles.get(sysid)
+    
+    def unregister(self, sysid: int) -> bool:
+        """註銷載具視圖"""
+        if sysid in self._vehicles:
+            del self._vehicles[sysid]
+            logger.info(f"Unregistered VehicleView for system {sysid}")
+            return True
+        return False
+    
+    def get_all(self) -> Dict[int, VehicleView]:
+        """取得所有載具視圖"""
+        return self._vehicles.copy()
+    
+    def clear(self) -> None:
+        """清空所有載具視圖"""
+        self._vehicles.clear()
+        logger.info("Cleared all VehicleViews")
+    
+    def __len__(self) -> int:
+        return len(self._vehicles)
+    
+    def __contains__(self, sysid: int) -> bool:
+        return sysid in self._vehicles
+
+
+# ====================== Global Instance =====================
+
+# 全域註冊表實例
+vehicle_registry = VehicleViewRegistry()
+
+'''
+================= 改版記錄 ============================
+
+2026.01.16
+1. 新增 重置指定組件的封包統計 功能
+
+'''
+

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -0,0 +1,611 @@
+'''
+
+
+'''
+
+# 基礎功能的 import
+import asyncio
+import serial_asyncio
+
+import os
+import sys
+import serial
+import signal
+import time
+import threading
+import struct
+from enum import Enum, auto
+
+# # XBee 模組
+# from xbee.frame import APIFrame
+
+# 自定義的 import
+from .utils import setup_logger
+
+# ====================== 分割線 =====================
+
+logger = setup_logger(os.path.basename(__file__))
+
+# ====================== 分割線 =====================
+class XBeeFrameHandler:
+    """XBee API Frame 處理器"""
+    
+    @staticmethod
+    def parse_at_command_response(frame: bytes) -> dict:
+        """解析 AT Command Response (0x88)"""
+        if len(frame) < 8:
+            return None
+        
+        frame_type = frame[3]
+        if frame_type != 0x88:
+            return None
+        
+        frame_id = frame[4]
+        at_command = frame[5:7]
+        status = frame[7]
+        data = frame[8:] if len(frame) > 8 else b''
+        
+        return {
+            'frame_id': frame_id,
+            'command': at_command,
+            'status': status,
+            'data': data,
+            'is_ok': status == 0x00
+        }
+    
+    @staticmethod
+    def parse_receive_packet(frame: bytes) -> dict:
+        # """解析 RX Packet (0x90) - 未來擴展用"""
+        # if len(frame) < 15 or frame[3] != 0x90:
+        #     return None
+        
+        # return {
+        #     'source_addr': frame[4:12],
+        #     'reserved': frame[12:14],
+        #     'options': frame[14],
+        #     'data': frame[15:-1]
+        # }
+        pass
+        return None
+
+    @staticmethod
+    def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes:
+        """
+        將數據封裝為 XBee API 傳輸幀
+        
+        使用 XBee API 格式封裝數據:
+        - 傳輸請求幀 (0x10)
+        - 使用廣播地址
+        - 添加適當的頭部和校驗和
+        """
+        frame_type = 0x10
+        dest_addr16 = b'\xFF\xFE'
+        broadcast_radius = 0x00
+        options = 0x00
+
+        frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+        frame += dest_addr64 + dest_addr16
+        frame += struct.pack(">BB", broadcast_radius, options) + data
+        checksum = 0xFF - (sum(frame) & 0xFF)
+        return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+    @staticmethod
+    def decapsulate_data(data: bytes):
+        # 這裡可以根據需要進行數據解封裝
+
+        # XBee API 幀格式: 
+        # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節)
+        
+        # 檢查幀起始符 (0x7E)
+        if not data or len(data) < 5 or data[0] != 0x7E:
+            return data
+            
+        # 獲取數據長度 (不包括校驗和)
+        # length = (data[1] << 8) + data[2]
+        length = (data[1] << 8) | data[2]
+        
+        # 檢查幀完整性
+        if len(data) < length + 4:  # 起始符 + 長度(2字節) + 數據 + 校驗和
+            return data
+            
+        # 提取API標識符和數據
+        frame_type = data[3]
+        # frame_data = data[4:4+length-1]  # 減1是因為API標識符已經算在長度中
+        
+        # 根據不同的幀類型進行處理
+        if frame_type == 0x90:  # 例如，這是"接收數據包"類型
+            rf_data_start = 3 + 12
+            return data[rf_data_start:3 + length]
+        else:
+            return None          
+        return data
+
+
+class ATCommandHandler:
+    """AT 指令回應處理器"""
+    
+    def __init__(self, serial_port: str):
+        self.serial_port = serial_port
+        self.handlers = {
+            b'DB': self._handle_rssi,
+            b'SH': self._handle_serial_high,
+            b'SL': self._handle_serial_low,
+            # 可擴展其他 AT 指令
+        }
+    
+    def handle_response(self, response: dict):
+        """根據 AT 指令類型分派處理"""
+        if not response or not response['is_ok']:
+            if response:
+                print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+            return
+        
+        command = response['command']
+        handler = self.handlers.get(command)
+        
+        if handler:
+            handler(response['data'])
+        else:
+            print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
+    
+    def _handle_rssi(self, data: bytes):
+        """處理 DB (RSSI) 回應"""
+        if not data:
+            return
+        
+        rssi_value = data[0]
+        now = time.time()
+        
+        # 檢查是否最近有收到 MAVLink
+        last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0)
+        if now - last_mavlink_time > 0.5:
+            print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+            return
+        
+        # 取得對應的 sysid
+        sysid = serial_to_sysid.get(self.serial_port)
+        if sysid is None:
+            print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+            return
+        
+        # 記錄 RSSI
+        rssi_history[sysid].append(-rssi_value)
+        time_history[sysid].append(now)
+        # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
+    
+    def _handle_serial_high(self, data: bytes):
+        # """處理 SH (Serial Number High) - 範例"""
+        # if len(data) >= 4:
+        #     serial_high = int.from_bytes(data[:4], 'big')
+        #     print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}")
+        pass
+    
+    def _handle_serial_low(self, data: bytes):
+        # """處理 SL (Serial Number Low) - 範例"""
+        # if len(data) >= 4:
+        #     serial_low = int.from_bytes(data[:4], 'big')
+        #     print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}")
+        pass
+
+# ====================== 分割線 =====================
+
+class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發
+    def __init__(self, udp_handler, serial_port_str):
+        self.udp_handler = udp_handler # UDP 的傳輸物件
+        self.serial_port_str = serial_port_str
+        self.at_handler = ATCommandHandler(serial_port_str)
+
+        self.buffer = bytearray()  # 用於緩存接收到的資料
+        self.transport = None # Serial 自己的傳輸物件
+        # self.first_data = True  # 標記是否為第一次收到資料
+        # self.has_processed = False # 測試模式用 處理數據旗標 # debug
+
+    def connection_made(self, transport):
+        self.transport = transport
+        if hasattr(self.udp_handler, 'set_serial_handler'):
+            self.udp_handler.set_serial_handler(self)
+        # logger.info(f"Serial port {self.serial_port_str} connected.") # debug
+
+    # Serial 收到資料的處理過程
+    def data_received(self, data):
+        # 1. 把收到的資料加入緩衝區
+        self.buffer.extend(data) 
+
+        # 2. 需要完整的 header 才能解析
+        while len(self.buffer) >= 3:
+            # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) 
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0) # 如果不是就丟掉
+                continue
+
+            # 4. 讀取 payload 長度
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+
+            # 5. 等待完整封包
+            if len(self.buffer) < full_length:
+                break
+
+            # 6. 提取完整 frame 並從緩衝區移除
+            an_frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+
+            # 7. 判斷 frame 類型
+            frame_type = an_frame[3]
+            
+            if frame_type == 0x88:
+                # 處理 AT Command 回應
+                # response = XBeeFrameHandler.parse_at_command_response(an_frame)
+                # self.at_handler.handle_response(response)
+                pass
+                
+            elif frame_type == 0x90:
+                # Receive Packet (RX) payload 先解碼 
+                processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame))
+                # 轉換失敗就捨棄了
+                if processed_data is None: 
+                    continue
+                # 再透過 UDP 送出
+                self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port))
+
+            elif frame_type == 0x8B:
+                pass
+
+            else:
+                # 其他類型的 frame 未來可擴展處理 現在忽略
+                logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}")
+
+            # # RSSI
+            # if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包
+            #                                              # frame[5:7] == b'DB' -> API 封包的DB參數
+            #     status = frame[7]                        # 
+            #     if status == 0x00 and len(frame) > 8:    # status == 0x00 -> 這個封包是有效封包
+            #         rssi_value = frame[8]
+            #         now = time.time()
+
+            #         # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
+            #         last_time = serial_last_mavlink_time.get(self.serial_port, 0)
+            #         if now - last_time <= 0.5:
+            #             sysid = serial_to_sysid.get(self.serial_port, None)
+            #             if sysid is not None:
+            #                 rssi_history[sysid].append(-rssi_value)
+            #                 time_history[sysid].append(now)
+            #                 # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
+            #             else:
+            #                 print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+            #         else:
+            #             print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+            #     else:
+            #         print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
+        
+
+class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發
+    
+    LOCAL_HOST_IP = '127.0.0.1'  # 只送給本地端IP
+
+    def __init__(self, target_port):
+        self.target_port = target_port # 目標 UDP 端口
+
+        self.serial_handler = None # Serial 的傳輸物件
+        self.transport = None # UDP 自己的傳輸物件
+        self.remote_addr = None  # 儲存動態獲取的遠程地址 # debug
+        # self.has_processed = False # 測試模式用 處理數據旗標 # debug
+
+    def connection_made(self, transport):
+        self.transport = transport
+        # logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug
+
+    def set_serial_handler(self, serial_handler):
+        self.serial_handler = serial_handler
+
+    # UDP 收到資料的處理過程
+    def datagram_received(self, data, addr):
+        # 儲存對方的地址（這樣就能向同一個來源回傳數據）
+        # self.remote_addr = addr # debug
+        # print(f"Received UDP data from {addr}, setting as remote address")
+        
+        processed_data = XBeeFrameHandler.encapsulate_data(data)
+
+        if self.serial_handler:
+            self.serial_handler.transport.write(processed_data)
+    
+#==================================================================
+
+class SerialReceiverType(Enum):
+    """連接類型"""
+    TELEMETRY = auto() 
+    XBEEAPI2AT = auto() 
+    OTHER = auto() 
+
+
+class serial_manager:
+
+    class serial_object:
+        def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
+            self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc
+            self.baudrate = baudrate
+            self.receiver_type = receiver_type
+            self.target_port = target_port # 指向的 UPD 端口
+
+            self.transport = None # TODO 這個變數可能沒有作用
+            self.protocol = None # TODO 這個變數可能沒有作用
+            self.udp_handler = None
+            self.serial_handler = None
+
+    def __init__(self):
+        self.thread = None
+        self.loop = None
+        self.running = False
+        self.serial_count = 0
+        self.serial_objects = {} # serial id num : serial_object
+
+    def __del__(self):
+        self.loop = None
+        self.thread = None
+
+    def start(self):
+
+        if self.running:
+            logger.warning("serial_manager already running")
+            return
+        
+        self.running = True
+
+        # 啟動獨立線程 命名為 SerialManager
+        self.thread = threading.Thread(
+            target=self._run_event_loop, 
+            name="SerialManager"
+        )
+        self.thread.daemon = False  # 不設為 daemon，確保正確關閉
+        self.thread.start()
+
+        # 等待 _run_event_loop 建立事件循環的物件 self.loop
+        start_timeout = 2.0
+        start_time = time.time()
+        while not self.loop and time.time() - start_time < start_timeout:
+            time.sleep(0.1)
+
+        # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop
+        if self.loop:
+            logger.info("serial_manager thread started <-")
+            return True
+        else:
+            logger.error("serial_manager failed to start")
+            return False
+
+    def shutdown(self):
+        """停止 serial_manager 和其管理的所有 serial_object"""
+        # 自己在 running 狀態下才執行停止程序
+        if not self.running:
+            logger.warning("serial_manager is not running")
+            return
+
+        # 停止所有被管理的 serial_object
+        for serial_id in list(self.serial_objects.keys()):
+            self.remove_serial_link(serial_id)
+
+        # 停止自己
+        self.running = False
+
+        # 解開事件循環的阻塞
+        if self.loop:
+            self.loop.call_soon_threadsafe(self.loop.stop)
+
+        # 等待線程結束
+        if self.thread and self.thread.is_alive():
+            self.thread.join(timeout=5.0)
+            if self.thread.is_alive():
+                logger.warning("serial_manager thread did not stop gracefully")
+        
+        logger.info("serial_manager thread END!")
+
+    def _run_event_loop(self):
+        """在獨立線程中運行 asyncio 事件循環"""
+        self.loop = asyncio.new_event_loop()
+        asyncio.set_event_loop(self.loop)
+
+        # # 為每個 serial_object 建立連接
+        # for serial_obj in self.serial_objects:
+        #     coro = serial_asyncio.create_serial_connection(
+        #         self.loop,
+        #         lambda: SerialProtocol(serial_obj.receiver_type),
+        #         serial_obj.serial_port,
+        #         baudrate=serial_obj.baudrate
+        #     )
+        #     transport, protocol = self.loop.run_until_complete(coro)
+        #     serial_obj.transport = transport
+        #     serial_obj.protocol = protocol
+
+        try:
+            self.loop.run_forever()
+        finally:
+            self.loop.close()
+
+    def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
+
+        if not self.running or not self.loop:
+            logger.error("Event loop not running, cannot create serial link")
+            return False
+
+        # 檢查 serial port 有效
+        if not self.check_serial_port(serial_port, baudrate):
+            logger.error(f"Serial port {serial_port} validation failed")
+            return False
+
+        # 使用 run_coroutine_threadsafe 執行協程並獲取結果
+        future = asyncio.run_coroutine_threadsafe(
+            self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type),
+            self.loop
+        )
+
+        try:
+            # 等待結果，設定合理的超時時間
+            result = future.result(timeout=5.0)
+            if result:
+                logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}")
+            return True
+        except asyncio.TimeoutError:
+            logger.error(f"Timeout creating serial link for {serial_port}")
+            return False
+        except Exception as e:
+            logger.error(f"Failed to create serial link for {serial_port}: {e}")
+            return False
+
+    async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
+        """在事件循環線程中執行實際的連接創建"""
+        try:
+            # 創建 serial_object 實例
+            serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type)
+
+            # 建立 UDP 處理器並指定目標端口位置
+            serial_obj.udp_handler = UDPHandler(target_port)
+            
+            # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配
+            udp_transport, udp_protocol = await self.loop.create_datagram_endpoint(
+                lambda: serial_obj.udp_handler,
+                local_addr=('0.0.0.0', 0)  # 使用端口 0 讓系統自動分配可用端口
+            )
+            serial_obj.transport = udp_transport
+            serial_obj.protocol = udp_protocol
+            
+            # logger.info(f"UDP endpoint created for {serial_port}") # debug
+
+            # 建立 Serial 處理器，將 UDP 處理器傳給它
+            serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port)
+
+            # 建立 Serial 連接
+            serial_transport, _ = await serial_asyncio.create_serial_connection(
+                self.loop, 
+                lambda: serial_obj.serial_handler, 
+                serial_port, 
+                baudrate=baudrate
+            )
+            
+            # logger.info(f"Serial connection created for {serial_port}") # debug
+
+            # 將 serial_object 加入管理列表
+            serial_id = self.serial_count + 1
+            self.serial_objects[serial_id] = serial_obj
+            self.serial_count += 1
+            
+            # logger.info(f"Serial object {serial_id} added to manager") # debug
+            return True
+            
+        except Exception as e:
+            logger.error(f"Exception in _async_create_serial_link for {serial_port}: {str(e)}")
+            # 清理已創建的資源
+            if 'serial_obj' in locals():
+                if hasattr(serial_obj, 'transport') and serial_obj.transport:
+                    serial_obj.transport.close()
+            return False
+
+    def remove_serial_link(self, serial_id):
+        """移除串口連接（線程安全方式）"""
+        # 確保事件循環正在運行
+        if not self.loop:
+            logger.error("Event loop not running")
+            return False
+        
+        # 檢查 serial_id 是否存在
+        if serial_id not in self.serial_objects:
+            logger.warning(f"Serial object {serial_id} not found")
+            return False
+
+        # 使用 run_coroutine_threadsafe 執行協程
+        future = asyncio.run_coroutine_threadsafe(
+            self._async_remove_serial_link(serial_id),
+            self.loop
+        )
+        
+        try:
+            result = future.result(timeout=3.0)
+            if result:
+                logger.info(f"Remove Serial Link {serial_id}")
+            return result
+        except asyncio.TimeoutError:
+            logger.error(f"Timeout removing serial link {serial_id}")
+            return False
+        except Exception as e:
+            logger.error(f"Failed to remove serial link {serial_id}: {e}")
+            return False
+
+    async def _async_remove_serial_link(self, serial_id):
+        """在事件循環線程中執行實際的連接移除"""
+        if serial_id not in self.serial_objects:
+            logger.warning(f"Serial object {serial_id} not in managed list")
+            return False
+        
+        try:
+            serial_obj = self.serial_objects[serial_id]
+            
+            # 關閉 UDP transport
+            if hasattr(serial_obj, 'transport') and serial_obj.transport:
+                serial_obj.transport.close()
+            
+            # 關閉 Serial transport
+            if hasattr(serial_obj, 'serial_handler') and serial_obj.serial_handler:
+                if hasattr(serial_obj.serial_handler, 'transport') and serial_obj.serial_handler.transport:
+                    serial_obj.serial_handler.transport.close()
+            
+            # 從管理列表中移除
+            del self.serial_objects[serial_id]
+            # logger.info(f"Serial object {serial_id} removed from manager") # debug
+            return True
+            
+        except Exception as e:
+            logger.error(f"Exception in _async_remove_serial_link for {serial_id}: {str(e)}")
+            return False
+
+    def get_serial_link(self):
+        ret = {} # serial id num : serial_port string
+        for key, obj in self.serial_objects.items():
+            ret[key] = obj.serial_port
+        return ret
+
+    @staticmethod
+    def check_serial_port(serial_port, baudrate):
+        """檢查串口是否存在與可用"""
+        # 檢查設備是否存在
+        if not os.path.exists(serial_port):
+            logger.error(f"Serial Device {serial_port} Not Found")
+            return False
+            
+        # 檢查是否有權限訪問設備
+        try:
+            if not os.access(serial_port, os.R_OK | os.W_OK):
+                logger.error(f"No permission to access {serial_port}")
+                return False
+        except Exception as e:
+            logger.error(f"Cannot Access Serial Device {serial_port}: {str(e)}")
+            return False
+        
+        # 檢查是否被占用
+        try:
+            # 嘗試打開串口
+            ser = serial.Serial(serial_port, baudrate)
+            ser.close()  # 打開成功後立即關閉
+            return True
+        except serial.SerialException as e:
+            logger.error(f"Serial Device {serial_port} is Occupied or Inaccessible: {str(e)}")
+            return False
+        except Exception as e:
+            logger.error(f"Unknown Error: {str(e)}")
+            return False
+
+
+if __name__ == '__main__':
+    sm = serial_manager()
+    sm.start()
+
+    SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
+    SERIAL_BAUDRATE = 115200
+    UDP_REMOTE_PORT = 14571
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT)
+
+    linked_serial = sm.get_serial_link()
+    print(linked_serial)
+    time.sleep(10)
+
+    sm.remove_serial_link(1)
+    time.sleep(3)
+    sm.shutdown()

src/fc_network_adapter/fc_network_adapter/socketManager.py

@@ -1,14 +0,0 @@
-
-'''
-
-透過某個地方 得到 udp 或 uart 接口
-對於每個接口 視為一個獨立的物件 
-
-物件對於不同的接口 是為不同類型的物件
-
-每個類型的物件 創建一個獨立的執行緒 來處理資料
-關於執行緒的實作 是寫在另一個模組
-
-物件之間 也可以做資料轉換/轉拋
-
-'''

src/fc_network_adapter/fc_network_adapter/utils/__init__.py

@@ -0,0 +1,7 @@
+"""
+共用工具模組
+"""
+from .ringBuffer import RingBuffer
+from .theLogger import setup_logger
+
+__all__ = ['RingBuffer', 'setup_logger']

src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py

@@ -0,0 +1,129 @@
+import socket
+import random
+import os
+
+
+def get_used_ports():
+    """
+    從 /proc/net/tcp 和 /proc/net/udp 讀取系統已占用的 port
+    直接讀取 Linux 系統資訊，避免暴力嘗試
+    
+    Returns:
+        set: 已被占用的 port 號集合
+    """
+    used_ports = set()
+    
+    # 讀取 TCP 占用的 port (包含 IPv4 和 IPv6)
+    for filepath in ['/proc/net/tcp', '/proc/net/tcp6']:
+        if os.path.exists(filepath):
+            try:
+                with open(filepath, 'r') as f:
+                    lines = f.readlines()[1:]  # 跳過標題行
+                    for line in lines:
+                        parts = line.split()
+                        if len(parts) > 1:
+                            # local_address 格式: "0100007F:1F90" (hex)
+                            local_addr = parts[1]
+                            port_hex = local_addr.split(':')[1]
+                            port = int(port_hex, 16)
+                            used_ports.add(port)
+            except (IOError, PermissionError):
+                pass
+    
+    # 讀取 UDP 占用的 port (包含 IPv4 和 IPv6)
+    for filepath in ['/proc/net/udp', '/proc/net/udp6']:
+        if os.path.exists(filepath):
+            try:
+                with open(filepath, 'r') as f:
+                    lines = f.readlines()[1:]  # 跳過標題行
+                    for line in lines:
+                        parts = line.split()
+                        if len(parts) > 1:
+                            local_addr = parts[1]
+                            port_hex = local_addr.split(':')[1]
+                            port = int(port_hex, 16)
+                            used_ports.add(port)
+            except (IOError, PermissionError):
+                pass
+    
+    return used_ports
+
+
+def is_port_available(port):
+    """
+    測試指定 port 是否可用 (TCP 和 UDP 都測試)
+    
+    Args:
+        port (int): 要測試的 port 號
+    
+    Returns:
+        bool: True 表示可用，False 表示被占用
+    """
+    # 測試 TCP
+    try:
+        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
+            sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            sock.bind(('', port))
+    except OSError:
+        return False
+    
+    # 測試 UDP
+    try:
+        with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
+            sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            sock.bind(('', port))
+    except OSError:
+        return False
+    
+    return True
+
+
+def find_available_port(start_port=1024, end_port=65535):
+    """
+    在指定的 port 區間內隨機找出一個未被占用的 port
+    使用 Linux /proc/net 系統資訊來過濾已占用的 port，避免暴力嘗試
+    確保 TCP 和 UDP 都可用
+    
+    Args:
+        start_port (int): 起始 port 號 (預設 1024)
+        end_port (int): 結束 port 號 (預設 65535)
+    
+    Returns:
+        int: 可用的 port 號，如果找不到則返回 None
+    """
+    if start_port < 1 or end_port > 65535 or start_port >= end_port:
+        raise ValueError("Port 範圍必須在 1-65535 之間，且起始 port 必須小於結束 port")
+    
+    # 從系統讀取已占用的 port
+    used_ports = get_used_ports()
+    
+    # 計算可用的 port 列表 (排除已占用的)
+    available_ports = [p for p in range(start_port, end_port + 1) if p not in used_ports]
+    
+    if not available_ports:
+        return None
+    
+    # 隨機打亂順序
+    random.shuffle(available_ports)
+    
+    # 從可用列表中挑選，再用 socket 雙重確認 TCP 和 UDP 都可用
+    for port in available_ports:
+        if is_port_available(port):
+            return port
+    
+    # 如果都不可用
+    return None
+
+
+if __name__ == "__main__":
+    # 使用範例
+    port = find_available_port(8000, 9000)
+    if port:
+        print(f"找到可用的 port: {port}")
+    else:
+        print("找不到可用的 port")
+    
+    # 自訂範圍範例
+    port = find_available_port(10000, 20000)
+    if port:
+        print(f"在 10000-20000 範圍找到可用的 port: {port}")

src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py

@@ -0,0 +1,111 @@
+"""
+Serial Port Discovery Utility
+
+This module provides functions to discover available serial ports on the system.
+It uses glob pattern matching to find serial device files in /dev/.
+"""
+
+import glob
+from typing import List, Union
+
+
+def get_serial_ports() -> List[str]:
+    """
+    獲取系統中所有可用的串口設備列表
+    
+    在 Linux 系統中，會搜尋以下模式的設備：
+    - /dev/ttyUSB*  (USB 串口設備)
+    - /dev/ttyACM*  (USB CDC ACM 設備)
+    - /dev/ttyS*    (標準串口)
+    
+    Returns:
+        List[str]: 包含所有找到的串口設備路徑的列表
+        
+    Example:
+        >>> ports = get_serial_ports()
+        >>> print(ports)
+        ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyS0']
+    """
+    serial_ports = []
+    
+    # 搜尋不同類型的串口設備
+    patterns = [
+        '/dev/ttyUSB*',  # USB 串口轉換器
+        '/dev/ttyACM*',  # USB CDC ACM 設備（如 Arduino）
+        '/dev/ttyS*',    # 標準串口
+    ]
+    
+    for pattern in patterns:
+        serial_ports.extend(glob.glob(pattern))
+    
+    # 排序以便於閱讀
+    serial_ports.sort()
+    
+    return serial_ports
+
+
+def get_serial_ports_with_filter(filter_patterns: Union[str, List[str]] = None) -> List[str]:
+    """
+    獲取串口設備列表，可選擇性地使用自訂篩選模式
+    
+    Args:
+        filter_patterns (Union[str, List[str]], optional): 
+            單一或多個 glob 模式
+            - 字串: '/dev/ttyUSB*'
+            - 列表: ['/dev/ttyUSB*', '/dev/ttyACM*']
+            如果為 None，則使用預設模式搜尋所有串口
+    
+    Returns:
+        List[str]: 符合條件的串口設備路徑列表
+        
+    Example:
+        >>> # 單一 pattern
+        >>> ports = get_serial_ports_with_filter('/dev/ttyUSB*')
+        >>> print(ports)
+        ['/dev/ttyUSB0', '/dev/ttyUSB1']
+        
+        >>> # 多個 patterns
+        >>> ports = get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
+        >>> print(ports)
+        ['/dev/ttyACM0', '/dev/ttyUSB0', '/dev/ttyUSB1']
+    """
+    if filter_patterns is None:
+        return get_serial_ports()
+    
+    # 統一轉成 list 處理
+    if isinstance(filter_patterns, str):
+        filter_patterns = [filter_patterns]
+    
+    serial_ports = []
+    for pattern in filter_patterns:
+        serial_ports.extend(glob.glob(pattern))
+    
+    serial_ports.sort()
+    return serial_ports
+
+
+if __name__ == "__main__":
+    # 使用範例
+    print("=== Serial Port Discovery ===\n")
+    
+    # 1. 獲取所有串口設備
+    all_ports = get_serial_ports()
+    print(f"找到 {len(all_ports)} 個串口設備:")
+    for port in all_ports:
+        print(f"  - {port}")
+    
+    print("\n" + "="*30 + "\n")
+    
+    # 2. 只搜尋 USB 串口
+    usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*')
+    print(f"找到 {len(usb_ports)} 個 USB 串口設備:")
+    for port in usb_ports:
+        print(f"  - {port}")
+    
+    print("\n" + "="*30 + "\n")
+    
+    # 3. 只搜尋 ACM 設備
+    acm_ports = get_serial_ports_with_filter('/dev/ttyACM*')
+    print(f"找到 {len(acm_ports)} 個 ACM 設備:")
+    for port in acm_ports:
+        print(f"  - {port}")

src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py

@@ -0,0 +1,231 @@
+# import array
+import threading
+import ctypes
+from typing import Any, Optional, Tuple
+
+class RingBuffer:
+    """
+    高效能無鎖環形緩衝區，使用原子操作避免鎖定
+    用於在不同執行緒間高效傳遞資料
+    """
+    # 緩衝區計數器，用於自動分配 buffer_id
+    _buffer_counter = 0
+    _counter_lock = threading.Lock()
+    
+    def __init__(self, capacity: int = 256, buffer_id: int = None):
+        """
+        初始化環形緩衝區
+        
+        Args:
+            capacity: 緩衝區容量 (必須是 2 的次方)
+            buffer_id: 緩衝區 ID，如果為 None 則自動分配
+        """
+        # 確保容量是 2 的次方，便於使用位運算取模
+        if capacity & (capacity - 1) != 0:
+            # 找到大於等於 capacity 的最小 2 的次方
+            capacity = 1 << (capacity - 1).bit_length()
+        
+        # 分配緩衝區 ID
+        if buffer_id is None:
+            with RingBuffer._counter_lock:
+                buffer_id = RingBuffer._buffer_counter
+                RingBuffer._buffer_counter += 1
+                
+        self.buffer_id = buffer_id
+        self.capacity = capacity
+        self.mask = capacity - 1  # 用於快速取模
+        self.buffer = [None] * capacity
+        
+        # 原子計數器作為讀寫指標
+        self.write_index = ctypes.c_uint64(0)
+        self.read_index = ctypes.c_uint64(0)
+        
+        # 用於檢測上次操作的執行緒 ID
+        self._last_read_thread = None
+        self._last_write_thread = None
+        
+        # 添加同時讀寫統計
+        self.concurrent_write_count = ctypes.c_uint64(0)  # 同時寫入計數
+        self.concurrent_read_count = ctypes.c_uint64(0)   # 同時讀取計數
+        self.total_write_count = ctypes.c_uint64(0)       # 總寫入操作計數
+        self.total_read_count = ctypes.c_uint64(0)        # 總讀取操作計數
+        self.overflow_count = ctypes.c_uint64(0)          # 緩衝區溢出次數
+        
+        # 記錄各執行緒的操作次數
+        self.thread_write_counts = {}  # {thread_id: count}
+        self.thread_read_counts = {}   # {thread_id: count}
+        
+        # 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作）
+        self._stats_lock = threading.Lock()
+        
+    def put(self, item: Any) -> bool:
+        """
+        將項目存入緩衝區
+        
+        Args:
+            item: 要存入的項目
+        
+        Returns:
+            bool: 成功寫入返回 True，緩衝區已滿返回 False
+        """
+        # 更新寫入統計
+        self.total_write_count.value += 1
+        
+        # 檢測上次寫入的執行緒
+        current_thread = threading.get_ident()
+        
+        # 記錄當前執行緒寫入次數
+        with self._stats_lock:
+            self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1
+        
+        # 檢測是否為不同執行緒同時寫入
+        if self._last_write_thread is not None and current_thread != self._last_write_thread:
+            self.concurrent_write_count.value += 1
+        
+        self._last_write_thread = current_thread
+        
+        # 原子獲取當前寫入位置
+        current = self.write_index.value
+        next_pos = (current + 1) & self.mask
+        
+        # 檢查緩衝區是否已滿
+        if next_pos == self.read_index.value:
+            self.overflow_count.value += 1
+            return False
+        
+        # 寫入資料並原子更新寫入位置
+        self.buffer[current] = item
+        self.write_index.value = next_pos
+        return True
+    
+    def get(self) -> Optional[Any]:
+        """
+        從緩衝區讀取項目
+        
+        Returns:
+            項目或 None（如果緩衝區為空）
+        """
+        # 更新讀取統計
+        self.total_read_count.value += 1
+        
+        # 檢測上次讀取的執行緒
+        current_thread = threading.get_ident()
+        
+        # 記錄當前執行緒讀取次數
+        with self._stats_lock:
+            self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1
+        
+        # 檢測是否為不同執行緒同時讀取
+        if self._last_read_thread is not None and current_thread != self._last_read_thread:
+            self.concurrent_read_count.value += 1
+        
+        self._last_read_thread = current_thread
+        
+        # 檢查緩衝區是否為空
+        if self.read_index.value == self.write_index.value:
+            return None
+        
+        # 原子獲取當前讀取位置並讀取資料
+        current = self.read_index.value
+        item = self.buffer[current]
+        
+        # 原子更新讀取位置
+        self.read_index.value = (current + 1) & self.mask
+        return item
+    
+    def get_all(self) -> list:
+        """
+        獲取緩衝區中的所有項目
+        
+        Returns:
+            list: 所有可用項目的列表
+        """
+        items = []
+        while True:
+            item = self.get()
+            if item is None:
+                break
+            items.append(item)
+        return items
+    
+    def size(self) -> int:
+        # 返回目前緩衝區內項目數量
+        return (self.write_index.value - self.read_index.value) & self.mask
+    
+    def is_empty(self) -> bool:
+        # 檢查緩衝區是否為空
+        return self.read_index.value == self.write_index.value
+    
+    def is_full(self) -> bool:
+        # 檢查緩衝區是否已滿
+        return ((self.write_index.value + 1) & self.mask) == self.read_index.value
+    
+    def clear(self) -> None:
+        """清空緩衝區"""
+        self.read_index.value = self.write_index.value
+        
+    def get_stats(self) -> dict:
+        """
+        獲取緩衝區統計資訊
+        
+        Returns:
+            dict: 包含各種統計數據的字典
+        """
+        with self._stats_lock:
+            stats = {
+                "buffer_id": self.buffer_id,
+                "capacity": self.capacity,
+                "current_size": self.size(),
+                "is_full": self.is_full(),
+                "is_empty": self.is_empty(),
+                "total_writes": self.total_write_count.value,
+                "total_reads": self.total_read_count.value,
+                "concurrent_writes": self.concurrent_write_count.value,
+                "concurrent_reads": self.concurrent_read_count.value,
+                "overflow_count": self.overflow_count.value,
+                "write_threads": len(self.thread_write_counts),
+                "read_threads": len(self.thread_read_counts),
+                "concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value),
+                "concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value),
+                "top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3],
+                "top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3],
+            }
+            return stats
+    
+    def print_stats(self) -> None:
+        """輸出當前緩衝區統計資訊"""
+        stats = self.get_stats()
+        
+        print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===")
+        print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}")
+        print(f"Total Write Operations: {stats['total_writes']}")
+        print(f"Total Read Operations: {stats['total_reads']}")
+        print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})")
+        print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})")
+        print(f"Buffer Overflow Count: {stats['overflow_count']}")
+        print(f"Writing Threads: {stats['write_threads']}")
+        print(f"Reading Threads: {stats['read_threads']}")
+        
+        print("Top Writer Threads:")
+        for thread_id, count in stats['top_writer_threads']:
+            print(f"  Thread {thread_id}: {count} writes")
+            
+        print("Top Reader Threads:")
+        for thread_id, count in stats['top_reader_threads']:
+            print(f"  Thread {thread_id}: {count} reads")
+        print("=============================\n")
+    
+    def reset_stats(self) -> None:
+        """重置所有統計數據"""
+        with self._stats_lock:
+            self.concurrent_write_count.value = 0
+            self.concurrent_read_count.value = 0
+            self.total_write_count.value = 0
+            self.total_read_count.value = 0
+            self.overflow_count.value = 0
+            self.thread_write_counts.clear()
+            self.thread_read_counts.clear()
+            
+    def __str__(self) -> str:
+        """返回緩衝區的字符串表示"""
+        return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})"

src/fc_network_adapter/fc_network_adapter/utils/theLogger.py

@@ -0,0 +1,43 @@
+import logging
+import os
+from logging.handlers import TimedRotatingFileHandler
+
+# 全域 Logger 實例
+_global_logger = None
+
+def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger:
+    global _global_logger
+
+    if _global_logger is None:
+        # 確保 logs 資料夾存在
+        os.makedirs(log_dir, exist_ok=True)
+
+        # 建立全域 Logger
+        _global_logger = logging.getLogger("global_logger")
+        _global_logger.setLevel(level)
+        _global_logger.propagate = False  # 防止重複輸出
+
+        formatter = logging.Formatter(
+            fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s",
+            datefmt="%m-%d %H:%M:%S"
+        )
+
+        # 檔案輸出（每天輪替）
+        file_handler = TimedRotatingFileHandler(
+            filename=os.path.join(log_dir, "application.log"),
+            when="midnight",  # 每天 0 點輪替
+            backupCount=7,    # 保留 7 天
+            encoding="utf-8"
+        )
+        file_handler.setFormatter(formatter)
+        _global_logger.addHandler(file_handler)
+
+        # 終端機輸出
+        console_handler = logging.StreamHandler()
+        console_handler.setFormatter(formatter)
+        _global_logger.addHandler(console_handler)
+
+    # 為每個模組建立子 Logger，並設定名稱
+    module_logger = _global_logger.getChild(name)
+    module_logger.name = name  # 修改子 Logger 的名稱，僅保留子 Logger 名稱
+    return module_logger

src/fc_network_adapter/setup.py

@@ -20,6 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main',
         ],
     },
 )

src/fc_network_adapter/tests/demo_integration.py

@@ -0,0 +1,277 @@
+import os
+import sys
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+# 基礎功能的 import
+import queue
+import time
+
+# ROS2 的 import
+import rclpy
+
+# mavlink 的 import
+from pymavlink import mavutil
+
+# 自定義的 import
+from ..fc_network_adapter import mavlinkObject as mo
+from ..fc_network_adapter import mavlinkVehicleView as mvv
+# from ..fc_network_adapter import mavlinkDevice as md
+
+# ====================== 分割線 =====================
+
+test_item = 1
+running_time = 3
+
+
+print('test_item : ', test_item)
+
+'''
+測試項 個位數 表示分離的功能
+
+測試項 1X 表示 mavlink_object 的功能 測試連線的能力
+'''
+
+if test_item == 1:
+    print('===> Start of Program .Test ', test_item)
+
+    connection_string="udp:127.0.0.1:14591"
+    mavlink_socket1 = mavutil.mavlink_connection(connection_string)
+    # mavlink_object1 = mo.mavlink_object(mavlink_socket1)
+
+    time.sleep(1)
+
+    print("mark A")
+
+    # print("Socket IP:", mavlink_socket1.target_system)
+    print("Socket port:", mavlink_socket1.port.getsockname())
+
+    # print("=== ", dir(mavlink_socket1.port))
+
+
+elif test_item == 10:
+    # 需要開啟一個 ardupilot 的模擬器
+    # 測試 mavlink_object 放入 ring buffer 的應用
+    print('===> Start of Program .Test ', test_item)
+
+    # 清空 ring buffer
+    mo.stream_bridge_ring.clear()
+    mo.return_packet_ring.clear()
+
+    manager = mo.async_io_manager()
+    manager.start()
+    time.sleep(0.5)  # 等待事件循環啟動
+
+    # 初始化輸入通道
+    connection_string="udp:127.0..1:14571"
+    mavlink_socket1 = mavutil.mavlink_connection(connection_string)
+    mavlink_object1 = mo.mavlink_object(mavlink_socket1)
+
+    sock = mavlink_socket1.port
+    print("Socket port:", sock)
+
+    manager.add_mavlink_object(mavlink_object1)
+
+    start_time = time.time()
+    while (time.time() - start_time) < running_time:
+        items_a = mo.stream_bridge_ring.get_all()
+        items_b = mo.return_packet_ring.get_all()
+        try:
+            print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}")
+        except IndexError:
+            print("stream_bridge_ring is empty")
+
+        try:
+            print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}")
+        except IndexError:
+            print("return_packet_ring is empty")
+        time.sleep(1)
+
+    manager.shutdown()
+
+    print('<=== End of Program')
+
+elif test_item == 11:
+    # 需要開啟一個 ardupilot 的模擬器
+    # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
+    print('===> Start of Program .Test ', test_item)
+
+    # 清空 ring buffer
+    mo.stream_bridge_ring.clear()
+    mo.return_packet_ring.clear()
+
+    manager = mo.async_io_manager()
+    manager.start()
+    time.sleep(0.5)  # 等待事件循環啟動
+
+    # 初始化輸入通道
+    connection_string="udp:127.0.0.1:14571"
+    mavlink_socket1 = mavutil.mavlink_connection(connection_string)
+    mavlink_object1 = mo.mavlink_object(mavlink_socket1)
+    manager.add_mavlink_object(mavlink_object1)
+
+    # 啟動 mavlink_bridge
+    bridge = mo.mavlink_bridge()
+    bridge.start()
+
+    time.sleep(3)
+
+    # 印出目前所有 mavlink_systems 的內容
+    print('目前所有的系統 : ')
+    all_vehicles = mvv.vehicle_registry.get_all()
+    for sysid, vehicle in all_vehicles.items():
+        print(f"  System {sysid}: {vehicle}")
+
+    start_time = time.time()
+    show_time = time.time()
+    while time.time() - start_time < running_time:
+        if (time.time() - show_time) >= 2:
+            # print("mark B")
+
+            show_time = time.time()
+            for sysid, vehicle in all_vehicles.items():
+                for compid in vehicle.components:
+                    comp = vehicle.get_component(compid)
+                    print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count))
+                comp.reset_packet_stats()
+            print("===================")
+
+    manager.shutdown()
+    bridge.stop()
+
+    print('<=== End of Program')
+
+elif test_item == 12:
+    # 需要開啟一個 ardupilot 的模擬器 與 GCS 
+    # 這邊是測試 mavlink object 作為交換器功能的代碼
+    print('===> Start of Program .Test ', test_item)
+
+    # 清空 ring buffer
+    mo.stream_bridge_ring.clear()
+    mo.return_packet_ring.clear()
+
+    manager = mo.async_io_manager()
+    manager.start()
+    time.sleep(0.5)  # 等待事件循環啟動
+
+    # 初始化輸入通道
+    connection_string="udp:127.0.0.1:14571"
+    mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
+    mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
+
+    connection_string="udp:127.0.0.1:14571"
+    mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
+    mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
+
+    # 初始化輸出通道
+    connection_string="udpout:127.0.0.1:14551"
+    mavlink_socket_out = mavutil.mavlink_connection(connection_string)
+    mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
+
+    manager.add_mavlink_object(mavlink_object_out)
+    manager.add_mavlink_object(mavlink_object_in1)
+    manager.add_mavlink_object(mavlink_object_in2)
+
+    time.sleep(1)  # 等待通道啟動
+
+    mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id)  
+    mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id)
+
+    mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id)  
+    mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id)
+
+    start_time = time.time()
+    while (time.time() - start_time) < running_time:
+
+        time.sleep(1)
+
+    manager.shutdown()
+
+    print('<=== End of Program')
+
+
+
+elif test_item == 21:
+    # 需要開啟一個 ardupilot 的模擬器
+    # 這邊是測試代碼 引入 rclpy 來測試 node 的運行
+    
+    print('===> Start of Program .Test ', test_item)
+    # 初始化 rclpy 才能使用 node
+    rclpy.init() 
+
+    # 清空 ring buffer
+    mo.stream_bridge_ring.clear()
+    mo.return_packet_ring.clear()
+
+    manager = mo.async_io_manager()
+    manager.start()
+
+    # 啟動 mavlink_bridge
+    analyzer = mo.mavlink_bridge()
+    # 關於 Node 的初始化
+    show_time = time.time()
+    analyzer._init_node() # 初始化 node
+    print('初始化 node 完成 耗時 : ',time.time() - show_time)
+
+    time.sleep(0.5)  # 系統 Setup 完成
+
+    
+    # 創建通道
+    connection_string="udp:127.0.0.1:14560"
+    mavlink_socket = mavutil.mavlink_connection(connection_string)
+    mavlink_object3 = mo.mavlink_object(mavlink_socket)
+    manager.add_mavlink_object(mavlink_object3)
+
+    print('=== waiting for mavlink data ...')
+    time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
+    
+    print('目前所有的系統 : ')
+    for sysid in analyzer.mavlink_systems:
+        print(analyzer.mavlink_systems[sysid])
+
+    compid = 1   
+    sysid = 1
+    start_time = time.time()
+    analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
+    end_time = time.time()
+    print(f"Execution time for create_flightMode: {end_time - start_time} seconds")
+
+    print("start emit info")
+
+    start_time = time.time()
+    show_time = time.time() 
+    while time.time() - start_time < running_time:
+        try:
+            # print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode'])
+            analyzer.emit_info() # 這邊是測試 node 的運行
+            time.sleep(1)
+        except KeyboardInterrupt:
+            break
+    
+
+    # 程式結束
+    analyzer.destroy_node()
+    rclpy.shutdown()      
+
+    # 結束程式 退出所有 thread
+    manager.stop()
+    analyzer.stop()
+    analyzer.thread.join()
+
+    mavlink_socket.close()
+    print('<=== End of Program')
+
+
+elif test_item == 52:
+    print('===> Start of Program .Test ', test_item)
+
+    manager = mo.async_io_manager()
+    manager.start()
+
+    # print(manager.thread.is_alive())
+
+    manager.shutdown()
+
+    time.sleep(1)
+    
+    print('manager stopped')
+

src/fc_network_adapter/tests/demo_mavlinkVehicleView.py

@@ -0,0 +1,331 @@
+"""
+VehicleView 使用範例
+展示如何使用純狀態容器來管理 MAVLink 載具資訊
+"""
+
+import time
+from ..fc_network_adapter.mavlinkVehicleView import (
+    VehicleView,
+    VehicleComponent,
+    RFModule,
+    vehicle_registry,
+    ConnectionType,
+    ComponentType,
+    RFModuleType
+)
+
+
+def example_basic_usage():
+    """基本使用範例"""
+    print("=== 基本使用範例 ===\n")
+    
+    # 1. 建立載具視圖
+    vehicle = VehicleView(sysid=1)
+    vehicle.kind = "Copter"
+    vehicle.vehicle_type = 2  # MAV_TYPE_QUADROTOR
+    vehicle.connected_via = ConnectionType.UDP
+    
+    print(f"建立載具: {vehicle}\n")
+    
+    # 2. 新增 autopilot 組件
+    autopilot = vehicle.add_component(
+        component_id=1,
+        comp_type=ComponentType.AUTOPILOT
+    )
+    autopilot.mav_type = 2  # MAV_TYPE_QUADROTOR
+    autopilot.mav_autopilot = 3  # MAV_AUTOPILOT_ARDUPILOTMEGA
+    
+    print(f"新增組件: {autopilot}\n")
+    
+    # 3. 手動餵入位置資訊
+    autopilot.status.position.latitude = 25.0330
+    autopilot.status.position.longitude = 121.5654
+    autopilot.status.position.altitude = 100.5
+    autopilot.status.position.timestamp = time.time()
+    
+    print(f"位置: 緯度={autopilot.status.position.latitude}, "
+          f"經度={autopilot.status.position.longitude}, "
+          f"高度={autopilot.status.position.altitude}m\n")
+    
+    # 4. 手動餵入姿態資訊
+    autopilot.status.attitude.roll = 0.05  # 弧度
+    autopilot.status.attitude.pitch = -0.02
+    autopilot.status.attitude.yaw = 1.57
+    autopilot.status.attitude.timestamp = time.time()
+    
+    print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, "
+          f"Pitch={autopilot.status.attitude.pitch:.3f}, "
+          f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n")
+    
+    # 5. 手動餵入飛行模式
+    autopilot.status.mode.base_mode = 89
+    autopilot.status.mode.custom_mode = 4
+    autopilot.status.mode.mode_name = "GUIDED"
+    autopilot.status.mode.timestamp = time.time()
+    
+    print(f"飛行模式: {autopilot.status.mode.mode_name}\n")
+    
+    # 6. 手動餵入電池資訊
+    autopilot.status.battery.voltage = 12.6
+    autopilot.status.battery.current = 15.2
+    autopilot.status.battery.remaining = 75
+    autopilot.status.battery.timestamp = time.time()
+    
+    print(f"電池: 電壓={autopilot.status.battery.voltage}V, "
+          f"電流={autopilot.status.battery.current}A, "
+          f"剩餘={autopilot.status.battery.remaining}%\n")
+
+
+def example_packet_tracking():
+    """封包追蹤範例"""
+    print("\n=== 封包追蹤範例 ===\n")
+    
+    vehicle = VehicleView(sysid=2)
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    
+    # 模擬接收封包
+    timestamp = time.time()
+    
+    # 接收 HEARTBEAT (msg_type=0)
+    autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp)
+    
+    # 接收 ATTITUDE (msg_type=30)
+    autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1)
+    
+    # 接收 GLOBAL_POSITION_INT (msg_type=33)
+    autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2)
+    
+    # 模擬封包遺失 (seq 跳過 3, 4, 5)
+    autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3)
+    
+    stats = autopilot.packet_stats
+    print(f"封包統計:")
+    print(f"  接收: {stats.received_count}")
+    print(f"  遺失: {stats.lost_count}")
+    print(f"  最後序號: {stats.last_seq}")
+    print(f"  訊息類型計數: {stats.msg_type_count}\n")
+
+
+def example_parameters():
+    """參數管理範例"""
+    print("\n=== 參數管理範例 ===\n")
+    
+    vehicle = VehicleView(sysid=3)
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    
+    # 手動設定參數 (不會主動下載)
+    autopilot.set_parameter("ARMING_CHECK", 1)
+    autopilot.set_parameter("ANGLE_MAX", 4500)
+    autopilot.set_parameter("WPNAV_SPEED", 500)
+    
+    print(f"參數數量: {len(autopilot.parameters)}")
+    print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}")
+    print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}")
+    print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n")
+
+
+def example_rf_module():
+    """RF模組範例"""
+    print("\n=== RF模組範例 ===\n")
+    
+    vehicle = VehicleView(sysid=4)
+    vehicle.connected_via = ConnectionType.SERIAL
+    
+    # 設定 XBee RF 模組
+    rf = vehicle.set_rf_module(RFModuleType.XBEE)
+    
+    # 更新 Socket 資訊
+    rf.update_socket_info(
+        ip="192.168.1.100",
+        port=14550,
+        local_ip="192.168.1.1",
+        local_port=14551,
+        connected=True
+    )
+    
+    # 更新 RSSI
+    rf.update_rssi(rssi=-65, timestamp=time.time())
+    
+    # 更新 AT 命令回應
+    rf.update_at_response("OK", timestamp=time.time())
+    
+    # 自定義狀態
+    rf.status.custom_status['signal_quality'] = 'excellent'
+    rf.status.custom_status['packet_error_rate'] = 0.001
+    
+    print(f"RF模組: {rf}")
+    print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}")
+    print(f"RSSI: {rf.status.rssi} dBm")
+    print(f"AT回應: {rf.status.at_response}")
+    print(f"自定義狀態: {rf.status.custom_status}\n")
+
+
+def example_multiple_components():
+    """多組件範例"""
+    print("\n=== 多組件範例 ===\n")
+    
+    vehicle = VehicleView(sysid=5)
+    vehicle.kind = "Copter with Gimbal"
+    
+    # Autopilot 組件
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    autopilot.mav_type = 2
+    autopilot.status.mode.mode_name = "AUTO"
+    
+    # Gimbal 組件
+    gimbal = vehicle.add_component(154, ComponentType.GIMBAL)
+    gimbal.mav_type = 26  # MAV_TYPE_GIMBAL
+    gimbal.status.attitude.pitch = -0.785  # 向下45度
+    gimbal.status.attitude.yaw = 0.0
+    
+    # Camera 組件
+    camera = vehicle.add_component(100, ComponentType.CAMERA)
+    camera.mav_type = 30  # MAV_TYPE_CAMERA
+    camera.status.custom_status['recording'] = True
+    camera.status.custom_status['photo_interval'] = 2.0
+    
+    print(f"載具: {vehicle}")
+    print(f"組件數量: {len(vehicle.components)}")
+    for cid, comp in vehicle.components.items():
+        print(f"  組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}")
+    print()
+
+
+def example_registry():
+    """註冊表使用範例"""
+    print("\n=== 註冊表使用範例 ===\n")
+    
+    # 註冊多個載具
+    v1 = vehicle_registry.register(sysid=1)
+    v1.kind = "Copter-1"
+    v1.add_component(1, ComponentType.AUTOPILOT)
+    
+    v2 = vehicle_registry.register(sysid=2)
+    v2.kind = "Plane-1"
+    v2.add_component(1, ComponentType.AUTOPILOT)
+    
+    v3 = vehicle_registry.register(sysid=3)
+    v3.kind = "Rover-1"
+    v3.add_component(1, ComponentType.AUTOPILOT)
+    
+    print(f"註冊表中的載具數量: {len(vehicle_registry)}")
+    
+    # 取得所有載具
+    all_vehicles = vehicle_registry.get_all()
+    for sysid, vehicle in all_vehicles.items():
+        print(f"  System {sysid}: {vehicle.kind}")
+    
+    # 檢查載具是否存在
+    print(f"\nSystem 2 存在? {2 in vehicle_registry}")
+    print(f"System 99 存在? {99 in vehicle_registry}")
+    
+    # 取得特定載具
+    vehicle = vehicle_registry.get(2)
+    if vehicle:
+        print(f"\n取得載具: {vehicle}")
+    
+    # 註銷載具
+    vehicle_registry.unregister(3)
+    print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n")
+
+
+def example_serialization():
+    """序列化範例 (除錯/日誌用)"""
+    print("\n=== 序列化範例 ===\n")
+    
+    vehicle = VehicleView(sysid=10)
+    vehicle.kind = "Test Copter"
+    vehicle.connected_via = ConnectionType.UDP
+    vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0'
+    vehicle.custom_meta['frame_type'] = 'X'
+    
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    autopilot.mav_type = 2
+    autopilot.status.position.altitude = 50.0
+    autopilot.status.battery.voltage = 12.4
+    autopilot.update_packet_stats(0, 0, time.time())
+    autopilot.update_packet_stats(1, 30, time.time())
+    
+    rf = vehicle.set_rf_module(RFModuleType.UDP)
+    rf.update_rssi(-70)
+    rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True)
+    
+    # 轉換為字典
+    data = vehicle.to_dict()
+    
+    print("載具資料 (字典格式):")
+    import json
+    print(json.dumps(data, indent=2, ensure_ascii=False))
+
+
+def example_gps_ekf():
+    """GPS 與 EKF 範例"""
+    print("\n\n=== GPS 與 EKF 範例 ===\n")
+    
+    vehicle = VehicleView(sysid=11)
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    
+    # GPS 資訊
+    autopilot.status.gps.fix_type = 3  # 3D Fix
+    autopilot.status.gps.satellites_visible = 12
+    autopilot.status.gps.eph = 120  # HDOP = 1.2
+    autopilot.status.gps.epv = 180  # VDOP = 1.8
+    autopilot.status.gps.timestamp = time.time()
+    
+    print(f"GPS:")
+    print(f"  Fix Type: {autopilot.status.gps.fix_type}")
+    print(f"  衛星數: {autopilot.status.gps.satellites_visible}")
+    print(f"  HDOP: {autopilot.status.gps.eph/100}")
+    
+    # EKF 資訊
+    autopilot.status.ekf.flags = 0x1FF  # 所有 flags 都 OK
+    autopilot.status.ekf.velocity_variance = 0.5
+    autopilot.status.ekf.pos_horiz_variance = 1.2
+    autopilot.status.ekf.pos_vert_variance = 2.0
+    autopilot.status.ekf.timestamp = time.time()
+    
+    print(f"\nEKF:")
+    print(f"  Flags: 0x{autopilot.status.ekf.flags:X}")
+    print(f"  速度變異: {autopilot.status.ekf.velocity_variance}")
+    print(f"  水平位置變異: {autopilot.status.ekf.pos_horiz_variance}")
+    print(f"  垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n")
+
+
+def example_vfr_hud():
+    """VFR HUD 範例"""
+    print("\n=== VFR HUD 範例 ===\n")
+    
+    vehicle = VehicleView(sysid=12)
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    
+    # VFR HUD 資訊
+    autopilot.status.vfr.airspeed = 15.5  # m/s
+    autopilot.status.vfr.groundspeed = 14.8  # m/s
+    autopilot.status.vfr.heading = 90  # 東方
+    autopilot.status.vfr.throttle = 65  # %
+    autopilot.status.vfr.climb = 2.5  # m/s
+    autopilot.status.vfr.timestamp = time.time()
+    
+    print(f"VFR HUD:")
+    print(f"  空速: {autopilot.status.vfr.airspeed} m/s")
+    print(f"  地速: {autopilot.status.vfr.groundspeed} m/s")
+    print(f"  航向: {autopilot.status.vfr.heading}°")
+    print(f"  油門: {autopilot.status.vfr.throttle}%")
+    print(f"  爬升率: {autopilot.status.vfr.climb} m/s\n")
+
+
+if __name__ == "__main__":
+    # 執行所有範例
+    # example_basic_usage()
+    # example_packet_tracking()
+    # example_parameters()
+    # example_rf_module()
+    # example_multiple_components()
+    # example_registry()
+    # example_serialization()
+    # example_gps_ekf()
+    example_vfr_hud()
+    
+    print("\n" + "="*50)
+    print("所有範例執行完成！")
+    print("="*50)

src/fc_network_adapter/tests/demo_ringBuffer.py

@@ -0,0 +1,152 @@
+import os
+import sys
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+import time
+import threading
+
+from ..fc_network_adapter.utils import RingBuffer
+
+
+def producer(buffer, count, interval=0.01):
+    """生產者：向緩衝區添加資料"""
+    print(f"Producer started (thread {threading.get_ident()})")
+    for i in range(count):
+        # 嘗試寫入數據，直到成功
+        while not buffer.put(f"Item-{i}"):
+            print(f"Buffer full, producer waiting... (thread {threading.get_ident()})")
+            time.sleep(0.1)
+        
+        print(f"Produced: Item-{i}, buffer size: {buffer.size()}")
+        time.sleep(interval)  # 模擬生產過程
+    
+    print(f"Producer finished (thread {threading.get_ident()})")
+
+def consumer(buffer, max_items, interval=0.05):
+    """消費者：從緩衝區讀取資料"""
+    print(f"Consumer started (thread {threading.get_ident()})")
+    items_consumed = 0
+    
+    while items_consumed < max_items:
+        # 嘗試讀取數據
+        item = buffer.get()
+        if item:
+            print(f"Consumed: {item}, buffer size: {buffer.size()}")
+            items_consumed += 1
+        else:
+            print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})")
+        
+        time.sleep(interval)  # 模擬消費過程
+    
+    print(f"Consumer finished (thread {threading.get_ident()})")
+
+def batch_consumer(buffer, interval=0.2):
+    """批量消費者：一次性讀取緩衝區中的所有資料"""
+    print(f"Batch consumer started (thread {threading.get_ident()})")
+    
+    for _ in range(5):  # 執行5次批量讀取
+        time.sleep(interval)  # 等待緩衝區積累數據
+        items = buffer.get_all()
+        if items:
+            print(f"Batch consumed {len(items)} items: {items}")
+        else:
+            print("Buffer empty for batch consumer")
+    
+    print(f"Batch consumer finished (thread {threading.get_ident()})")
+
+def demonstrate_multi_writer():
+    """示範多個寫入執行緒同時操作緩衝區"""
+    print("\n=== Demonstrating Multiple Writers ===")
+    buffer = RingBuffer(capacity=80)
+    
+    # 創建多個生產者執行緒
+    threads = []
+    for i in range(3):
+        thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1)))
+        threads.append(thread)
+        thread.start()
+    
+    # 等待所有執行緒完成
+    for thread in threads:
+        thread.join()
+
+    buffer.print_stats()  # 印出統計資訊
+
+    # 讀出所有剩餘資料
+    remaining = buffer.get_all()
+    print(f"Remaining items in buffer after multiple writers: {remaining}")
+
+def demonstrate_basic_usage():
+    """示範基本使用方式"""
+    print("\n=== Demonstrating Basic Usage ===")
+    # 創建緩衝區
+    buffer = RingBuffer(capacity=20, buffer_id=7)  
+    
+    # 檢查初始狀態
+    print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
+    
+    # 添加幾個項目
+    for i in range(5):
+        buffer.put(f"Test-{i}")
+    
+    # 檢查狀態
+    print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
+    
+    # 讀取一個項目
+    item = buffer.get()
+    print(f"Read item: {item}")
+    print(f"After reading 1 item - Content Size: {buffer.size()}")
+    
+    # 添加更多項目直到滿
+    items_added = 0
+    while not buffer.is_full():
+        buffer.put(f"Fill-{items_added}")
+        items_added += 1
+    
+    print(f"Added {items_added} more items until full")
+    print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
+    
+    # 嘗試添加到已滿的緩衝區
+    result = buffer.put("Overflow")
+    print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}")
+    
+    # 獲取所有項目
+    all_items = buffer.get_all()
+    print(f"All items in buffer: {all_items}")
+    print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}")
+
+    # 印出統計資訊
+    buffer.print_stats()
+
+def demonstrate_producer_consumer():
+    """示範生產者-消費者模式"""
+    print("\n=== Demonstrating Producer-Consumer Pattern ===")
+    buffer = RingBuffer(capacity=16)
+    
+    # 創建生產者和消費者執行緒
+    producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1))
+    consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2))
+    batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5))
+    
+    # 啟動執行緒
+    producer_thread.start()
+    consumer_thread.start()
+    batch_thread.start()
+    
+    # 等待執行緒完成
+    producer_thread.join()
+    consumer_thread.join()
+    batch_thread.join()
+    
+    # 檢查最終狀態
+    print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}")
+
+    buffer.print_stats()
+
+if __name__ == "__main__":
+    # 展示各種使用場景
+    # demonstrate_basic_usage()
+    # demonstrate_producer_consumer()
+    demonstrate_multi_writer()
+    
+    print("\nAll demonstrations completed!")

src/fc_network_adapter/tests/test_mavlinkObject.py

@@ -13,10 +13,9 @@ import time
 import threading
 import socket
 import asyncio
-from unittest.mock import MagicMock, patch
 
 # 導入要測試的模組
-from fc_network_adapter.mavlinkObject import (
+from ..fc_network_adapter.mavlinkObject import (
     mavlink_object, 
     async_io_manager, 
     MavlinkObjectState, 
@@ -24,62 +23,100 @@ from fc_network_adapter.mavlinkObject import (
     return_packet_ring
 )
 
+# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式)
+# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes)
+HEARTBEAT_PACKET_1 = bytes([
+    0xFE,           # STX (MAVLink 1.0)
+    0x09,           # payload length (9 bytes)
+    0x00,           # sequence
+    0x01,           # system ID = 1
+    0x01,           # component ID = 1
+    0x00,           # message ID (HEARTBEAT = 0)
+    # Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1)
+    0x00, 0x00, 0x00, 0x00,  # custom_mode = 0
+    0x02,                      # type = MAV_TYPE_QUADROTOR (2)
+    0x03,                      # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3)
+    0x40,                      # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64)
+    0x03,                      # system_status = MAV_STATE_STANDBY (3)
+    0x03,                      # mavlink_version = 3
+    0x62, 0x8E                 # CRC (simplified placeholder)
+])
+
+HEARTBEAT_PACKET_2 = bytes([
+    0xFE,           # STX
+    0x09,           # payload length
+    0x01,           # sequence (增加)
+    0x01,           # system ID = 1
+    0x01,           # component ID = 1
+    0x00,           # message ID (HEARTBEAT = 0)
+    0x00, 0x00, 0x00, 0x00,
+    0x02, 0x03, 0x41, 0x03, 0x03,
+    0x33, 0xEC
+])
+
+HEARTBEAT_PACKET_3 = bytes([
+    0xFE,           # STX
+    0x09,           # payload length
+    0x02,           # sequence
+    0x02,           # system ID = 2
+    0x01,           # component ID = 1
+    0x00,           # message ID (HEARTBEAT = 0)
+    0x00, 0x00, 0x00, 0x00,
+    0x02, 0x03, 0x42, 0x03, 0x03,
+    0x37, 0x44
+])
+
     
 class MockMavlinkSocket:
-    """模擬 Mavlink Socket 的類別，用於測試"""
+    """模擬 Mavlink Socket 的類別，用於測試
+    使用真實的 MAVLink 封包，而不是模擬的訊息對象
+    """
     
-    def __init__(self, test_data=None):
+    def __init__(self, test_packets=None):
+        """
+        Args:
+            test_packets: list of bytes，每個元素都是完整的 MAVLink 封包
+        """
         self.closed = False
-        self.test_data = test_data or []
-        self.data_index = 0
+        self.test_packets = test_packets or []
+        self.packet_index = 0
         self.written_data = []
         
+        # 使用 pymavlink 來解析封包
+        from pymavlink import mavutil
+        self.mav_parser = mavutil.mavlink.MAVLink(self)
+        
     def recv_msg(self):
-        if not self.test_data or self.data_index >= len(self.test_data):
+        """返回解析後的 MAVLink 訊息對象"""
+        if not self.test_packets or self.packet_index >= len(self.test_packets):
             return None
         
-        data = self.test_data[self.data_index]
-        self.data_index += 1
-        return data
+        packet = self.test_packets[self.packet_index]
+        self.packet_index += 1
+        
+        # 使用 pymavlink 解析封包
+        try:
+            for byte in packet:
+                msg = self.mav_parser.parse_char(bytes([byte]))
+                if msg:
+                    return msg
+        except Exception as e:
+            print(f"Error parsing packet: {e}")
+            return None
+        
+        return None
         
     def write(self, data):
+        """寫入數據（用於檢查轉發）"""
         self.written_data.append(data)
         
     def close(self):
+        """關閉 socket"""
         self.closed = True
-        
-        
-class MockMavlinkMessage:
-    """模擬 Mavlink 訊息的類別，用於測試"""
-    
-    def __init__(self, msg_id, sysid, compid, seq=0):
-        self.msg_id = msg_id
-        self.sysid = sysid
-        self.compid = compid
-        self.seq = seq
-        self.msg_buf = bytes([msg_id, sysid, compid, seq])
-        
-    def get_msgId(self):
-        return self.msg_id
-        
-    def get_srcSystem(self):
-        return self.sysid
-        
-    def get_srcComponent(self):
-        return self.compid
-        
-    def get_seq(self):
-        return self.seq
-        
-    def get_msgbuf(self):
-        return self.msg_buf
-        
-    def get_type(self):
-        return f"MSG_ID_{self.msg_id}"
 
 
 class TestMavlinkObject(unittest.TestCase):
-    """測試 mavlink_object 類別"""
+    """測試 mavlink_object 類別的獨立功能"""
     
     def setUp(self):
         """在每個測試方法執行前準備環境"""
@@ -91,35 +128,27 @@ class TestMavlinkObject(unittest.TestCase):
         stream_bridge_ring.clear()
         return_packet_ring.clear()
         
-        # 創建測試訊息
-        self.heartbeat_msg = MockMavlinkMessage(msg_id=0, sysid=1, compid=1)
-        self.attitude_msg = MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
-        self.position_msg = MockMavlinkMessage(msg_id=32, sysid=1, compid=1)
-        
-        # 創建模擬的 socket # 假的 Mavlink Socket
-        self.mock_socket = MockMavlinkSocket([
-            self.heartbeat_msg,
-            self.attitude_msg,
-            self.position_msg
-        ])
+        # 創建模擬的 socket，使用真實封包
+        self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1])
         
         # 創建測試對象
         self.mavlink_obj = mavlink_object(self.mock_socket)
     
     def test_initialization(self):
         """測試 mavlink_object 初始化是否正確"""
-        # print("Testing mavlink_object initialization")
         self.assertEqual(self.mavlink_obj.socket_id, 0)
         self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT)
         self.assertEqual(len(self.mavlink_obj.target_sockets), 0)
         self.assertEqual(self.mavlink_obj.bridge_msg_types, [0])
-        
+        self.assertEqual(self.mavlink_obj.return_msg_types, [])
+
     def test_add_remove_target_socket(self):
         """測試添加和移除目標端口功能"""
         # 添加目標端口
         self.assertTrue(self.mavlink_obj.add_target_socket(1))
         self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
         self.assertEqual(self.mavlink_obj.target_sockets[0], 1)
+        
         self.assertTrue(self.mavlink_obj.add_target_socket(2))
         self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
         self.assertIn(2, self.mavlink_obj.target_sockets)
@@ -153,78 +182,31 @@ class TestMavlinkObject(unittest.TestCase):
         self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid"))
         self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"]))
         
-    def test_send_message(self):
-        """測試 send_message 功能"""
-        # 創建一個新的 mavlink_object 實例
-        mock_socket = MockMavlinkSocket()
-        mavlink_obj = mavlink_object(mock_socket)
-        
-        # 準備測試數據
-        test_message1 = b"test_message_1"
-        test_message2 = b"test_message_2"
-        
-        # 測試初始狀態
-        self.assertEqual(len(mock_socket.written_data), 0)
-        
+    def test_send_message_validation(self):
+        """測試 send_message 的數據驗證功能（不需要啟動 manager）"""
         # 測試非運行狀態下發送消息
-        self.assertFalse(mavlink_obj.send_message(test_message1))
-        self.assertEqual(len(mock_socket.written_data), 0)
+        self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
         
-        # 啟動 manager
-        manager = async_io_manager()
-        manager.start()
-        time.sleep(0.5)  # 等待事件循環啟動
+        # 測試無效的數據類型
+        self.mavlink_obj.state = MavlinkObjectState.RUNNING  # 臨時設置狀態
+        self.assertFalse(self.mavlink_obj.send_message("invalid"))
+        self.assertFalse(self.mavlink_obj.send_message(123))
         
-        # 添加對象到 manager
-        manager.add_mavlink_object(mavlink_obj)
-        time.sleep(0.1)  # 等待對象啟動
+        # 測試太短的封包
+        self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00])))
         
-        # 確認對象狀態
-        self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
+        # 測試無效的起始標記
+        invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00])
+        self.assertFalse(self.mavlink_obj.send_message(invalid_packet))
         
-        # 測試發送消息
-        self.assertTrue(mavlink_obj.send_message(test_message1))
-        time.sleep(0.2)  # 等待消息處理
+        # 測試有效的封包可以加入佇列
+        self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+        self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1)
         
-        # 確認消息已發送
-        self.assertEqual(len(mock_socket.written_data), 1)
-        self.assertEqual(mock_socket.written_data[0], test_message1)
-        
-        # 測試連續發送多條消息
-        self.assertTrue(mavlink_obj.send_message(test_message2))
-        time.sleep(0.2)  # 等待消息處理
-        
-        # 確認兩條消息都已發送
-        self.assertEqual(len(mock_socket.written_data), 2)
-        self.assertEqual(mock_socket.written_data[1], test_message2)
-        
-        # 模擬發送出錯的情況
-        class ErrorWriteSocket(MockMavlinkSocket):
-            def write(self, data):
-                raise Exception("Write error")
-        
-        error_socket = ErrorWriteSocket()
-        error_obj = mavlink_object(error_socket)
-        manager.add_mavlink_object(error_obj)
-        time.sleep(0.1)  # 等待對象啟動
-        
-        # 發送消息到錯誤的 socket
-        self.assertTrue(error_obj.send_message(test_message1))
-        time.sleep(0.2)  # 等待消息處理
-        
-        # 即使寫入失敗，send_message 應該也返回 True
-        # 因為消息已成功加入到佇列中，只是後續的實際發送失敗
-        
-        # 停止 manager
-        manager.stop()
-        time.sleep(0.5)  # 等待 manager 停止
-        
-        # 測試對象已關閉後發送消息
-        self.assertFalse(mavlink_obj.send_message(test_message1))
-        self.assertEqual(len(mock_socket.written_data), 2)  # 消息數量未增加
+        self.mavlink_obj.state = MavlinkObjectState.INIT  # 恢復狀態
 
 class TestAsyncIOManager(unittest.TestCase):
-    """測試 async_io_manager 類別"""
+    """測試 async_io_manager 類別的獨立功能"""
     
     def setUp(self):
         """在每個測試方法執行前準備環境"""
@@ -239,14 +221,9 @@ class TestAsyncIOManager(unittest.TestCase):
         # 創建 async_io_manager 實例
         self.manager = async_io_manager()
         
-        # 模擬 mavlink 對象
-        self.mock_socket1 = MockMavlinkSocket([
-            MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
-            MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
-        ])
-        self.mock_socket2 = MockMavlinkSocket([
-            MockMavlinkMessage(msg_id=0, sysid=2, compid=1)
-        ])
+        # 創建模擬 mavlink 對象，使用真實封包
+        self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2])
+        self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3])
         
         self.mavlink_obj1 = mavlink_object(self.mock_socket1)
         self.mavlink_obj2 = mavlink_object(self.mock_socket2)
@@ -254,10 +231,10 @@ class TestAsyncIOManager(unittest.TestCase):
     def tearDown(self):
         """在每個測試方法執行後清理環境"""
         if self.manager.running:
-            self.manager.stop()
+            self.manager.shutdown()
         
     def test_singleton_pattern(self):
-        """測試 async_io_manager 的單例模式 就是不會產生兩個 magager"""
+        """測試 async_io_manager 的單例模式"""
         manager1 = async_io_manager()
         manager2 = async_io_manager()
         self.assertIs(manager1, manager2)
@@ -275,7 +252,7 @@ class TestAsyncIOManager(unittest.TestCase):
         self.assertIs(self.manager.thread, old_thread)
         
         # 停止管理器
-        self.manager.stop()
+        self.manager.shutdown()
         self.assertFalse(self.manager.running)
         
         # 最多等待 5 秒讓線程結束
@@ -313,156 +290,179 @@ class TestAsyncIOManager(unittest.TestCase):
         self.assertFalse(self.manager.remove_mavlink_object(999))
         
         # 停止管理器
-        self.manager.stop()
+        self.manager.shutdown()
         
-    def test_data_processing(self):
-        """測試數據處理"""
+class TestIntegration(unittest.TestCase):
+    """整合測試，測試多個 mavlink_object 之間的互動與資料流"""
+    
+    def setUp(self):
+        """在每個測試方法執行前準備環境"""
+        # 清空全局變數
+        mavlink_object.mavlinkObjects = {}
+        mavlink_object.socket_num = 0
+        
+        # 清空 ring buffer
+        stream_bridge_ring.clear()
+        return_packet_ring.clear()
+        
+        # 創建 async_io_manager 實例
+        self.manager = async_io_manager()
+
+    def tearDown(self):
+        """在每個測試方法執行後清理環境"""
+        if self.manager.running:
+            self.manager.shutdown()
+
+    def test_send_message_with_manager(self):
+        """測試透過 async_io_manager 發送訊息的完整流程"""
+        # 創建一個新的 mavlink_object 實例
+        mock_socket = MockMavlinkSocket()
+        mavlink_obj = mavlink_object(mock_socket)
+        
+        # 測試初始狀態
+        self.assertEqual(len(mock_socket.written_data), 0)
+        
+        # 測試非運行狀態下發送消息
+        self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+        self.assertEqual(len(mock_socket.written_data), 0)
+        
+        # 啟動 manager
+        self.manager.start()
+        time.sleep(0.5)  # 等待事件循環啟動
+        
+        # 添加對象到 manager
+        self.manager.add_mavlink_object(mavlink_obj)
+        time.sleep(0.1)  # 等待對象啟動
+        
+        # 確認對象狀態
+        self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
+        
+        # 測試發送消息
+        self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+        time.sleep(0.2)  # 等待消息處理
+        
+        # 確認消息已發送
+        self.assertEqual(len(mock_socket.written_data), 1)
+        self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1)
+        
+        # 測試連續發送多條消息
+        self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2))
+        time.sleep(0.2)  # 等待消息處理
+        
+        # 確認兩條消息都已發送
+        self.assertEqual(len(mock_socket.written_data), 2)
+        self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2)
+        
+        # 停止 manager
+        self.manager.shutdown()
+        time.sleep(0.5)  # 等待 manager 停止
+        
+        # 測試對象已關閉後發送消息
+        self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+        self.assertEqual(len(mock_socket.written_data), 2)  # 消息數量未增加
+    
+    def test_data_processing_and_forwarding(self):
+        """測試數據處理與轉發流程"""
         # 創建用於轉發的 mavlink_objects
+        mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,])
         mock_socket3 = MockMavlinkSocket()
+        
+        mavlink_obj1 = mavlink_object(mock_socket1)
         mavlink_obj3 = mavlink_object(mock_socket3)
-        
-        # 設置轉發
-        self.mavlink_obj1.add_target_socket(2)  # socket1 轉發到 socket3
-        
+
         # 設置訊息類型
-        self.mavlink_obj1.set_bridge_message_types([0, 30])  # HEARTBEAT, ATTITUDE
-        # self.mavlink_obj1.enable_return(True)
-        self.mavlink_obj1.set_return_message_types([30])  # ATTITUDE
+        mavlink_obj1.set_bridge_message_types([0])  # 只處理 HEARTBEAT
+        
+        # 設置轉發: obj1 -> obj3
+        mavlink_obj1.add_target_socket(mavlink_obj3.socket_id)  # socket1 轉發到 socket3 (socket_id=1)
         
         # 啟動管理器並添加對象
         self.manager.start()
         time.sleep(0.5)  # 等待事件循環啟動
         
-        self.manager.add_mavlink_object(self.mavlink_obj1)
+        """
+        這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現
+        mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包
+        若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失
+        """
         self.manager.add_mavlink_object(mavlink_obj3)
+        self.manager.add_mavlink_object(mavlink_obj1)
         
         # 等待處理完成
-        time.sleep(1.0)
+        time.sleep(0.5)
 
-        # print("testing Mark A")
-        
         # 檢查 Ring buffer 是否有正確的數據
-        self.assertEqual(stream_bridge_ring.size(), 2)  # HEARTBEAT + ATTITUDE
-        self.assertEqual(return_packet_ring.size(), 1)  # ATTITUDE
+        self.assertGreaterEqual(stream_bridge_ring.size(), 2)  # 至少 2 個 HEARTBEAT
 
-        a = stream_bridge_ring.get()
-        print(f"stream_bridge_ring: {a}")
-        
         # 檢查是否正確轉發
-        self.assertEqual(len(mock_socket3.written_data), 2)  # HEARTBEAT + ATTITUDE
-        
-        # print("testing Mark B")
-        
+        self.assertGreaterEqual(len(mock_socket3.written_data), 2)  # 至少 2 個 HEARTBEAT
+
         # 停止管理器
-        self.manager.stop()
+        self.manager.shutdown()
+
+    def test_bidirectional_forwarding(self):
+        """測試雙向轉發"""
+        # 清空全局變數和 ring buffer
+        mavlink_object.mavlinkObjects = {}
+        mavlink_object.socket_num = 0
+        stream_bridge_ring.clear()
+        return_packet_ring.clear()
         
-    def test_error_handling(self):
-        """測試錯誤處理情況"""
-        print("=== mark A ===")
-        # 創建一個會引發異常的 socket
-        class ErrorSocket(MockMavlinkSocket):
-            def recv_msg(self):
-                raise Exception("Test exception")
-
-        error_socket = ErrorSocket()
-        mavlink_obj_err = mavlink_object(error_socket)
-
-        # 啟動管理器並添加對象
+        # 創建三個 mavlink 對象，模擬三個通道
+        socket1 = MockMavlinkSocket()
+        socket2 = MockMavlinkSocket()
+        socket3 = MockMavlinkSocket()
+        
+        obj1 = mavlink_object(socket1)
+        obj2 = mavlink_object(socket2)
+        obj3 = mavlink_object(socket3)
+        
+        # 設置雙向轉發
+        # obj1 <-> obj2 <-> obj3
+        obj1.add_target_socket(1)  # obj1 -> obj2
+        obj2.add_target_socket(0)  # obj2 -> obj1
+        obj2.add_target_socket(2)  # obj2 -> obj3
+        obj3.add_target_socket(1)  # obj3 -> obj2
+        
+        # 啟動 async_io_manager
         self.manager.start()
         time.sleep(0.5)  # 等待事件循環啟動
+        
+        # 添加所有 mavlink_object
+        self.manager.add_mavlink_object(obj1)
+        self.manager.add_mavlink_object(obj2)
+        self.manager.add_mavlink_object(obj3)
 
-        self.manager.add_mavlink_object(mavlink_obj_err)
-
-        print("=== mark B ===")
-
-        # 等待錯誤處理
+        # 對三個對象添加數據
+        socket1.test_packets.append(HEARTBEAT_PACKET_1)
+        socket2.test_packets.append(HEARTBEAT_PACKET_2)
+        socket3.test_packets.append(HEARTBEAT_PACKET_3)
+        
+        # 等待處理所有訊息
         time.sleep(1.0)
-
-        # 對象應該進入錯誤狀態，但不會崩潰
-        # self.assertEqual(mavlink_obj_err.state, MavlinkObjectState.ERROR)
-
-        print("=== mark C ===")
-
-        # 管理器應該仍在運行
-        self.assertTrue(self.manager.running)
-
-
-        # 故意等一段時間 確認 socket 有被 manager 處理掉
-        time.sleep(3)
-
+        
+        # 檢查轉發結果
+        # socket1 應該收到 socket2 的訊息
+        self.assertGreaterEqual(len(socket1.written_data), 1)
+        
+        # socket2 應該收到 socket1 和 socket3 的訊息
+        self.assertGreaterEqual(len(socket2.written_data), 2)
+        
+        # socket3 應該收到 socket2 的訊息
+        self.assertGreaterEqual(len(socket3.written_data), 1)
+        
+        # 檢查 ring buffer 的數據
+        # 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
+        self.assertGreaterEqual(stream_bridge_ring.size(), 3)  # 至少 3 個 HEARTBEAT
+        
         # 停止管理器
-        self.manager.stop()
-
-
-class TestIntegration(unittest.TestCase):
-    """整合測試，測試多個 mavlink_object 之間的互動"""
-    
-    def test_bidirectional_forwarding(self):
-                """測試雙向轉發"""
-                # 清空全局變數和 ring buffer
-                mavlink_object.mavlinkObjects = {}
-                mavlink_object.socket_num = 0
-                stream_bridge_ring.clear()
-                return_packet_ring.clear()
-                
-                # 創建三個 mavlink 對象，模擬三個通道
-                socket1 = MockMavlinkSocket([
-                    MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
-                    MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
-                ])
-                socket2 = MockMavlinkSocket([
-                    MockMavlinkMessage(msg_id=0, sysid=2, compid=1),
-                    MockMavlinkMessage(msg_id=32, sysid=2, compid=1)
-                ])
-                socket3 = MockMavlinkSocket([
-                    MockMavlinkMessage(msg_id=0, sysid=3, compid=1),
-                    MockMavlinkMessage(msg_id=33, sysid=3, compid=1)
-                ])
-                
-                obj1 = mavlink_object(socket1)
-                obj2 = mavlink_object(socket2)
-                obj3 = mavlink_object(socket3)
-                
-                # 設置雙向轉發
-                # obj1 <-> obj2 <-> obj3
-                obj1.add_target_socket(1)  # obj1 -> obj2
-                obj2.add_target_socket(0)  # obj2 -> obj1
-                obj2.add_target_socket(2)  # obj2 -> obj3
-                obj3.add_target_socket(1)  # obj3 -> obj2
-                
-                # 啟動 async_io_manager
-                manager = async_io_manager()
-                manager.start()
-                time.sleep(0.5)  # 等待事件循環啟動
-                
-                # 添加所有 mavlink_object
-                manager.add_mavlink_object(obj1)
-                manager.add_mavlink_object(obj2)
-                manager.add_mavlink_object(obj3)
-                
-                # 等待處理所有訊息
-                time.sleep(1.5)
-                
-                # 檢查轉發結果
-                # socket1 應該收到 socket2 和 socket3 的訊息
-                self.assertEqual(len(socket1.written_data), 4)  # 2 from obj2, 2 from obj3 via obj2
-                
-                # socket2 應該收到 socket1 和 socket3 的訊息
-                self.assertEqual(len(socket2.written_data), 4)  # 2 from obj1, 2 from obj3
-                
-                # socket3 應該收到 socket1 和 socket2 的訊息
-                self.assertEqual(len(socket3.written_data), 4)  # 2 from obj1 via obj2, 2 from obj2
-                
-                # 檢查 ring buffer 的數據
-                # 假設所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
-                # 應該有 3 個 HEARTBEAT 訊息
-                self.assertEqual(stream_bridge_ring.size(), 3)  # 3 HEARTBEAT
-                
-                # 停止管理器
-                manager.stop()
-
-
+        self.manager.shutdown()
 
 
 if __name__ == "__main__":
-    unittest.main(defaultTest="TestMavlinkObject.test_send_message")
+    # 可以指定要運行的測試
+    # unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation")
+    # unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects")
+    unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding")
+    unittest.main()
+

src/fc_network_adapter/tests/test_ringBuffer.py

@@ -1,152 +1,296 @@
-import os
-import sys
-sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+#!/usr/bin/env python
+"""
+測試 RingBuffer 類別的功能
+"""
 
-import time
+import unittest
 import threading
+import time
+from concurrent.futures import ThreadPoolExecutor
 
-from fc_network_adapter.ringBuffer import RingBuffer
+from ..fc_network_adapter.utils import RingBuffer
 
-
-def producer(buffer, count, interval=0.01):
-    """生產者：向緩衝區添加資料"""
-    print(f"Producer started (thread {threading.get_ident()})")
-    for i in range(count):
-        # 嘗試寫入數據，直到成功
-        while not buffer.put(f"Item-{i}"):
-            print(f"Buffer full, producer waiting... (thread {threading.get_ident()})")
-            time.sleep(0.1)
+class TestRingBuffer(unittest.TestCase):
+    """測試 RingBuffer 基本功能"""
+    
+    def setUp(self):
+        """每個測試前的準備"""
+        self.buffer = RingBuffer(capacity=8)
+    
+    def test_initialization(self):
+        """測試初始化"""
+        self.assertEqual(self.buffer.capacity, 8)
+        self.assertTrue(self.buffer.is_empty())
+        self.assertFalse(self.buffer.is_full())
+        self.assertEqual(self.buffer.size(), 0)
+    
+    def test_put_get_basic(self):
+        """測試基本的放入和取出"""
+        # 測試放入
+        self.assertTrue(self.buffer.put("item1"))
+        self.assertTrue(self.buffer.put("item2"))
+        self.assertEqual(self.buffer.size(), 2)
+        self.assertFalse(self.buffer.is_empty())
         
-        print(f"Produced: Item-{i}, buffer size: {buffer.size()}")
-        time.sleep(interval)  # 模擬生產過程
-    
-    print(f"Producer finished (thread {threading.get_ident()})")
-
-def consumer(buffer, max_items, interval=0.05):
-    """消費者：從緩衝區讀取資料"""
-    print(f"Consumer started (thread {threading.get_ident()})")
-    items_consumed = 0
-    
-    while items_consumed < max_items:
-        # 嘗試讀取數據
-        item = buffer.get()
-        if item:
-            print(f"Consumed: {item}, buffer size: {buffer.size()}")
-            items_consumed += 1
-        else:
-            print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})")
+        # 測試取出
+        item1 = self.buffer.get()
+        self.assertEqual(item1, "item1")
+        self.assertEqual(self.buffer.size(), 1)
         
-        time.sleep(interval)  # 模擬消費過程
+        item2 = self.buffer.get()
+        self.assertEqual(item2, "item2")
+        self.assertTrue(self.buffer.is_empty())
+        
+        # 空緩衝區取出應返回 None
+        self.assertIsNone(self.buffer.get())
     
-    print(f"Consumer finished (thread {threading.get_ident()})")
+    def test_capacity_overflow(self):
+        """測試緩衝區容量限制"""
+        # 填滿緩衝區 (容量-1，因為需要預留一個位置)
+        for i in range(7):  # 8-1=7
+            self.assertTrue(self.buffer.put(f"item{i}"))
+        
+        self.assertTrue(self.buffer.is_full())
+        
+        # 嘗試再放入一個應該失敗
+        self.assertFalse(self.buffer.put("overflow"))
+        self.assertEqual(self.buffer.overflow_count.value, 1)
+    
+    def test_get_all(self):
+        """測試取出所有項目"""
+        items = ["a", "b", "c", "d"]
+        for item in items:
+            self.buffer.put(item)
+        
+        all_items = self.buffer.get_all()
+        self.assertEqual(all_items, items)
+        self.assertTrue(self.buffer.is_empty())
+    
+    def test_clear(self):
+        """測試清空緩衝區"""
+        for i in range(5):
+            self.buffer.put(f"item{i}")
+        
+        self.buffer.clear()
+        self.assertTrue(self.buffer.is_empty())
+        self.assertEqual(self.buffer.size(), 0)
 
-def batch_consumer(buffer, interval=0.2):
-    """批量消費者：一次性讀取緩衝區中的所有資料"""
-    print(f"Batch consumer started (thread {threading.get_ident()})")
-    
-    for _ in range(5):  # 執行5次批量讀取
-        time.sleep(interval)  # 等待緩衝區積累數據
-        items = buffer.get_all()
-        if items:
-            print(f"Batch consumed {len(items)} items: {items}")
-        else:
-            print("Buffer empty for batch consumer")
-    
-    print(f"Batch consumer finished (thread {threading.get_ident()})")
 
-def demonstrate_multi_writer():
-    """示範多個寫入執行緒同時操作緩衝區"""
-    print("\n=== Demonstrating Multiple Writers ===")
-    buffer = RingBuffer(capacity=80)
+class TestRingBufferThreadSafety(unittest.TestCase):
+    """測試 RingBuffer 的線程安全性"""
     
-    # 創建多個生產者執行緒
-    threads = []
-    for i in range(3):
-        thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1)))
-        threads.append(thread)
-        thread.start()
+    def setUp(self):
+        """每個測試前的準備"""
+        self.buffer = RingBuffer(capacity=256)
+        self.results = []
+        self.write_count = 1000
+        self.read_count = 1000
     
-    # 等待所有執行緒完成
-    for thread in threads:
-        thread.join()
+    def test_concurrent_producers_consumers(self):
+        """測試多生產者多消費者場景"""
+        self.results = []
+        stats = self.buffer.get_stats()
+        self.assertEqual(stats['total_writes'], 0)
+        
+        def producer(producer_id, count):
+            """生產者函數"""
+            for i in range(count):
+                item = f"producer_{producer_id}_item_{i}"
+                while not self.buffer.put(item):
+                    time.sleep(0.001)  # 緩衝區滿時稍微等待
+        
+        def consumer(consumer_id, count):
+            """消費者函數"""
+            items = []
+            for _ in range(count):
+                item = None
+                while item is None:
+                    item = self.buffer.get()
+                    if item is None:
+                        time.sleep(0.001)  # 緩衝區空時稍微等待
+                items.append(item)
+            self.results.extend(items)
+        
+        # 創建多個生產者和消費者
+        with ThreadPoolExecutor(max_workers=8) as executor:
+            # 2 個生產者，每個寫入 500 個項目
+            producer_futures = [
+                executor.submit(producer, 0, 500),
+                executor.submit(producer, 1, 500)
+            ]
+            
+            # 2 個消費者，每個讀取 500 個項目
+            consumer_futures = [
+                executor.submit(consumer, 0, 500),
+                executor.submit(consumer, 1, 500)
+            ]
+            
+            # 等待所有任務完成
+            for future in producer_futures + consumer_futures:
+                future.result()
+        
+        # 驗證結果
+        self.assertEqual(len(self.results), 1000)
+        self.assertTrue(self.buffer.is_empty())
+        
+        # 檢查統計數據
+        stats = self.buffer.get_stats()
+        self.assertEqual(stats['total_writes'], 1000)
+        self.assertGreater(stats['total_reads'], 1000)  # 包含失敗的讀取嘗試
+        self.assertGreater(stats['write_threads'], 1)
+        self.assertGreater(stats['read_threads'], 1)
+    
+    def test_high_throughput(self):
+        """測試高吞吐量場景"""
+        items_per_thread = 10000
+        num_threads = 4
+        
+        def writer():
+            for i in range(items_per_thread):
+                while not self.buffer.put(i):
+                    pass  # 忙等待
+        
+        def reader():
+            items = []
+            for _ in range(items_per_thread):
+                item = None
+                while item is None:
+                    item = self.buffer.get()
+                items.append(item)
+            self.results.extend(items)
+        
+        start_time = time.time()
+        
+        with ThreadPoolExecutor(max_workers=num_threads * 2) as executor:
+            # 啟動寫入線程
+            writer_futures = [executor.submit(writer) for _ in range(num_threads)]
+            
+            # 啟動讀取線程
+            reader_futures = [executor.submit(reader) for _ in range(num_threads)]
+            
+            # 等待完成
+            for future in writer_futures + reader_futures:
+                future.result()
+        
+        end_time = time.time()
+        
+        # 驗證結果
+        total_items = items_per_thread * num_threads
+        self.assertEqual(len(self.results), total_items)
+        
+        # 性能統計
+        duration = end_time - start_time
+        throughput = total_items / duration
+        
+        print(f"\nHigh Throughput Test Results:")
+        print(f"Total items: {total_items}")
+        print(f"Duration: {duration:.2f}s")
+        print(f"Throughput: {throughput:.0f} items/sec")
+        
+        # 顯示詳細統計
+        self.buffer.print_stats()
 
-    buffer.print_stats()  # 印出統計資訊
 
-    # 讀出所有剩餘資料
-    remaining = buffer.get_all()
-    print(f"Remaining items in buffer after multiple writers: {remaining}")
+class TestRingBufferStatistics(unittest.TestCase):
+    """測試 RingBuffer 的統計功能"""
+    
+    def test_statistics_tracking(self):
+        """測試統計數據追蹤"""
+        buffer = RingBuffer(capacity=16)
+        
+        # 寫入一些數據
+        for i in range(10):
+            buffer.put(f"item{i}")
+        
+        # 讀取一些數據
+        for _ in range(5):
+            buffer.get()
+        
+        stats = buffer.get_stats()
+        
+        # 驗證基本統計
+        self.assertEqual(stats['total_writes'], 10)
+        self.assertEqual(stats['total_reads'], 5)
+        self.assertEqual(stats['current_size'], 5)
+        self.assertEqual(stats['write_threads'], 1)
+        self.assertEqual(stats['read_threads'], 1)
+        
+    def test_reset_statistics(self):
+        """測試重置統計數據"""
+        buffer = RingBuffer(capacity=16)
+        
+        # 產生一些活動
+        for i in range(5):
+            buffer.put(f"item{i}")
+        for _ in range(3):
+            buffer.get()
+        
+        # 重置統計
+        buffer.reset_stats()
+        
+        stats = buffer.get_stats()
+        self.assertEqual(stats['total_writes'], 0)
+        self.assertEqual(stats['total_reads'], 0)
+        self.assertEqual(stats['concurrent_writes'], 0)
+        self.assertEqual(stats['concurrent_reads'], 0)
+        self.assertEqual(stats['overflow_count'], 0)
 
-def demonstrate_basic_usage():
-    """示範基本使用方式"""
-    print("\n=== Demonstrating Basic Usage ===")
-    # 創建緩衝區
-    buffer = RingBuffer(capacity=20, buffer_id=7)  
-    
-    # 檢查初始狀態
-    print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
-    
-    # 添加幾個項目
-    for i in range(5):
-        buffer.put(f"Test-{i}")
-    
-    # 檢查狀態
-    print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
-    
-    # 讀取一個項目
-    item = buffer.get()
-    print(f"Read item: {item}")
-    print(f"After reading 1 item - Content Size: {buffer.size()}")
-    
-    # 添加更多項目直到滿
-    items_added = 0
-    while not buffer.is_full():
-        buffer.put(f"Fill-{items_added}")
-        items_added += 1
-    
-    print(f"Added {items_added} more items until full")
-    print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
-    
-    # 嘗試添加到已滿的緩衝區
-    result = buffer.put("Overflow")
-    print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}")
-    
-    # 獲取所有項目
-    all_items = buffer.get_all()
-    print(f"All items in buffer: {all_items}")
-    print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}")
 
-    # 印出統計資訊
+def benchmark_ringbuffer():
+    """RingBuffer 性能基準測試"""
+    print("\n=== RingBuffer Performance Benchmark ===")
+    
+    buffer = RingBuffer(capacity=1024)
+    num_operations = 100000
+    
+    # 單線程性能測試
+    start_time = time.time()
+    for i in range(num_operations):
+        buffer.put(i)
+    for _ in range(num_operations):
+        buffer.get()
+    end_time = time.time()
+    
+    single_thread_time = end_time - start_time
+    throughput = (num_operations * 2) / single_thread_time
+    
+    print(f"Single Thread: {throughput:.0f} ops/sec")
+    
+    # 多線程性能測試
+    buffer = RingBuffer(capacity=1024)
+    
+    def producer():
+        for i in range(num_operations // 2):
+            while not buffer.put(i):
+                pass
+    
+    def consumer():
+        for _ in range(num_operations // 2):
+            while buffer.get() is None:
+                pass
+    
+    start_time = time.time()
+    
+    with ThreadPoolExecutor(max_workers=2) as executor:
+        future1 = executor.submit(producer)
+        future2 = executor.submit(consumer)
+        future1.result()
+        future2.result()
+    
+    end_time = time.time()
+    
+    multi_thread_time = end_time - start_time
+    throughput = num_operations / multi_thread_time
+    
+    print(f"Multi Thread: {throughput:.0f} ops/sec")
+    print(f"Speedup: {single_thread_time/multi_thread_time:.2f}x")
+    
     buffer.print_stats()
 
-def demonstrate_producer_consumer():
-    """示範生產者-消費者模式"""
-    print("\n=== Demonstrating Producer-Consumer Pattern ===")
-    buffer = RingBuffer(capacity=16)
-    
-    # 創建生產者和消費者執行緒
-    producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1))
-    consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2))
-    batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5))
-    
-    # 啟動執行緒
-    producer_thread.start()
-    consumer_thread.start()
-    batch_thread.start()
-    
-    # 等待執行緒完成
-    producer_thread.join()
-    consumer_thread.join()
-    batch_thread.join()
-    
-    # 檢查最終狀態
-    print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}")
-
-    buffer.print_stats()
 
 if __name__ == "__main__":
-    # 展示各種使用場景
-    # demonstrate_basic_usage()
-    # demonstrate_producer_consumer()
-    demonstrate_multi_writer()
+    # 運行單元測試
+    unittest.main(argv=[''], exit=False, verbosity=2)
     
-    print("\nAll demonstrations completed!")
+    # 運行性能基準測試
+    benchmark_ringbuffer()

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -0,0 +1,507 @@
+"""
+VehicleStatusPublisher 測試程式
+
+測試從 vehicle_registry 讀取資料並發布到 ROS2 topics
+"""
+
+import time
+import json
+import threading
+
+# ROS2 imports
+import rclpy
+from rclpy.node import Node
+
+# 標準 ROS2 消息類型
+import std_msgs.msg
+import sensor_msgs.msg
+import geometry_msgs.msg
+import mavros_msgs.msg
+
+# 專案 imports
+from ..fc_network_adapter.mavlinkROS2Nodes import (
+    VehicleStatusPublisher,
+    fc_ros_manager,
+    ros2_manager
+)
+from ..fc_network_adapter.mavlinkVehicleView import (
+    vehicle_registry,
+    ConnectionType,
+    ComponentType,
+)
+
+
+class TestSubscriber(Node):
+    """測試用的訂閱者節點 - 接收並記錄收到的消息"""
+    
+    def __init__(self, sysid: int = 1):
+        super().__init__(f'test_subscriber_sys{sysid}')
+        
+        self.sysid = sysid
+        self.received_messages = {
+            'position': [],
+            'attitude': [],
+            'velocity': [],
+            'battery': [],
+            'vfr_hud': [],
+            'mode': [],
+            'summary': [],
+        }
+        
+        # 建立所有訂閱者
+        self._create_subscriptions()
+        
+        print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics")
+    
+    def _create_subscriptions(self):
+        """建立所有 topic 的訂閱者"""
+        
+        base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}'
+        
+        # Position
+        self.create_subscription(
+            sensor_msgs.msg.NavSatFix,
+            f'{base_topic}/position',
+            lambda msg: self._on_message('position', msg),
+            10
+        )
+        
+        # Attitude
+        self.create_subscription(
+            sensor_msgs.msg.Imu,
+            f'{base_topic}/attitude',
+            lambda msg: self._on_message('attitude', msg),
+            10
+        )
+        
+        # Velocity
+        self.create_subscription(
+            geometry_msgs.msg.TwistStamped,
+            f'{base_topic}/velocity',
+            lambda msg: self._on_message('velocity', msg),
+            10
+        )
+        
+        # Battery
+        self.create_subscription(
+            sensor_msgs.msg.BatteryState,
+            f'{base_topic}/battery',
+            lambda msg: self._on_message('battery', msg),
+            10
+        )
+        
+        # VFR HUD
+        self.create_subscription(
+            mavros_msgs.msg.VfrHud,
+            f'{base_topic}/vfr_hud',
+            lambda msg: self._on_message('vfr_hud', msg),
+            10
+        )
+        
+        # Mode
+        self.create_subscription(
+            std_msgs.msg.String,
+            f'{base_topic}/mode',
+            lambda msg: self._on_message('mode', msg),
+            10
+        )
+        
+        # Summary
+        self.create_subscription(
+            std_msgs.msg.String,
+            f'{base_topic}/summary',
+            lambda msg: self._on_message('summary', msg),
+            10
+        )
+    
+    def _on_message(self, topic_name: str, msg):
+        """通用消息接收回調"""
+        self.received_messages[topic_name].append(msg)
+        print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}")
+    
+    def _format_msg(self, topic_name: str, msg) -> str:
+        """格式化消息以便顯示"""
+        if topic_name == 'position':
+            return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
+        elif topic_name == 'attitude':
+            return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
+        elif topic_name == 'velocity':
+            return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})"
+        elif topic_name == 'battery':
+            return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%"
+        elif topic_name == 'vfr_hud':
+            return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}"
+        elif topic_name == 'mode':
+            return f"mode={msg.data}"
+        elif topic_name == 'summary':
+            try:
+                data = json.loads(msg.data)
+                return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}"
+            except:
+                return msg.data
+        return str(msg)
+    
+    def get_message_count(self, topic_name: str) -> int:
+        """獲取收到的消息數量"""
+        return len(self.received_messages[topic_name])
+    
+    def clear_messages(self):
+        """清空已收到的消息"""
+        for key in self.received_messages:
+            self.received_messages[key].clear()
+
+
+def setup_test_vehicle(sysid: int = 1, socket_id: int = 10):
+    """
+    建立測試用的載具數據
+    
+    Args:
+        sysid: 系統 ID
+        socket_id: Socket ID
+    """
+    print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===")
+    
+    # 註冊載具
+    vehicle = vehicle_registry.register(sysid)
+    vehicle.kind = "Copter"
+    vehicle.vehicle_type = 2  # MAV_TYPE_QUADROTOR
+    vehicle.connected_via = ConnectionType.UDP
+    vehicle.custom_meta['socket_id'] = socket_id
+    
+    # 新增 autopilot 組件 (component_id=1)
+    autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+    autopilot.mav_type = 2  # MAV_TYPE_QUADROTOR
+    autopilot.mav_autopilot = 3  # MAV_AUTOPILOT_ARDUPILOTMEGA
+    
+    # 填充狀態資料
+    status = autopilot.status
+    
+    # 位置
+    status.position.latitude = 25.0330
+    status.position.longitude = 121.5654
+    status.position.altitude = 100.5
+    status.position.relative_altitude = 50.0
+    status.position.timestamp = time.time()
+    
+    # 姿態
+    status.attitude.roll = 0.1
+    status.attitude.pitch = -0.05
+    status.attitude.yaw = 1.57
+    status.attitude.rollspeed = 0.01
+    status.attitude.pitchspeed = 0.02
+    status.attitude.yawspeed = 0.03
+    status.attitude.timestamp = time.time()
+    
+    # 飛行模式
+    status.mode.base_mode = 89
+    status.mode.custom_mode = 4
+    status.mode.mode_name = "GUIDED"
+    status.mode.timestamp = time.time()
+    
+    # 電池
+    status.battery.voltage = 12.6
+    status.battery.current = 15.3
+    status.battery.remaining = 75
+    status.battery.temperature = 35.2
+    status.battery.timestamp = time.time()
+    
+    # GPS
+    status.gps.fix_type = 3  # 3D fix
+    status.gps.satellites_visible = 12
+    status.gps.eph = 100
+    status.gps.epv = 150
+    status.gps.timestamp = time.time()
+    
+    # VFR
+    status.vfr.airspeed = 5.5
+    status.vfr.groundspeed = 6.0
+    status.vfr.heading = 90
+    status.vfr.throttle = 65
+    status.vfr.climb = 1.2
+    status.vfr.timestamp = time.time()
+    
+    # 系統狀態
+    status.armed = True
+    status.system_status = 4  # MAV_STATE_ACTIVE
+    
+    # 更新封包統計
+    autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time())
+    
+    print(f"✓ 載具 {sysid} 已建立並填充測試數據")
+    return vehicle
+
+
+def test_basic_publishing():
+    """測試基本的發布功能"""
+    print("\n" + "="*60)
+    print("測試 1: 基本發布功能")
+    print("="*60)
+    
+    # 清空 registry
+    vehicle_registry.clear()
+    
+    # 建立測試載具
+    vehicle = setup_test_vehicle(sysid=1, socket_id=10)
+    
+    # 初始化 ROS2 管理器
+    if not ros2_manager.initialized:
+        ros2_manager.initialize()
+    
+    # 建立測試訂閱者
+    test_sub = TestSubscriber(sysid=1)
+    
+    # 啟動 publisher
+    ros2_manager.start()
+    
+    print("\n--- 開始發布，等待 5 秒 ---")
+    
+    # 運行 5 秒，持續 spin
+    start_time = time.time()
+    while time.time() - start_time < 5.0:
+        rclpy.spin_once(test_sub, timeout_sec=0.1)
+        time.sleep(0.1)
+    
+    # 檢查收到的消息
+    print("\n--- 消息統計 ---")
+    total_messages = 0
+    for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+        count = test_sub.get_message_count(topic)
+        total_messages += count
+        print(f"  {topic:15s}: {count:3d} 條消息")
+    
+    print(f"\n總計收到: {total_messages} 條消息")
+    
+    # 驗證
+    if total_messages > 0:
+        print("\n✓ 測試通過：成功接收到消息")
+    else:
+        print("\n✗ 測試失敗：沒有接收到任何消息")
+    
+    # 停止
+    ros2_manager.stop()
+    test_sub.destroy_node()
+
+
+def test_frequency_control():
+    """測試頻率控制功能"""
+    print("\n" + "="*60)
+    print("測試 2: 頻率控制")
+    print("="*60)
+    
+    # 清空 registry
+    vehicle_registry.clear()
+    
+    # 建立測試載具
+    vehicle = setup_test_vehicle(sysid=1, socket_id=10)
+    
+    # 初始化（如果還沒初始化）
+    if not ros2_manager.initialized:
+        ros2_manager.initialize()
+    
+    # 建立測試訂閱者
+    test_sub = TestSubscriber(sysid=1)
+
+    # 修改頻率設定
+    publisher_node = ros2_manager.status_publisher
+    publisher_node.rate_controller.topic_intervals = {
+        'position': 1.5,   
+        'attitude': 1.0,    
+        'velocity': 1.0,     
+        'battery': 1.0,   
+        'vfr_hud': 0.5,    
+        'mode': 0.0,       
+        'summary': 0.0,    
+    }
+    
+    # 啟動 publisher
+    ros2_manager.start()
+    
+    print("\n--- 測試頻率控制，運行 3 秒 ---")
+    
+    # 運行 3 秒
+    start_time = time.time()
+    while time.time() - start_time < 3.0:
+        rclpy.spin_once(test_sub, timeout_sec=0.1)
+        time.sleep(0.1)
+    
+    # 檢查頻率
+    print("\n--- 頻率分析 ---")
+    print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
+    
+    print("2Hz Topics (預期 ~6 條):")
+    for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
+        count = test_sub.get_message_count(topic)
+        print(f"  {topic:15s}: {count:3d} 條")
+    for topic in ['battery', 'mode', 'summary']:
+        count = test_sub.get_message_count(topic)
+        print(f"  {topic:15s}: {count:3d} 條")
+    
+    print("\n✓ 頻率控制測試完成")
+    
+    # 停止
+    ros2_manager.stop()
+    test_sub.destroy_node()
+
+
+def test_multi_vehicle():
+    """測試多載具發布"""
+    print("\n" + "="*60)
+    print("測試 3: 多載具發布")
+    print("="*60)
+    
+    # 清空 registry
+    vehicle_registry.clear()
+    
+    # 建立 3 個測試載具
+    v1 = setup_test_vehicle(sysid=1, socket_id=10)
+    v2 = setup_test_vehicle(sysid=2, socket_id=11)
+    v3 = setup_test_vehicle(sysid=3, socket_id=12)
+    
+    # 修改各載具的位置以便區分
+    v2.components[1].status.position.latitude = 26.0
+    v3.components[1].status.position.latitude = 27.0
+    
+    # 初始化
+    if not ros2_manager.initialized:
+        ros2_manager.initialize()
+    
+    # 建立 3 個測試訂閱者
+    test_sub1 = TestSubscriber(sysid=1)
+    test_sub2 = TestSubscriber(sysid=2)
+    test_sub3 = TestSubscriber(sysid=3)
+    
+    # 啟動 publisher
+    ros2_manager.start()
+    
+    print("\n--- 測試多載具，運行 3 秒 ---")
+    
+    # 運行 3 秒
+    start_time = time.time()
+    while time.time() - start_time < 3.0:
+        rclpy.spin_once(test_sub1, timeout_sec=0.05)
+        rclpy.spin_once(test_sub2, timeout_sec=0.05)
+        rclpy.spin_once(test_sub3, timeout_sec=0.05)
+        time.sleep(0.1)
+    
+    # 檢查每個載具的消息
+    print("\n--- 各載具消息統計 ---")
+    for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]:
+        total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+        print(f"載具 {sysid}: {total:3d} 條消息")
+        
+        # 檢查 summary 中的 socket_id
+        if test_sub.get_message_count('summary') > 0:
+            last_summary = test_sub.received_messages['summary'][-1]
+            data = json.loads(last_summary.data)
+            print(f"  └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}")
+    
+    print("\n✓ 多載具測試完成")
+    
+    # 停止
+    ros2_manager.stop()
+    test_sub1.destroy_node()
+    test_sub2.destroy_node()
+    test_sub3.destroy_node()
+
+
+def test_dynamic_vehicle():
+    """測試動態新增/移除載具"""
+    print("\n" + "="*60)
+    print("測試 4: 動態載具管理")
+    print("="*60)
+    
+    # 清空 registry
+    vehicle_registry.clear()
+    
+    # 初始化
+    if not ros2_manager.initialized:
+        ros2_manager.initialize()
+    
+    # 建立測試訂閱者
+    test_sub = TestSubscriber(sysid=1)
+    
+    # 啟動 publisher
+    ros2_manager.start()
+    
+    print("\n--- 階段 1: 無載具，運行 1 秒 ---")
+    start_time = time.time()
+    while time.time() - start_time < 1.0:
+        rclpy.spin_once(test_sub, timeout_sec=0.1)
+        time.sleep(0.1)
+    
+    count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+    print(f"收到消息: {count_before} 條")
+    
+    # 新增載具
+    print("\n--- 階段 2: 新增載具，運行 2 秒 ---")
+    vehicle = setup_test_vehicle(sysid=1, socket_id=10)
+    
+    start_time = time.time()
+    while time.time() - start_time < 2.0:
+        rclpy.spin_once(test_sub, timeout_sec=0.1)
+        time.sleep(0.1)
+    
+    count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+    print(f"收到消息: {count_after - count_before} 條")
+    
+    # 移除載具
+    print("\n--- 階段 3: 移除載具，運行 1 秒 ---")
+    vehicle_registry.unregister(1)
+    
+    start_time = time.time()
+    while time.time() - start_time < 1.0:
+        rclpy.spin_once(test_sub, timeout_sec=0.1)
+        time.sleep(0.1)
+    
+    count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+    print(f"收到消息: {count_final - count_after} 條（應該為 0）")
+    
+    if count_final - count_after == 0:
+        print("\n✓ 動態載具管理測試通過")
+    else:
+        print("\n✗ 移除載具後仍收到消息")
+    
+    # 停止
+    ros2_manager.stop()
+    test_sub.destroy_node()
+
+
+def main():
+    """主測試函數"""
+    print("\n" + "="*60)
+    print("VehicleStatusPublisher 測試程式")
+    print("="*60)
+    
+    try:
+        # 執行各項測試
+        test_basic_publishing()
+        # time.sleep(1)
+        
+        # test_frequency_control()
+        # time.sleep(1)
+        
+        # test_multi_vehicle()
+        # time.sleep(1)
+        
+        # test_dynamic_vehicle()
+        
+        print("\n" + "="*60)
+        print("所有測試完成！")
+        print("="*60)
+        
+    except KeyboardInterrupt:
+        print("\n\n測試被中斷")
+    except Exception as e:
+        print(f"\n\n測試出錯: {e}")
+        import traceback
+        traceback.print_exc()
+    finally:
+        # 清理
+        if ros2_manager.initialized:
+            ros2_manager.shutdown()
+        vehicle_registry.clear()
+        print("\n清理完成")
+
+
+if __name__ == '__main__':
+    main()

src/unitdev01/unitdev01/comm_panel.py

@@ -0,0 +1,398 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+                            QPushButton, QLineEdit)
+from PyQt6.QtCore import pyqtSignal
+
+class CommPanel(QWidget):
+    """通讯设置面板类"""
+    
+    # 定义信号
+    udp_connection_added = pyqtSignal(str, int)  # ip, port
+    udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    udp_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    ws_connection_added = pyqtSignal(str)  # url
+    ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    ws_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    status_message = pyqtSignal(str, int)  # message, timeout
+    
+    def __init__(self, parent=None):
+        super().__init__(parent)
+        self.udp_connections = []
+        self.ws_connections = []
+        self._init_ui()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(10)
+        
+        # ========== UDP MAVLink 區域 ==========
+        udp_title = QLabel("UDP")
+        udp_title.setStyleSheet("""
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(udp_title)
+        
+        # UDP 連接列表容器
+        self.udp_list_widget = QWidget()
+        self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
+        self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.udp_list_layout.setSpacing(5)
+        layout.addWidget(self.udp_list_widget)
+        
+        # UDP 添加新連接區域
+        add_udp_widget = QWidget()
+        add_udp_layout = QHBoxLayout(add_udp_widget)
+        add_udp_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.udp_ip_input = QLineEdit()
+        self.udp_ip_input.setText("127.0.0.1")
+        self.udp_ip_input.setPlaceholderText("IP")
+        self.udp_ip_input.setStyleSheet("""
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        self.udp_port_input = QLineEdit()
+        self.udp_port_input.setText("14540")
+        self.udp_port_input.setPlaceholderText("Port")
+        self.udp_port_input.setFixedWidth(80)
+        self.udp_port_input.setStyleSheet("""
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        add_udp_btn = QPushButton("添加")
+        add_udp_btn.clicked.connect(self._handle_add_udp)
+        add_udp_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
+        add_udp_layout.addWidget(self.udp_ip_input)
+        add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+        add_udp_layout.addWidget(self.udp_port_input)
+        add_udp_layout.addWidget(add_udp_btn)
+        
+        layout.addWidget(add_udp_widget)
+        
+        # 分隔線
+        separator = QWidget()
+        separator.setFixedHeight(20)
+        layout.addWidget(separator)
+        
+        # ========== WebSocket 區域 ==========
+        ws_title = QLabel("WebSocket")
+        ws_title.setStyleSheet("""
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(ws_title)
+        
+        # WebSocket 連接列表容器
+        self.ws_list_widget = QWidget()
+        self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
+        self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.ws_list_layout.setSpacing(5)
+        layout.addWidget(self.ws_list_widget)
+        
+        # WebSocket 添加新連接區域
+        add_ws_widget = QWidget()
+        add_ws_layout = QHBoxLayout(add_ws_widget)
+        add_ws_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.ws_url_input = QLineEdit()
+        self.ws_url_input.setPlaceholderText("host")
+        self.ws_url_input.setStyleSheet("""
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        add_ws_btn = QPushButton("添加")
+        add_ws_btn.clicked.connect(self._handle_add_ws)
+        add_ws_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
+        add_ws_layout.addWidget(self.ws_url_input)
+        add_ws_layout.addWidget(add_ws_btn)
+        
+        layout.addWidget(add_ws_widget)
+        layout.addStretch()
+    
+    def _handle_add_udp(self):
+        """處理添加 UDP 連接"""
+        ip = self.udp_ip_input.text().strip()
+        port_text = self.udp_port_input.text().strip()
+        
+        if not ip or not port_text:
+            self.status_message.emit("請輸入 IP 和 Port", 3000)
+            return
+        
+        try:
+            port = int(port_text)
+            if port < 1 or port > 65535:
+                raise ValueError("Port 超出範圍")
+        except ValueError:
+            self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.udp_connections:
+            if conn['ip'] == ip and conn['port'] == port:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.udp_connection_added.emit(ip, port)
+        
+        # 清空輸入框
+        self.udp_ip_input.clear()
+        self.udp_port_input.clear()
+    
+    def _handle_add_ws(self):
+        """處理添加 WebSocket 連接"""
+        input_url = self.ws_url_input.text().strip()
+        
+        if not input_url:
+            self.status_message.emit("請輸入 WebSocket URL", 3000)
+            return
+        
+        # 自動添加 ws:// 前綴
+        if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+            url = f'ws://{input_url}'
+        else:
+            url = input_url
+        
+        # 基本 URL 格式驗證
+        try:
+            if '://' in url:
+                parts = url.split('://', 1)
+                if len(parts) == 2 and ':' not in parts[1]:
+                    self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+                    return
+        except:
+            self.status_message.emit("URL 格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.ws_connections:
+            if conn['url'] == url:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.ws_connection_added.emit(url)
+        
+        # 清空輸入框
+        self.ws_url_input.clear()
+    
+    def create_udp_connection_panel(self, conn):
+        """創建 UDP 連接面板"""
+        panel = QWidget()
+        panel.setStyleSheet("""
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            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))
+        toggle_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
+        remove_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def create_ws_connection_panel(self, conn):
+        """創建 WebSocket 連接面板"""
+        panel = QWidget()
+        panel.setStyleSheet("""
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['url']}")
+        info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            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))
+        toggle_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
+        remove_btn.setStyleSheet("""
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def add_udp_panel(self, conn):
+        """添加 UDP 連接面板到列表"""
+        panel = self.create_udp_connection_panel(conn)
+        self.udp_list_layout.addWidget(panel)
+        self.udp_connections.append(conn)
+        return panel
+    
+    def add_ws_panel(self, conn):
+        """添加 WebSocket 連接面板到列表"""
+        panel = self.create_ws_connection_panel(conn)
+        self.ws_list_layout.addWidget(panel)
+        self.ws_connections.append(conn)
+        return panel
+    
+    def remove_udp_connection_from_list(self, conn):
+        """從列表中移除 UDP 連接"""
+        if conn in self.udp_connections:
+            self.udp_connections.remove(conn)
+    
+    def remove_ws_connection_from_list(self, conn):
+        """從列表中移除 WebSocket 連接"""
+        if conn in self.ws_connections:
+            self.ws_connections.remove(conn)

src/unitdev01/unitdev01/communication.py

@@ -0,0 +1,646 @@
+from rclpy.node import Node
+from PyQt6.QtCore import QObject, pyqtSignal
+import math
+import re
+import threading
+from threading import Lock
+import asyncio
+import websockets
+import json
+import socket
+from pymavlink import mavutil
+from geometry_msgs.msg import Point, Vector3
+from sensor_msgs.msg import BatteryState, NavSatFix, Imu
+from std_msgs.msg import Float64
+from mavros_msgs.msg import State, VfrHud
+from mavros_msgs.srv import CommandBool, CommandTOL
+
+class DroneSignals(QObject):
+    update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
+
+class UDPMavlinkReceiver(threading.Thread):
+    """UDP MAVLink 接收器"""
+    def __init__(self, ip, port, signals, connection_name):
+        super().__init__(daemon=True)
+        self.ip = ip
+        self.port = port
+        self.signals = signals
+        self.connection_name = connection_name
+        self.running = False
+        self.sock = None
+        
+    def run(self):
+        """執行 UDP 接收循環"""
+        self.running = True
+        try:
+            print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
+            
+            # 創建 MAVLink 連接
+            mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
+            while self.running:
+                try:
+                    msg = mav.recv_match(blocking=True, timeout=1.0)
+                    if msg is None:
+                        continue
+                    
+                    self.process_mavlink_message(msg)
+                    
+                except socket.timeout:
+                    continue
+                except Exception as e:
+                    print(f"Error receiving MAVLink message: {e}")
+                    
+        except Exception as e:
+            print(f"UDP receiver error: {e}")
+        finally:
+            if self.sock:
+                self.sock.close()
+                
+    def process_mavlink_message(self, msg):
+        """處理 MAVLink 訊息"""
+        try:
+            msg_type = msg.get_type()
+            system_id = msg.get_srcSystem()
+            drone_id = f"s8_{system_id}"  # 使用 s8_ 前綴表示 UDP 來源
+            
+            if msg_type == "HEARTBEAT":
+                mode = mavutil.mode_string_v10(msg)
+                armed = bool(msg.base_mode & 128)
+                self.signals.update_signal.emit('state', drone_id, {
+                    'mode': mode,
+                    'armed': armed
+                })
+
+            elif msg_type == "BATTERY_STATUS":
+                voltage = msg.voltages[0] / 1000
+                self.signals.update_signal.emit('battery', drone_id, {
+                    'voltage': voltage
+                })
+
+            elif msg_type == "GLOBAL_POSITION_INT":
+                latitude = msg.lat / 1e7
+                longitude = msg.lon / 1e7
+                self.signals.update_signal.emit('gps', drone_id, {
+                    'lat': latitude,
+                    'lon': longitude
+                })
+
+            elif msg_type == "GPS_RAW_INT":
+                fix_type = msg.fix_type
+
+            elif msg_type == "LOCAL_POSITION_NED":
+                x = msg.y
+                y = msg.x
+                z = -msg.z
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': x,
+                    'y': y,
+                    'z': z
+                })
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': z
+                })
+                self.signals.update_signal.emit('velocity', drone_id, {
+                    'vx': msg.vx,
+                    'vy': msg.vy,
+                    'vz': msg.vz
+                })
+
+            elif msg_type == "ATTITUDE":
+                pitch = math.degrees(msg.pitch)
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'pitch': pitch,
+                    'roll': 0,
+                    'yaw': 0,
+                    'rates': (0, 0, 0)
+                })
+
+            elif msg_type == "VFR_HUD":
+                groundspeed = msg.groundspeed
+                heading = msg.heading
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': heading,
+                    'groundspeed': groundspeed
+                })
+                
+        except Exception as e:
+            print(f"Error processing MAVLink message: {e}")
+            
+    def stop(self):
+        """停止接收器"""
+        self.running = False
+
+class SerialMavlinkReceiver(threading.Thread):
+    """串口 MAVLink 接收器"""
+    def __init__(self, port, baudrate, signals, connection_name):
+        super().__init__(daemon=True)
+        self.port = port
+        self.baudrate = baudrate
+        self.signals = signals
+        self.connection_name = connection_name
+        self.running = False
+        self.mav = None
+        
+    def run(self):
+        """執行串口接收循環"""
+        self.running = True
+        try:
+            print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
+            
+            # 創建 MAVLink 串口連接
+            self.mav = mavutil.mavlink_connection(
+                self.port,
+                baud=self.baudrate,
+                source_system=255
+            )
+            
+            print(f"Waiting for heartbeat from {self.port}...")
+            self.mav.wait_heartbeat()
+            print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
+            
+            while self.running:
+                try:
+                    msg = self.mav.recv_match(blocking=True, timeout=1.0)
+                    if msg is None:
+                        continue
+                    
+                    self.process_mavlink_message(msg)
+                    
+                except Exception as e:
+                    if self.running:
+                        print(f"Error receiving MAVLink message from serial: {e}")
+                    
+        except Exception as e:
+            print(f"Serial receiver error: {e}")
+        finally:
+            if self.mav:
+                try:
+                    self.mav.close()
+                except:
+                    pass
+                
+    def process_mavlink_message(self, msg):
+        """處理 MAVLink 訊息"""
+        try:
+            msg_type = msg.get_type()
+            system_id = msg.get_srcSystem()
+            drone_id = f"s5_{system_id}"  # 使用 serial_ 前綴表示串口來源
+            
+            if msg_type == "HEARTBEAT":
+                mode = mavutil.mode_string_v10(msg)
+                armed = bool(msg.base_mode & 128)
+                self.signals.update_signal.emit('state', drone_id, {
+                    'mode': mode,
+                    'armed': armed
+                })
+
+            elif msg_type == "BATTERY_STATUS":
+                voltage = msg.voltages[0] / 1000
+                self.signals.update_signal.emit('battery', drone_id, {
+                    'voltage': voltage
+                })
+
+            elif msg_type == "GLOBAL_POSITION_INT":
+                latitude = msg.lat / 1e7
+                longitude = msg.lon / 1e7
+                self.signals.update_signal.emit('gps', drone_id, {
+                    'lat': latitude,
+                    'lon': longitude
+                })
+
+            elif msg_type == "GPS_RAW_INT":
+                fix_type = msg.fix_type
+
+            elif msg_type == "LOCAL_POSITION_NED":
+                x = msg.y
+                y = msg.x
+                z = -msg.z
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': x,
+                    'y': y,
+                    'z': z
+                })
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': z
+                })
+                self.signals.update_signal.emit('velocity', drone_id, {
+                    'vx': msg.vx,
+                    'vy': msg.vy,
+                    'vz': msg.vz
+                })
+
+            elif msg_type == "ATTITUDE":
+                pitch = math.degrees(msg.pitch)
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'pitch': pitch,
+                    'roll': 0,
+                    'yaw': 0,
+                    'rates': (0, 0, 0)
+                })
+
+            elif msg_type == "VFR_HUD":
+                groundspeed = msg.groundspeed
+                heading = msg.heading
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': heading,
+                    'groundspeed': groundspeed
+                })
+                
+        except Exception as e:
+            print(f"Error processing MAVLink message from serial: {e}")
+            
+    def stop(self):
+        """停止接收器"""
+        self.running = False
+
+class WebSocketMavlinkReceiver(threading.Thread):
+    """WebSocket MAVLink 接收器"""
+    def __init__(self, url, signals, connection_name):
+        super().__init__(daemon=True)
+        self.url = url
+        self.signals = signals
+        self.connection_name = connection_name
+        self.running = False
+        self.max_retries = 5
+        self.base_delay = 1.0
+        
+    def run(self):
+        """執行 WebSocket 接收循環"""
+        self.running = True
+        asyncio.set_event_loop(asyncio.new_event_loop())
+        asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
+    
+    async def ws_client_loop(self):
+        """WebSocket 連接的主循環"""
+        retry_count = 0
+        
+        print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
+        
+        while self.running and retry_count < self.max_retries:
+            try:
+                async with websockets.connect(self.url) as websocket:
+                    print(f"WebSocket {self.connection_name} connected to {self.url}")
+                    retry_count = 0  # 重置重試計數
+                    
+                    async for message in websocket:
+                        if not self.running:
+                            break
+                            
+                        try:
+                            data = json.loads(message)
+                            data['_connection_source'] = self.connection_name
+                            self.process_websocket_message(data)
+                        except json.JSONDecodeError as e:
+                            print(f"WebSocket {self.connection_name} JSON decode error: {e}")
+                        except Exception as e:
+                            print(f"WebSocket {self.connection_name} message processing error: {e}")
+                            
+            except websockets.exceptions.ConnectionClosedError:
+                print(f"WebSocket {self.connection_name} connection closed")
+                if self.running:
+                    retry_count += 1
+                    if retry_count < self.max_retries:
+                        delay = self.base_delay * (2 ** min(retry_count, 4))
+                        print(f"Reconnecting in {delay}s...")
+                        await asyncio.sleep(delay)
+                else:
+                    break
+            except Exception as e:
+                retry_count += 1
+                if retry_count < self.max_retries and self.running:
+                    delay = self.base_delay * (2 ** min(retry_count, 4))
+                    print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
+                    await asyncio.sleep(delay)
+                else:
+                    break
+        
+        print(f"WebSocket client {self.connection_name} stopped")
+    
+    def process_websocket_message(self, data):
+        """處理 WebSocket 訊息"""
+        try:
+            drone_id = data.get('system_id')
+            drone_id = f"s9_{drone_id}" if drone_id else None
+            if not drone_id:
+                return
+
+            # 模式
+            if 'mode' in data:
+                self.signals.update_signal.emit('state', drone_id, {
+                    'mode': data['mode'],
+                })
+
+            # 電池
+            if 'battery' in data:
+                self.signals.update_signal.emit('battery', drone_id, {
+                    'percentage': data['battery']  
+                })
+
+            # 位置
+            if 'position' in data:
+                pos = data['position']
+                self.signals.update_signal.emit('gps', drone_id, {
+                    'lat': pos.get('lat', 0),
+                    'lon': pos.get('lon', 0)
+                })
+
+                # Local position - 設定 x, y 為 0.0
+                self.signals.update_signal.emit('local_pose', drone_id, {
+                    'x': 0.0,
+                    'y': 0.0,
+                    'z': 0.0
+                })
+                
+                # Altitude - 設定為 0.0
+                self.signals.update_signal.emit('altitude', drone_id, {
+                    'altitude': 0.0
+                })
+
+            # 航向
+            if 'heading' in data:
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'heading': data['heading'],
+                    'groundspeed': 0.0
+                })
+            
+        except Exception as e:
+            print(f"WebSocket message processing error: {e}")
+            
+    def stop(self):
+        """停止接收器"""
+        self.running = False
+
+class DroneMonitor(Node):
+    def __init__(self):
+        super().__init__('drone_monitor')
+        self.signals = DroneSignals()
+        self.drone_topics = {}
+        self.lock = Lock()
+        
+        self.arm_clients = {}
+        self.takeoff_clients = {}
+        self.setpoint_pubs = {}
+        self.selected_drones = set()
+        self.latest_data = {}
+
+        # 定義需要過濾的模式
+        self.filtered_modes = ['Mode(0x000000c0)']
+
+        # WebSocket 接收器列表
+        self.ws_receivers = []
+        
+        # 串口接收器列表
+        self.serial_receivers = []
+        
+        # 主题检测定时器
+        self.create_timer(1.0, self.scan_topics)
+       
+    def scan_topics(self):
+        topics = self.get_topic_names_and_types()
+        drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
+        
+        found_drones = set()
+        for topic_name, _ in topics:
+            if match := drone_pattern.match(topic_name):
+                drone_id, topic_type = match.groups()
+                found_drones.add(drone_id)
+                with self.lock:
+                    self.drone_topics.setdefault(drone_id, set()).add(topic_type)
+        
+        for drone_id in found_drones:
+            if not hasattr(self, f'drone_{drone_id}_subs'):
+                self.setup_drone(drone_id)
+    
+    def setup_drone(self, drone_id):
+        base_topic = f'/MavLinkBus/{drone_id}'
+        
+        # Add service clients
+        self.arm_clients[drone_id] = self.create_client(
+            CommandBool,
+            f'{base_topic}/cmd/arming'
+        )
+        self.takeoff_clients[drone_id] = self.create_client(
+            CommandTOL,
+            f'{base_topic}/cmd/takeoff'
+        )
+
+        # Add setpoint publisher
+        self.setpoint_pubs[drone_id] = self.create_publisher(
+            Point,
+            f'{base_topic}/setpoint_position/local',
+            10
+        )
+
+        subs = {
+            'attitude': self.create_subscription(
+                Imu,
+                f'{base_topic}/attitude',
+                lambda msg, did=drone_id: self.attitude_callback(did, msg),
+                10
+            ),
+            'battery': self.create_subscription(
+                BatteryState,
+                f'{base_topic}/battery',
+                lambda msg, did=drone_id: self.battery_callback(did, msg),
+                10
+            ),
+            'global': self.create_subscription(
+                NavSatFix,
+                f'{base_topic}/global_position/global',
+                lambda msg, did=drone_id: self.gps_callback(did, msg),
+                10
+            ),
+            'rel_alt': self.create_subscription(
+                Float64,
+                f'{base_topic}/global_position/rel_alt',
+                lambda msg, did=drone_id: self.altitude_callback(did, msg),
+                10
+            ),
+            'local_pose': self.create_subscription(
+                Point,
+                f'{base_topic}/local_position/pose',
+                lambda msg, did=drone_id: self.local_pose_callback(did, msg),
+                10
+            ),
+            'local_vel': self.create_subscription(
+                Vector3,
+                f'{base_topic}/local_position/velocity',
+                lambda msg, did=drone_id: self.local_vel_callback(did, msg),
+                10
+            ),
+            'loss_rate': self.create_subscription(
+                Float64,
+                f'{base_topic}/packet_loss_rate',
+                lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
+                10
+            ),
+            'state': self.create_subscription(
+                State,
+                f'{base_topic}/state',
+                lambda msg, did=drone_id: self.state_callback(did, msg),
+                10
+            ),
+            'ping': self.create_subscription(
+                Float64,
+                f'{base_topic}/ping',
+                lambda msg, did=drone_id: self.ping_callback(did, msg),
+                10
+            ),
+            'vfr_hud': self.create_subscription(
+                VfrHud,
+                f'{base_topic}/vfr_hud',
+                lambda msg, did=drone_id: self.hud_callback(did, msg),
+                10
+            )
+        }
+        
+        setattr(self, f'drone_{drone_id}_subs', subs)
+
+    async def arm_drone(self, drone_id, arm):
+        if drone_id not in self.arm_clients:
+            return False
+        
+        client = self.arm_clients[drone_id]
+        if not client.wait_for_service(timeout_sec=1.0):
+            return False
+            
+        request = CommandBool.Request()
+        request.value = arm
+        
+        future = client.call_async(request)
+        try:
+            response = await future
+            return response.success
+        except Exception as e:
+            self.get_logger().error(f'Arm service call failed: {e}')
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude=10.0):
+        if drone_id not in self.takeoff_clients:
+            return False
+            
+        client = self.takeoff_clients[drone_id]
+        if not client.wait_for_service(timeout_sec=1.0):
+            return False
+            
+        request = CommandTOL.Request()
+        request.altitude = altitude
+        request.min_pitch = 0.0
+        request.yaw = 0.0
+        
+        future = client.call_async(request)
+        try:
+            response = await future
+            return response.success
+        except Exception as e:
+            self.get_logger().error(f'Takeoff service call failed: {e}')
+            return False
+
+    def send_setpoint(self, drone_id, x, y, z):
+        """Send setpoint position command"""
+        if drone_id not in self.setpoint_pubs:
+            return False
+            
+        msg = Point()
+        msg.x = float(x)
+        msg.y = float(y)
+        msg.z = float(z)
+        
+        self.setpoint_pubs[drone_id].publish(msg)
+        return True
+    
+    def quaternion_to_euler(self, q):
+        sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
+        cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
+        roll = math.atan2(sinr_cosp, cosr_cosp)
+        
+        sinp = 2 * (q.w * q.y - q.z * q.x)
+        pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
+        
+        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
+        yaw = math.atan2(siny_cosp, cosy_cosp)
+        
+        return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
+
+    # callbacks
+    def attitude_callback(self, drone_id, msg):
+        if hasattr(msg, 'orientation'):
+            roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
+            self.latest_data[(drone_id, 'attitude')] = {
+                'roll': roll,
+                'pitch': pitch,
+                'yaw': yaw,
+                'rates': (msg.angular_velocity.x, 
+                         msg.angular_velocity.y,
+                         msg.angular_velocity.z)
+            }
+
+    def battery_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'battery')] = {
+            'voltage': msg.voltage
+        }
+
+    def state_callback(self, drone_id, msg):
+        mode = msg.mode
+        if mode in self.filtered_modes:
+            return
+        self.latest_data[(drone_id, 'state')] = {
+            'mode': msg.mode,
+            'armed': msg.armed
+        }
+
+    def gps_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'gps')] = {
+            'lat': msg.latitude,
+            'lon': msg.longitude,
+            'alt': msg.altitude
+        }
+
+    def local_vel_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'velocity')] = {
+            'vx': msg.x,
+            'vy': msg.y,
+            'vz': msg.z
+        }
+
+    def altitude_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'altitude')] = {
+            'altitude': msg.data
+        }
+
+    def local_pose_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'local_pose')] = {
+            'x': msg.x,
+            'y': msg.y,
+            'z': msg.z
+        }
+
+    def hud_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'hud')] = {
+            'airspeed': msg.airspeed,
+            'groundspeed': msg.groundspeed,
+            'heading': msg.heading,
+            'throttle': msg.throttle,
+            'alt': msg.altitude,
+            'climb': msg.climb
+        }
+
+    def loss_rate_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'loss_rate')] = {
+            'loss_rate': msg.data
+        }
+
+    def ping_callback(self, drone_id, msg):
+        self.latest_data[(drone_id, 'ping')] = {
+            'ping': msg.data
+        }
+
+    def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
+        """啟動串口 MAVLink 連接"""
+        connection_name = f"Serial_{port.replace('/', '_')}"
+        receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name)
+        receiver.start()
+        self.serial_receivers.append(receiver)
+        print(f"Started serial connection on {port} at {baudrate} baud")
+        return receiver

src/unitdev01/unitdev01/drone_panel.py

@@ -0,0 +1,378 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+                            QPushButton, QSizePolicy, QCheckBox)
+from PyQt6.QtCore import pyqtSignal
+
+class DronePanel(QWidget):
+    """單個無人機面板類別"""
+    
+    # 定義信號
+    mode_change_requested = pyqtSignal(str)  # drone_id
+    arm_requested = pyqtSignal(str)  # drone_id
+    takeoff_requested = pyqtSignal(str)  # drone_id
+    setpoint_requested = pyqtSignal(str)  # drone_id
+    selection_changed = pyqtSignal(str, int)  # drone_id, state
+    
+    def __init__(self, drone_id, parent=None):
+        super().__init__(parent)
+        self.drone_id = drone_id
+        self.display_id = 's' + drone_id.split('_')[1]
+        self._init_ui()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        self.setObjectName(f"panel_{self.drone_id}")
+        self.setFixedHeight(140)
+        self.setStyleSheet("""
+            background-color: #2A2A2A;
+            border-radius: 8px;
+        """)
+        
+        # 主佈局
+        main_layout = QHBoxLayout(self)
+        main_layout.setContentsMargins(8, 8, 8, 8)
+        main_layout.setSpacing(0)
+        
+        # 創建內容容器（包含 info 和 control）
+        content_widget = QWidget()
+        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)
+        
+        # 左側資訊區域
+        info_widget = self._create_info_section()
+        
+        # 右側控制按鈕區域
+        control_widget = self._create_control_section()
+        
+        # 將 info 和 control 加入內容容器
+        content_layout.addWidget(info_widget)
+        content_layout.addWidget(control_widget)
+        
+        # 將內容容器加入主佈局
+        main_layout.addWidget(content_widget)
+    
+    def _create_info_section(self):
+        """創建資訊顯示區域"""
+        info_widget = QWidget()
+        info_layout = QVBoxLayout(info_widget)
+        info_layout.setContentsMargins(0, 0, 0, 0)
+        info_layout.setSpacing(4)
+        
+        # 頂部標題欄
+        header = QWidget()
+        header_layout = QHBoxLayout(header)
+        header_layout.setContentsMargins(0, 0, 0, 0)
+        
+        # 勾選框
+        self.checkbox = QCheckBox()
+        self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
+        self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }")
+        self.checkbox.stateChanged.connect(
+            lambda state: self.selection_changed.emit(self.drone_id, state)
+        )
+        
+        # ID 顯示
+        id_label = QLabel(self.display_id)
+        id_label.setStyleSheet("""
+            font-weight: bold; 
+            font-size: 14px; 
+            color: #7FFFD4;
+            min-width: 80px;
+        """)
+        
+        header_layout.addWidget(self.checkbox)
+        header_layout.addWidget(id_label)
+        header_layout.addStretch()
+        
+        info_layout.addWidget(header)
+        
+        # 第一行：狀態 (模式 + ARM狀態)
+        status_row = self._create_status_row()
+        info_layout.addWidget(status_row)
+        
+        # 第二行：電池
+        battery_row = self._create_battery_row()
+        info_layout.addWidget(battery_row)
+        
+        # 第三行：位置 + 高度
+        position_row = self._create_position_row()
+        info_layout.addWidget(position_row)
+        
+        # 第四行：航向 + 速度
+        nav_row = self._create_nav_row()
+        info_layout.addWidget(nav_row)
+        
+        return info_widget
+    
+    def _create_status_row(self):
+        """創建狀態行"""
+        status_row = QWidget()
+        status_layout = QHBoxLayout(status_row)
+        status_layout.setContentsMargins(0, 0, 0, 0)
+        
+        status_title = QLabel("狀態：")
+        status_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        self.mode_label = QLabel("--")
+        self.mode_label.setObjectName(f"{self.drone_id}_mode")
+        self.mode_label.setStyleSheet("color: #DDD;")
+        
+        self.armed_label = QLabel("--")
+        self.armed_label.setObjectName(f"{self.drone_id}_armed")
+        self.armed_label.setStyleSheet("color: #DDD;")
+        
+        status_layout.addWidget(status_title)
+        status_layout.addWidget(self.mode_label)
+        status_layout.addWidget(self.armed_label)
+        status_layout.addStretch()
+        
+        return status_row
+    
+    def _create_battery_row(self):
+        """創建電池行"""
+        battery_row = QWidget()
+        battery_layout = QHBoxLayout(battery_row)
+        battery_layout.setContentsMargins(0, 0, 0, 0)
+        
+        battery_title = QLabel("電池：")
+        battery_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        self.battery_label = QLabel("--")
+        self.battery_label.setObjectName(f"{self.drone_id}_battery")
+        self.battery_label.setStyleSheet("color: #DDD;")
+        
+        battery_layout.addWidget(battery_title)
+        battery_layout.addWidget(self.battery_label)
+        battery_layout.addStretch()
+        
+        return battery_row
+    
+    def _create_position_row(self):
+        """創建位置行"""
+        position_row = QWidget()
+        position_layout = QHBoxLayout(position_row)
+        position_layout.setContentsMargins(0, 0, 0, 0)
+        
+        position_title = QLabel("位置：")
+        position_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        self.local_label = QLabel("--")
+        self.local_label.setObjectName(f"{self.drone_id}_local")
+        self.local_label.setStyleSheet("color: #DDD;")
+        
+        altitude_title = QLabel("高度：")
+        altitude_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        self.altitude_label = QLabel("--")
+        self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
+        self.altitude_label.setStyleSheet("color: #DDD;")
+        
+        position_layout.addWidget(position_title)
+        position_layout.addWidget(self.local_label)
+        position_layout.addWidget(altitude_title)
+        position_layout.addWidget(self.altitude_label)
+        position_layout.addStretch()
+        
+        return position_row
+    
+    def _create_nav_row(self):
+        """創建導航行"""
+        nav_row = QWidget()
+        nav_layout = QHBoxLayout(nav_row)
+        nav_layout.setContentsMargins(0, 0, 0, 0)
+        
+        heading_title = QLabel("航向：")
+        heading_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        self.heading_label = QLabel("--")
+        self.heading_label.setObjectName(f"{self.drone_id}_heading")
+        self.heading_label.setStyleSheet("color: #DDD;")
+        
+        speed_title = QLabel("速度：")
+        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        self.groundspeed_label = QLabel("--")
+        self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed")
+        self.groundspeed_label.setStyleSheet("color: #DDD;")
+        
+        nav_layout.addWidget(heading_title)
+        nav_layout.addWidget(self.heading_label)
+        nav_layout.addWidget(speed_title)
+        nav_layout.addWidget(self.groundspeed_label)
+        nav_layout.addStretch()
+        
+        return nav_row
+    
+    def _create_control_section(self):
+        """創建控制按鈕區域"""
+        control_widget = QWidget()
+        control_layout = QVBoxLayout(control_widget)
+        control_layout.setContentsMargins(0, 0, 0, 0)
+        control_layout.setSpacing(6)
+        
+        control_widget.setFixedWidth(80)
+        
+        btn_style = """
+	        QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                border-radius: 4px;
+                font-size: 11px;
+            }
+            QPushButton:hover {
+                background-color: #555;
+            }
+        """
+        # 模式切換按鈕
+        mode_btn = QPushButton("切換模式")
+        mode_btn.setStyleSheet(btn_style)
+        mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id))
+        
+        # 解鎖按鈕
+        arm_btn = QPushButton("解鎖")
+        arm_btn.setStyleSheet(btn_style)
+        arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id))
+        
+        # 起飛按鈕
+        takeoff_btn = QPushButton("起飛")
+        takeoff_btn.setStyleSheet(btn_style)
+        takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id))
+        
+        # Setpoint 按鈕
+        setpoint_btn = QPushButton("Setpoint")
+        setpoint_btn.setStyleSheet(btn_style)
+        setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+        setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id))
+        
+        control_layout.addWidget(mode_btn)
+        control_layout.addWidget(arm_btn)
+        control_layout.addWidget(takeoff_btn)
+        control_layout.addWidget(setpoint_btn)
+        
+        return control_widget
+    
+    def update_field(self, field, text, color=None):
+        """更新指定欄位的值"""
+        label = self.findChild(QLabel, f"{self.drone_id}_{field}")
+        if label and label.text() != text:
+            label.setText(text)
+            if color:
+                label.setStyleSheet(f"color: {color};")
+    
+    def get_checkbox(self):
+        """獲取勾選框"""
+        return self.checkbox
+    
+    def set_checked(self, checked):
+        """設置勾選狀態"""
+        self.checkbox.setChecked(checked)
+    
+    def is_checked(self):
+        """獲取勾選狀態"""
+        return self.checkbox.isChecked()
+    
+class SocketGroupPanel(QWidget):
+    # 定義信號
+    group_selection_changed = pyqtSignal(str, int)  # socket_id, state
+    
+    def __init__(self, socket_id, color='#AAAAAA', parent=None):
+        super().__init__(parent)
+        self.socket_id = socket_id
+        self.color = color
+        self._init_ui()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        self.setObjectName(f"socket_group_{self.socket_id}")
+        self.setStyleSheet("""
+            background-color: #1E1E1E;
+            border-radius: 12px;
+        """)
+        
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(6)
+        
+        # Socket 分組標題行 - 包含勾選框
+        title_row = QWidget()
+        title_layout = QHBoxLayout(title_row)
+        title_layout.setContentsMargins(0, 0, 0, 0)
+        
+        # 分組勾選框
+        self.group_checkbox = QCheckBox()
+        self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
+        self.group_checkbox.setStyleSheet(f"""
+            QCheckBox {{ color: #DDD; }}
+            QCheckBox::indicator {{
+                width: 14px;
+                height: 14px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }}
+            QCheckBox::indicator:checked {{
+                background-color: {self.color};
+                border: 2px solid #888888;
+            }}
+            QCheckBox::indicator:indeterminate {{
+                background-color: #666;
+                border: 2px solid #888888;
+            }}
+        """)
+        self.group_checkbox.stateChanged.connect(
+            lambda state: self.group_selection_changed.emit(self.socket_id, state)
+        )
+        
+        # Socket 分組標題
+        title_label = QLabel(f"Socket {self.socket_id}")
+        title_label.setStyleSheet(f"""
+            font-weight: bold; 
+            font-size: 16px; 
+            color: {self.color};
+            margin-bottom: 8px;
+            padding: 4px 8px;
+            border-radius: 6px;
+        """)
+        
+        title_layout.addWidget(self.group_checkbox)
+        title_layout.addWidget(title_label)
+        title_layout.addStretch()
+        
+        layout.addWidget(title_row)
+        
+        # 創建子容器用於放置該 socket 下的所有無人機面板
+        self.drones_container = QWidget()
+        self.drones_layout = QVBoxLayout(self.drones_container)
+        self.drones_layout.setContentsMargins(0, 0, 0, 0)
+        self.drones_layout.setSpacing(4)
+        
+        layout.addWidget(self.drones_container)
+    
+    def add_drone_panel(self, panel):
+        """添加無人機面板到分組"""
+        self.drones_layout.addWidget(panel)
+    
+    def clear_drones(self):
+        """清空所有無人機面板"""
+        while self.drones_layout.count():
+            item = self.drones_layout.takeAt(0)
+            if item.widget():
+                item.widget().setParent(None)
+    
+    def get_checkbox(self):
+        """獲取分組勾選框"""
+        return self.group_checkbox
+    
+    def set_checked(self, checked):
+        """設置分組勾選狀態"""
+        self.group_checkbox.setChecked(checked)
+    
+    def set_check_state(self, state):
+        """設置分組勾選狀態（支持半選）"""
+        self.group_checkbox.setCheckState(state)

src/unitdev01/unitdev01/map_layout.py

@@ -0,0 +1,292 @@
+#!/usr/bin/env python3
+from PyQt6.QtWebEngineWidgets import QWebEngineView
+from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
+from PyQt6.QtWebChannel import QWebChannel
+
+class DroneMap:
+    """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
+    
+    def __init__(self):
+        """初始化地圖"""
+        self.map_view = QWebEngineView()
+        self.map_loaded = False
+        self.pending_map_updates = {}
+        
+        # 創建橋接對象
+        self.bridge = MapBridge()
+        
+        # 設置 QWebChannel
+        self.channel = QWebChannel()
+        self.channel.registerObject('bridge', self.bridge)
+        self.map_view.page().setWebChannel(self.channel)
+        
+        # 設置地圖 HTML
+        inline_html = '''
+            <!DOCTYPE html>
+            <html>
+            <head>
+            <meta charset="utf-8" />
+            <link rel="stylesheet" href="https://unpkg.com/leaflet/dist/leaflet.css" />
+            <script src="https://unpkg.com/leaflet/dist/leaflet.js"></script>
+            <script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
+            <script src="qrc:///qtwebchannel/qwebchannel.js"></script>
+            <style>
+                html, body, #map { height: 100%; margin: 0; }
+                .map-controls {
+                    position: absolute;
+                    top: 10px;
+                    right: 10px;
+                    z-index: 1000;
+                }
+                .control-button {
+                    padding: 8px 12px;
+                    background-color: #f44336;
+                    color: white;
+                    border: none;
+                    border-radius: 4px;
+                    cursor: pointer;
+                    font-size: 12px;
+                    box-shadow: 0 2px 5px rgba(0,0,0,0.2);
+                }
+                .control-button:hover {
+                    background-color: #d32f2f;
+                }
+            </style>
+            </head>
+            <body>
+            <div id="map"></div>
+            <div class="map-controls">
+                <button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
+            </div>
+
+            <script>
+                var bridge;
+                new QWebChannel(qt.webChannelTransport, function(channel) {
+                    bridge = channel.objects.bridge;
+                });
+
+                var map = L.map('map').setView([0, 0], 19); 
+                L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
+                    maxZoom: 19,
+                    attribution: '© OpenStreetMap contributors'
+                }).addTo(map);
+
+                // 地圖點擊事件
+                map.on('click', function(e) {
+                    if (bridge) {
+                        bridge.emitGpsSignal(e.latlng.lat, e.latlng.lng);
+                        console.log('點擊位置:', e.latlng.lat, e.latlng.lng);
+                    }
+                });
+
+                var arrowIcon = L.icon({
+                    iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png',
+                    iconSize: [40, 40],
+                    iconAnchor: [20, 20]
+                });
+
+                function getColorBySocketId(id) {
+                    if (id.startsWith("s0_")) return "#00BFFF";  // 天藍色
+                    if (id.startsWith("s1_")) return "#FFD700";  // 金色
+                    if (id.startsWith("s2_")) return "#FF6969";  // 淺紅色
+                    if (id.startsWith("s3_")) return "#FF69B4";  // 熱粉紅
+                    if (id.startsWith("s4_")) return "#00FA9A";  // 中春綠
+                    if (id.startsWith("s5_")) return "#9370DB";  // 中紫色 (串口)
+                    if (id.startsWith("s6_")) return "#FFA500";  // 橙色
+                    if (id.startsWith("s7_")) return "#20B2AA";  // 淺海綠
+                    if (id.startsWith("s8_")) return "#7CFC00";  // 草綠色
+                    if (id.startsWith("s9_")) return "#FF8C00";  // 深橙色
+                    return "#AAAAAA";  // 灰色 (預設)
+                }
+
+                function createIdIcon(id) {
+                    const color = getColorBySocketId(id);
+                    const sysid = id.split('_')[1];
+                    return L.divIcon({
+                        className: 'drone-id',
+                        html: `<div style="
+                            position: relative;
+                            left: 2px;
+                            background-color: ${color};
+                            color: black;
+                            width: 16px;
+                            height: 16px;
+                            border-radius: 50%;
+                            display: flex;
+                            align-items: center;
+                            justify-content: center;
+                            font-weight: bold;
+                            font-size: 10px;
+                            ">${sysid}</div>`,
+                        iconSize: [20, 20],
+                        iconAnchor: [10, 10]
+                    });
+                }
+
+                var markers = {};
+                var idLabels = {};
+                var trajectories = {};
+                var trajectoryLines = {};
+                var focusedId = null;
+                var initialized = false;
+                var trajectoryGroup = L.layerGroup().addTo(map);
+
+                function initTrajectory(id) {
+                    if (!trajectories[id]) {
+                        trajectories[id] = [];
+                        const color = getColorBySocketId(id);
+                        trajectoryLines[id] = L.polyline([], {
+                            color: color,
+                            weight: 3,
+                            opacity: 0.7,
+                            smoothFactor: 1
+                        }).addTo(trajectoryGroup);
+                    }
+                }
+
+                function addTrajectoryPoint(id, lat, lon) {
+                    initTrajectory(id);
+                    const point = [lat, lon];
+                    trajectories[id].push(point);
+                    
+                    if (trajectories[id].length > 1000) {
+                        trajectories[id].shift();
+                    }
+                    
+                    trajectoryLines[id].setLatLngs([...trajectories[id]]);
+                }
+
+                function focusOn(id) {
+                    if (!markers[id]) return;
+                    focusedId = id;
+                    var latlng = markers[id].getLatLng();
+                    map.flyTo(latlng, map.getZoom());
+                }
+
+                setInterval(() => {
+                    if (focusedId && markers[focusedId]) {
+                        var latlng = markers[focusedId].getLatLng();
+                        map.panTo(latlng);
+                    }
+                }, 1000);
+
+                function updateDrone(lat, lon, id, heading) {
+                    if (markers[id]) {
+                        const lastPos = markers[id].getLatLng();
+                        const distance = lastPos.distanceTo([lat, lon]);
+                        if (distance > 1) {
+                            addTrajectoryPoint(id, lat, lon);
+                        }
+                        
+                        markers[id]
+                            .setLatLng([lat, lon])
+                            .setRotationAngle(heading);
+                        idLabels[id].setLatLng([lat, lon]);
+                    } else {
+                        initTrajectory(id);
+                        addTrajectoryPoint(id, lat, lon);
+                        
+                        markers[id] = L.marker([lat, lon], {
+                            icon: arrowIcon,
+                            rotationAngle: heading,
+                            rotationOrigin: 'center'
+                        })
+                        .on('click', function () {
+                            focusOn(id);
+                        })
+                        .addTo(map);
+
+                        idLabels[id] = L.marker([lat, lon], {
+                            icon: createIdIcon(id),
+                            zIndexOffset: 1000
+                        })
+                        .on('click', function() {
+                            focusOn(id);
+                        })
+                        .addTo(map);
+
+                        if (!initialized || id < focusedId) {
+                            focusOn(id);
+                            initialized = true;
+                        }
+                    }
+                }
+
+                function clearAllTrajectories() {
+                    trajectories = {};
+                    Object.values(trajectoryLines).forEach(line => {
+                        trajectoryGroup.removeLayer(line);
+                    });
+                    trajectoryLines = {};
+                    console.log('所有軌跡已清除');
+                }
+            </script>
+            </body>
+            </html>
+        '''
+        
+        self.map_view.setHtml(inline_html)
+        self.map_view.loadFinished.connect(self._on_map_loaded)
+        
+        # 設置地圖更新計時器
+        self.map_update_timer = QTimer()
+        self.map_update_timer.timeout.connect(self.update_map_positions)
+        self.map_update_timer.start(200)  # 每 200ms 更新一次
+    
+    def _on_map_loaded(self, ok: bool):
+        """地圖加載完成回調"""
+        if ok:
+            self.map_loaded = True
+        else:
+            print("⚠️ 地圖加載失敗")
+    
+    def update_drone_position(self, drone_id, lat, lon, heading):
+        """更新無人機位置（加入待處理隊列）"""
+        self.pending_map_updates[drone_id] = (lat, lon, heading)
+    
+    def update_map_positions(self):
+        """批量更新地圖上的無人機位置"""
+        if not self.map_loaded or not self.pending_map_updates:
+            return
+        
+        # 批量執行所有待更新的位置
+        js_commands = []
+        for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
+            js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
+        
+        if js_commands:
+            combined_js = "\n".join(js_commands)
+            self.map_view.page().runJavaScript(combined_js)
+        
+        # 清空待更新緩存
+        self.pending_map_updates.clear()
+    
+    def clear_trajectories(self):
+        """清除所有軌跡"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript("clearAllTrajectories();")
+    
+    def focus_on_drone(self, drone_id):
+        """聚焦到指定無人機"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
+    
+    def get_widget(self):
+        """獲取地圖 widget"""
+        return self.map_view
+    
+    def get_gps_signal(self):
+        """獲取 GPS 信號"""
+        return self.bridge.gps_signal
+    
+class MapBridge(QObject):
+    """JavaScript 和 Python 之間的橋接類"""
+    gps_signal = pyqtSignal(float, float)  # lat, lon
+    
+    def __init__(self):
+        super().__init__()
+    
+    @pyqtSlot(float, float)
+    def emitGpsSignal(self, lat, lon):
+        """供 JavaScript 調用的方法"""
+        self.gps_signal.emit(lat, lon)

src/unitdev01/unitdev01/mavlink.py

@@ -21,13 +21,15 @@ class MAVLinkWorker(QThread):
     param_signal = pyqtSignal(str, int)
     kbps_signal = pyqtSignal(str, float)
 
-    def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
+    def __init__(self, connection_string="udp:127.0.0.1:14560   ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'):
         super().__init__()
-        self.namespaces = ['UAV1', 'UAV2', 'UAV3']
-        self.connection = mavutil.mavlink_connection(usb, baud=57600)
+        self.sysids = [1, 5, 10]
+        self.namespaces = {}
+        self.namespaces = [f"UAV{sysid}" for sysid in self.sysids]
+        
+        self.connection = mavutil.mavlink_connection(connection_string, baud=115200)
         self.connection.wait_heartbeat()
-        for sysid in self.namespaces:
-            sysid = int(sysid.replace('UAV', ''))
+        for sysid in self.sysids:
             self.set_sr_params(sysid)
         self.running = True
 
@@ -42,8 +44,8 @@ class MAVLinkWorker(QThread):
         self.total_bytes_received = {}
         self.throughput_KBps = {}
 
-        for namespace in self.namespaces:
-            self.request_param(namespace, "SR1_EXTRA1")
+        for sysid in self.sysids:
+            self.request_param(sysid, "SR1_EXTRA1")
         self.connection.mav.timesync_send( 
             0, #tc1
             int(time.time() * 1e9) #ts1
@@ -52,7 +54,7 @@ class MAVLinkWorker(QThread):
     def run(self):
         while self.running:
             try:
-                msg = self.connection.recv_msg()
+                msg = self.connection.recv_msg() # Debugging line to see received messages
                 current_time = time.time()
                 if not msg:
                     continue
@@ -64,7 +66,6 @@ class MAVLinkWorker(QThread):
                 if msg_type =="BAD_DATA":
                     continue
                 
-
                 if sysid not in self.total_bytes_received:
                     self.total_bytes_received[sysid] = 0
                     self.throughput_KBps[sysid] = 0
@@ -162,7 +163,7 @@ class MAVLinkWorker(QThread):
                 elif msg_type == 'PARAM_VALUE':
                     param_name = "SR1_EXTRA1"
                     if msg.param_id == param_name:
-                        self.param_signal.emit(namespace, msg.param_value)
+                        self.param_signal.emit(namespace, int(msg.param_value))
 
             except Exception as e:
                 print(f"Error reading message: {e}")
@@ -171,8 +172,11 @@ class MAVLinkWorker(QThread):
         self.running = False
         self.connection.close()
 
+    def get_sysid(self, namespace):
+        return int(namespace.replace('UAV', ''))
+    
     def set_mode(self, namespace, mode):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         if mode == 'STABILIZE':
             mode = 0
         elif mode == 'AUTO':
@@ -188,7 +192,7 @@ class MAVLinkWorker(QThread):
         )
 
     def arm(self, namespace, arm):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
@@ -199,7 +203,7 @@ class MAVLinkWorker(QThread):
         )
 
     def takeoff(self, namespace, altitude):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
@@ -210,7 +214,7 @@ class MAVLinkWorker(QThread):
         )
 
     def land(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
@@ -221,7 +225,7 @@ class MAVLinkWorker(QThread):
         )
 
     def set_local_position(self, namespace, x, y, z):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.set_position_target_local_ned_send(
             0, sysid, 1,  # time_boot_ms, sysid, compid
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # Frame
@@ -233,7 +237,7 @@ class MAVLinkWorker(QThread):
         )
     
     def reboot_drone(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         self.connection.mav.command_long_send(
             sysid,
             1,  # target_component (一般為1)
@@ -244,7 +248,7 @@ class MAVLinkWorker(QThread):
         )
 
     def set_param(self, namespace, param_name, value):
-        sysid = int(namespace.replace('UAV', ''))
+        sysid = self.get_sysid(namespace)
         try:
             self.connection.mav.param_set_send(
                 sysid,
@@ -256,8 +260,7 @@ class MAVLinkWorker(QThread):
         except Exception as e:
             print(f"Failed to set parameter {param_name}: {e}")
 
-    def request_param(self, namespace, param_name):
-        sysid = int(namespace.replace('UAV', ''))
+    def request_param(self, sysid, param_name):
         try:
             self.connection.mav.param_request_read_send(
                 sysid,  # 系統 ID
@@ -268,10 +271,35 @@ class MAVLinkWorker(QThread):
         except Exception as e:
             print(f"Failed to set parameter {param_name}: {e}")
 
-    '''
+     
     def set_sr_params(self, sysid):
         """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 2, 4]
+        freqs = [0, 1, 4]
+        params = {
+            "SR1_ADSB": freqs[0],
+            "SR1_EXT_STAT": freqs[1],
+            "SR1_EXTRA1": freqs[2],
+            "SR1_EXTRA2": freqs[1],
+            "SR1_EXTRA3": freqs[1],
+            "SR1_POSITION": freqs[2],
+            "SR1_RAW_SENS": freqs[1],
+            "SR1_RC_CHAN": freqs[1]
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
+        
+    '''    
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        params = {
+            "SERIAL1_BAUD": 38
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
+           
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        freqs = [0, 3, 3]
         params = {
             "SR1_ADSB": freqs[0],
             "SR1_EXT_STAT": freqs[1],
@@ -285,26 +313,15 @@ class MAVLinkWorker(QThread):
         for param, value in params.items():
             self.set_param(f"UAV{sysid}", param, value)
     '''
-    def set_sr_params(self, sysid):
-        """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 1, 1]
-        params = {
-            "SR1_ADSB": freqs[0],
-            "SR1_EXT_STAT": freqs[0],
-            "SR1_EXTRA1": freqs[2],
-            "SR1_EXTRA2": freqs[2],
-            "SR1_EXTRA3": freqs[0],
-            "SR1_POSITION": freqs[1],
-            "SR1_RAW_SENS": freqs[1],
-            "SR1_RC_CHAN": freqs[0]
-        }
-        for param, value in params.items():
-            self.set_param(f"UAV{sysid}", param, value)
+    def init_drone(self, namespace):
+        sysid = self.get_sysid(namespace)
+        self.set_local_position(sysid, 0, 0, 0)
 
 class DroneControlApp(QMainWindow):
     def __init__(self):
         super().__init__()
         self.workers = MAVLinkWorker()
+        self.sysids = self.workers.sysids
         self.namespaces = self.workers.namespaces
         self.initUI()
         
@@ -438,6 +455,15 @@ class DroneControlApp(QMainWindow):
         self.rebootButton.clicked.connect(self.reboot_drone)
         main_layout.addWidget(self.rebootButton)
 
+        mission_layout = QHBoxLayout()
+        self.initButton = QPushButton("重設位置")
+        self.initButton.clicked.connect(self.init_drone)
+        self.startButton = QPushButton("開始任務")
+        self.startButton.clicked.connect(self.start_mission)
+        mission_layout.addWidget(self.initButton)
+        mission_layout.addWidget(self.startButton)
+        main_layout.addLayout(mission_layout)
+
         # 設置主佈局
         central_widget = QWidget(self)
         central_widget.setLayout(main_layout)
@@ -563,6 +589,24 @@ class DroneControlApp(QMainWindow):
         except ValueError:
             QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
 
+    def init_drone(self):
+        try:
+            for namespace in self.namespaces:
+                if self.uav_checkboxes[namespace].isChecked():
+                    self.workers.init_drone(namespace)
+                    print(f"重設位置：{namespace}")
+        except Exception as e:
+            QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}")
+
+    def start_mission(self):
+        try:
+            for namespace in self.namespaces:
+                if self.uav_checkboxes[namespace].isChecked():
+                    self.workers.start_mission(namespace)
+                    print(f"開始任務：{namespace}")
+        except Exception as e:
+            QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}")
+
 def main():
     app = QtWidgets.QApplication(sys.argv)
     window = DroneControlApp()

src/unitdev01/unitdev01/xbee.py

@@ -1,619 +0,0 @@
-import sys
-import time
-import math
-import serial
-import struct
-from PyQt5 import QtWidgets
-from PyQt5.QtCore import QThread, pyqtSignal
-from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox
-from pymavlink import mavutil
-from pymavlink.dialects.v20 import ardupilotmega as mavlink2
-
-class PacketCapture:
-    def __init__(self):
-        self.data = bytearray()
-
-    def write(self, b):
-        self.data.extend(b)
-        return len(b)
-  
-class MAVLinkWorker(QThread):
-    state_signal = pyqtSignal(str, str)
-    battery_signal = pyqtSignal(str, float)
-    gps_signal = pyqtSignal(str, float, float)
-    gps_status_signal = pyqtSignal(str, int, int)
-    local_position_signal = pyqtSignal(str, float, float, float)
-    imu_signal = pyqtSignal(str, float)
-    hdg_signal = pyqtSignal(str, float)
-    groundspeed_signal = pyqtSignal(str, float)
-    ping_signal = pyqtSignal(str, float)
-    loss_signal = pyqtSignal(str, float)
-    frequency_signal = pyqtSignal(str, float)
-    param_signal = pyqtSignal(str, int)
-    kbps_signal = pyqtSignal(str, float)
-
-    def __init__(self):
-        super().__init__()
-        self.namespaces = ['UAV1', 'UAV2', 'UAV3']
-        self.connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)
-        self.connection.wait_heartbeat()
-        for sysid in self.namespaces:
-            sysid = int(sysid.replace('UAV', ''))
-            self.set_sr_params(sysid)
-        self.running = True
-
-        # 用於計算頻率丟包
-        self.message_count = {}
-        self.frequency = {}
-        self.start_time = {}
-        self.seq_numbers = {}
-        self.packet_loss_count = {}
-        self.total_packet_count = {}
-        self.loss_percentage = {}
-        self.total_bytes_received = {}
-        self.throughput_KBps = {}
-
-        for namespace in self.namespaces:
-            self.request_param(namespace, "SR1_EXTRA1")
-        self.connection.mav.timesync_send( 
-            0, #tc1
-            int(time.time() * 1e9) #ts1
-        )
-
-    def run(self):
-        while self.running:
-            try:
-                msg = self.connection.recv_msg()
-                current_time = time.time()
-                if not msg:
-                    continue
-                sysid = msg.get_srcSystem()
-                if sysid == 0:
-                    continue
-                namespace = f"UAV{sysid}"
-                msg_type = msg.get_type()
-                if msg_type =="BAD_DATA":
-                    continue
-                
-                if sysid not in self.total_bytes_received:
-                    self.total_bytes_received[sysid] = 0
-                    self.throughput_KBps[sysid] = 0
-
-                # 計算訊息大小
-                msg_bytes = msg.get_msgbuf()  # 取得封包的 bytes
-                if msg_bytes:
-                    self.total_bytes_received[sysid] += len(msg_bytes)
-                
-                # Packet loss calculation
-                if sysid not in self.seq_numbers:
-                    self.seq_numbers[sysid] = msg.get_seq()  # Initialize sequence number tracking
-                    self.packet_loss_count[sysid] = 0
-                    self.total_packet_count[sysid] = 1
-                else:
-                    current_seq = msg.get_seq()
-                    expected_seq = (self.seq_numbers[sysid] + 1) % 256
-                    self.total_packet_count[sysid] += 1
-
-                    if current_seq != expected_seq: # Packet(s) lost
-                        lost_packets = (current_seq - expected_seq) % 256
-                        self.packet_loss_count[sysid] += lost_packets
-                        self.total_packet_count[sysid] += lost_packets
-
-                    self.seq_numbers[sysid] = current_seq
-                    self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100
-                    self.loss_signal.emit(namespace, self.loss_percentage[sysid])
-
-                # Frequency calculation
-                if sysid not in self.message_count:
-                    self.message_count[sysid] = 0
-                    self.start_time[sysid] = current_time
-
-                self.message_count[sysid] += 1
-
-                # 每隔一段時間更新
-                elapsed_time = current_time - self.start_time[sysid]
-                if elapsed_time >= 1:
-                    # 每秒頻率
-                    self.frequency[sysid] = self.message_count[sysid] / elapsed_time
-                    self.frequency_signal.emit(namespace, self.frequency[sysid])
-
-                    # 發送 timesync request
-                    self.connection.mav.timesync_send( 
-                        0, #tc1
-                        int(current_time * 1e9) #ts1
-                    )
-                    #吞吐量
-                    self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time 
-                    self.kbps_signal.emit(namespace, self.throughput_KBps[sysid])
-
-                    self.start_time[sysid] = current_time
-                    self.message_count[sysid] = 0
-                    self.total_bytes_received[sysid] = 0
-
-                if msg_type == "HEARTBEAT":
-                    mode = mavutil.mode_string_v10(msg)
-                    self.state_signal.emit(namespace, mode)
-
-                elif msg_type == "BATTERY_STATUS":
-                    voltage = msg.voltages[0] / 1000
-                    self.battery_signal.emit(namespace, voltage)
-
-                elif msg_type == "GLOBAL_POSITION_INT":
-                    latitude = msg.lat / 1e7
-                    longitude = msg.lon / 1e7
-                    self.gps_signal.emit(namespace, latitude, longitude)
-
-                elif msg_type == "GPS_RAW_INT":
-                    fix_type = msg.fix_type
-                    satellites_visible = msg.satellites_visible
-                    self.gps_status_signal.emit(namespace, fix_type, satellites_visible)
-
-                elif msg_type == "LOCAL_POSITION_NED":
-                    x = msg.y
-                    y = msg.x
-                    z = -msg.z
-                    self.local_position_signal.emit(namespace, x, y, z)
-
-                elif msg_type == "ATTITUDE":
-                    pitch = math.degrees(msg.pitch)
-                    self.imu_signal.emit(namespace, pitch)
-
-                elif msg_type == "VFR_HUD":
-                    groundspeed = msg.groundspeed
-                    heading = msg.heading
-                    self.hdg_signal.emit(namespace, heading)
-                    self.groundspeed_signal.emit(namespace, groundspeed)
-
-                elif msg_type == "TIMESYNC":
-                    round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6
-                    if(round_trip_time<1e6):
-                        self.ping_signal.emit(namespace, round_trip_time)
-                
-                elif msg_type == 'PARAM_VALUE':
-                    param_name = "SR1_EXTRA1"
-                    if msg.param_id == param_name:
-                        self.param_signal.emit(namespace, msg.param_value)
-
-            except Exception as e:
-                print(f"Error reading message: {e}")
-
-    def stop(self):
-        self.running = False
-        self.connection.close()
-
-    def build_api_tx_frame(self, data: bytes, frame_id=0x01):
-        frame_type = 0x10
-        dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # Broadcast
-        dest_addr16 = b'\xFF\xFE'
-        broadcast_radius = 0x00
-        options = 0x00
-
-        frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
-        frame += dest_addr64 + dest_addr16
-        frame += struct.pack(">BB", broadcast_radius, options) + data
-        checksum = 0xFF - (sum(frame) & 0xFF)
-        return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
-
-    def send_command(self, msg):
-        # Create the packet and send via serial port
-        PORT = "/dev/ttyUSB0"
-        BAUD = 57600
-        ser = serial.Serial(PORT, BAUD)
-        capture = PacketCapture()
-        mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1)
-        mav.version = 2
-        mav.send(msg)
-        api_frame = self.build_api_tx_frame(capture.data)
-        ser.write(api_frame)
-        print("RAW HEX:", capture.data.hex())
-
-    def set_mode(self, namespace, mode):
-        sysid = int(namespace.replace('UAV', ''))
-        if mode == 'STABILIZE':
-            mode = 0
-        elif mode == 'AUTO':
-            mode = 3
-        elif mode == 'GUIDED':
-            mode = 4
-        elif mode == 'LOITER':
-            mode = 5
-
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_DO_SET_MODE,
-            confirmation=0,
-            param1=1,  # Custom mode enabled
-            param2=mode,
-            param3=0, param4=0, param5=0, param6=0, param7=0
-        )
-        self.send_command(msg)
-
-    def arm(self, namespace, arm):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-            confirmation=0,
-            param1=1 if arm else 0,  # 1 to arm, 0 to disarm
-            param2=0, param3=0, param4=0, param5=0, param6=0, param7=0
-        )
-        self.send_command(msg)
-
-    def takeoff(self, namespace, altitude):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
-            confirmation=0,
-            param1=0, param2=0, param3=0, param4=0, param5=0, param6=0,
-            param7=altitude
-        )
-        self.send_command(msg)
-
-    def land(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.command_long_encode(
-            target_system=sysid,
-            target_component=1,
-            command=mavutil.mavlink.MAV_CMD_NAV_LAND,
-            confirmation=0,
-            param1=0, param2=0, param3=0, param4=0, param5=0, param6=0,
-            param7=0
-        )
-        self.send_command(msg)
-
-    def set_local_position(self, namespace, x, y, z):
-        sysid = int(namespace.replace('UAV', ''))
-        msg = self.connection.mav.set_position_target_local_ned_encode(
-            0, sysid, 1,  # time_boot_ms, sysid, compid
-            mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # Frame
-            0b110111111000,  # Mask
-            y, x, -z,  # Position
-            0, 0, 0,  # Velocity
-            0, 0, 0,  # Acceleration
-            0, 0  # Yaw, yaw_rate
-        )
-        self.send_command(msg)
-
-    def reboot_drone(self, namespace):
-        sysid = int(namespace.replace('UAV', ''))
-        self.connection.mav.command_long_send(
-            sysid,
-            1,  # target_component (一般為1)
-            mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,  # Reboot 命令
-            0,  # confirmation
-            1,  # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機)
-            0, 0, 0, 0, 0, 0  # 其餘參數保留
-        )
-
-    def set_param(self, namespace, param_name, value):
-        sysid = int(namespace.replace('UAV', ''))
-        try:
-            self.connection.mav.param_set_send(
-                sysid,
-                1,
-                param_name.encode('utf-8'),
-                float(value),
-                mavutil.mavlink.MAV_PARAM_TYPE_REAL32
-            )
-        except Exception as e:
-            print(f"Failed to set parameter {param_name}: {e}")
-
-    def request_param(self, namespace, param_name):
-        sysid = int(namespace.replace('UAV', ''))
-        try:
-            self.connection.mav.param_request_read_send(
-                sysid,  # 系統 ID
-                1,  # 組件 ID
-                param_name.encode('utf-8'),  # 參數名稱
-                -1  # 參數索引，-1 表示根據名稱請求
-            )
-        except Exception as e:
-            print(f"Failed to set parameter {param_name}: {e}")
-
-    '''
-    def set_sr_params(self, sysid):
-        """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 2, 4]
-        params = {
-            "SR1_ADSB": freqs[0],
-            "SR1_EXT_STAT": freqs[1],
-            "SR1_EXTRA1": freqs[2],
-            "SR1_EXTRA2": freqs[2],
-            "SR1_EXTRA3": freqs[1],
-            "SR1_POSITION": freqs[1],
-            "SR1_RAW_SENS": freqs[1],
-            "SR1_RC_CHAN": freqs[1]
-        }
-        for param, value in params.items():
-            self.set_param(f"UAV{sysid}", param, value)
-    '''
-    def set_sr_params(self, sysid):
-        """ 直接設置 MAVLink 訊息頻率 """
-        freqs = [0, 1, 1]
-        params = {
-            "SR1_ADSB": freqs[0],
-            "SR1_EXT_STAT": freqs[0],
-            "SR1_EXTRA1": freqs[2],
-            "SR1_EXTRA2": freqs[2],
-            "SR1_EXTRA3": freqs[0],
-            "SR1_POSITION": freqs[1],
-            "SR1_RAW_SENS": freqs[1],
-            "SR1_RC_CHAN": freqs[0]
-        }
-        for param, value in params.items():
-            self.set_param(f"UAV{sysid}", param, value)
-
-class DroneControlApp(QMainWindow):
-    def __init__(self):
-        super().__init__()
-        self.workers = MAVLinkWorker()
-        self.namespaces = self.workers.namespaces
-        self.initUI()
-        
-        # Connect signals to update the UI
-        self.workers.state_signal.connect(self.update_state)
-        self.workers.battery_signal.connect(self.update_battery)
-        self.workers.gps_signal.connect(self.update_gps)
-        self.workers.gps_status_signal.connect(self.update_gps_status)
-        self.workers.local_position_signal.connect(self.update_local_position)
-        self.workers.imu_signal.connect(self.update_imu)
-        self.workers.hdg_signal.connect(self.update_hdg)
-        self.workers.groundspeed_signal.connect(self.update_groundspeed)
-        self.workers.ping_signal.connect(self.update_ping)
-        self.workers.loss_signal.connect(self.update_loss)
-        self.workers.frequency_signal.connect(self.update_frequency)
-        self.workers.param_signal.connect(self.update_param)
-        self.workers.kbps_signal.connect(self.update_kbps)
-        self.workers.start()
-            
-    def initUI(self):
-        self.setWindowTitle("多無人機控制介面")
-        self.setGeometry(100, 100, 800, 600)
-
-        # 主佈局
-        main_layout = QVBoxLayout()
-
-        # 無人機選擇區域
-        uav_layout = QHBoxLayout()
-        self.uav_checkboxes = {}
-        for namespace in self.namespaces:
-            checkbox = QCheckBox(namespace)
-            checkbox.setChecked(True)  # 預設勾選
-            self.uav_checkboxes[namespace] = checkbox
-            uav_layout.addWidget(checkbox)
-        main_layout.addLayout(uav_layout)
-
-        # 顯示所有無人機資訊，從左到右顯示
-        uav_layout = QHBoxLayout()
-        
-        # 逐個顯示 UAV 的資訊
-        self.uav_labels = {}
-        for namespace in self.namespaces:
-            uav_info_layout = QVBoxLayout()  # 每台 UAV 的資訊垂直排列
-
-            # 狀態顯示
-            self.uav_labels[namespace] = {
-                "status": QLabel("狀態：等待數據..."),
-                "battery": QLabel("電壓：等待數據..."),
-                "local_position": QLabel("Local Position：等待數據..."),
-                "gps": QLabel("GPS位置：等待數據...\n\n"),
-                "fix": QLabel("Fix Type：等待數據..."),
-                "satellites": QLabel("衛星數量：等待數據..."),
-                "groundspeed": QLabel("地面速度：等待數據..."),
-                "pitch": QLabel("Pitch：等待數據..."),
-                "heading": QLabel("Heading：等待數據..."),
-                "ping": QLabel("Ping：等待數據..."),
-                "loss": QLabel("丟包：等待數據..."),
-                "frequency": QLabel("頻率：等待數據..."),
-                "kbps": QLabel("吞吐量：等待數據..."),
-                "param": QLabel("SR1_EXTRA1：等待數據...")
-            }
-
-            # 把每個資訊添加到該 UAV 的垂直佈局中
-            for label in self.uav_labels[namespace].values():
-                uav_info_layout.addWidget(label)
-
-            # 將該 UAV 的資訊佈局加到主佈局中（從左到右排列）
-            uav_layout.addLayout(uav_info_layout)
-
-        main_layout.addLayout(uav_layout)
-
-        # SR1_EXTRA1參數設置
-        param_layout = QHBoxLayout()
-        self.paramInput = QLineEdit()
-        self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值")
-        self.setParamButton = QPushButton("設置SR1_EXTRA1")
-        self.setParamButton.clicked.connect(self.set_SR1_EXTRA1)
-        param_layout.addWidget(self.paramInput)
-        param_layout.addWidget(self.setParamButton)
-        main_layout.addLayout(param_layout)
-
-        # 模式切換區域
-        mode_layout = QHBoxLayout()
-        self.modeComboBox = QComboBox()
-        self.modeComboBox.addItems(["GUIDED", "STABILIZE", "AUTO", "LOITER"])
-        self.modeButton = QPushButton("切換模式")
-        self.modeButton.clicked.connect(self.change_mode)
-        mode_layout.addWidget(QLabel("選擇模式："))
-        mode_layout.addWidget(self.modeComboBox)
-        mode_layout.addWidget(self.modeButton)
-        main_layout.addLayout(mode_layout)
-
-        # 解鎖按鈕
-        fly_layout = QHBoxLayout()
-        self.armButton = QPushButton("解鎖")
-        self.armButton.clicked.connect(self.arm_drone)
-
-        # 起飛按鈕
-        self.takeoffButton = QPushButton("起飛")
-        self.takeoffButton.clicked.connect(self.takeoff_drone)
-
-        # 降落按鈕
-        self.landButton = QPushButton("降落")
-        self.landButton.clicked.connect(self.land_drone)
-        fly_layout.addWidget(self.armButton)
-        fly_layout.addWidget(self.takeoffButton)
-        fly_layout.addWidget(self.landButton)
-        main_layout.addLayout(fly_layout)
-
-        # XYZ 設置欄位
-        xyz_layout = QHBoxLayout()
-        self.positionX = QLineEdit()
-        self.positionX.setPlaceholderText("X")
-        self.positionY = QLineEdit()
-        self.positionY.setPlaceholderText("Y")
-        self.positionZ = QLineEdit()
-        self.positionZ.setPlaceholderText("Z")
-        self.setPositionButton = QPushButton("設置位置")
-        self.setPositionButton.clicked.connect(self.set_local_position)
-        xyz_layout.addWidget(QLabel("輸入位置："))
-        xyz_layout.addWidget(self.positionX)
-        xyz_layout.addWidget(self.positionY)
-        xyz_layout.addWidget(self.positionZ)
-        xyz_layout.addWidget(self.setPositionButton)
-        main_layout.addLayout(xyz_layout)
-
-        # 設置 XYZ 的按鈕
-
-        #設置重啟按鈕
-        self.rebootButton = QPushButton("重啟")
-        self.rebootButton.clicked.connect(self.reboot_drone)
-        main_layout.addWidget(self.rebootButton)
-
-        # 設置主佈局
-        central_widget = QWidget(self)
-        central_widget.setLayout(main_layout)
-        self.setCentralWidget(central_widget)
-        self.show()
-
-    def closeEvent(self, event):
-        try:
-            self.workers.stop()
-        finally:
-            event.accept()
-
-    def update_state(self, namespace, mode):
-        self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
-
-    def update_battery(self, namespace, voltage):
-        self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V")
-
-    def update_gps(self, namespace, latitude, longitude):
-        self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n    Lat:{latitude:.6f}° \n    Lon:{longitude:.6f}°")
-
-    def update_gps_status(self, namespace, fix_type, satellites_visible):
-        fix_type_str = {
-            0: "No Fix",
-            1: "No Fix",
-            2: "2D Fix",
-            3: "3D Fix",
-            4: "RTK Float",
-            5: "RTK Fix"
-        }.get(fix_type, "Unknown")
-        self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}")
-        self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}")
-
-    def update_local_position(self, namespace, x, y, z):
-        self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})")
-
-    def update_imu(self, namespace, pitch):
-        self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°")
-
-    def update_hdg(self, namespace, heading):
-        self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°")
-
-    def update_groundspeed(self, namespace, groundspeed):
-        self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s")
-    
-    def update_ping(self, namespace, ping):
-        self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms")
-
-    def update_loss(self, namespace, loss):
-        self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%")
-
-    def update_frequency(self, namespace, frequency):
-        self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
-
-    def update_kbps(self, namespace, kbps):
-        self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s")
-
-    def update_param(self, namespace, value):
-        self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
-
-    def change_mode(self):
-        try:
-            selected_mode = self.modeComboBox.currentText()
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_mode(namespace, selected_mode)
-        except Exception as e:
-            QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}")
-
-    def arm_drone(self):
-        try:
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.arm(namespace, True)
-        except Exception as e:
-            QtWidgets.QMessageBox.warning(self, "錯誤", f"解鎖失敗：{e}")
-
-    def takeoff_drone(self):
-        try:
-            z_text = self.positionZ.text().strip()
-            z = float(z_text) if z_text else 5.0
-            h = 2
-            for i, namespace in enumerate(self.namespaces):
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.takeoff(namespace, z + h * i)
-            
-        except ValueError:
-            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-
-    def land_drone(self):
-        for namespace in self.namespaces:
-            if self.uav_checkboxes[namespace].isChecked():
-                self.workers.land(namespace)
-
-    def set_local_position(self):
-        try:
-            x = float(self.positionX.text().strip())
-            y = float(self.positionY.text().strip())
-            z = float(self.positionZ.text().strip())
-            h = 2
-            for i, namespace in enumerate(self.namespaces):
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_local_position(namespace, x, y, z + h * i)
-            
-        except ValueError:
-            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-
-    def reboot_drone(self):
-        try:
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.reboot_drone(namespace)
-        except Exception as e:
-            QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}")
-
-    def set_SR1_EXTRA1(self):
-        param_value = self.paramInput.text().strip()
-        try:
-            for namespace in self.namespaces:
-                if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_param(namespace, "SR1_EXTRA1", param_value)
-                    self.workers.request_param(namespace, "SR1_EXTRA1")
-        except ValueError:
-            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-
-def main():
-    app = QtWidgets.QApplication(sys.argv)
-    window = DroneControlApp()
-    window.show()
-    sys.exit(app.exec_())
-
-if __name__ == '__main__':
-    main()