Force sync local master to remote ken

主專案整合

.gitignore

@@ -25,3 +25,4 @@ Makefile
 **/*.class
 **/*.pyc
 **/*.pyo
+**/.cursor/

README.md

@@ -1,37 +1,46 @@
 這是天雷系統的專案
 
+
+
 ===
-專案核心框架
+## 功能簡介
+1. mavlink 多對多支援平台
+2. 不允許進到 ros 系統有相同 sysid
+3. 假設一台載具上所有 component 共用同一 socket
+
+===
+## 運行環境
+
+### 專案核心框架
 1. ROS2 Humble
 2. Python 3.8.10
 
-
+### 必要相依套件及版本
-===
+- Python
-必要相依套件 順便記錄我開發時的環境版本
+[Package] fc_network_adapter
-
-Python
     1. pymavlink -> Version: 2.4.42
     2. conda-forge 中的 pyserial-asyncio
     3. importlib_metadata -> Version: 8.5.0
     4. setuptools -> Version: 58.2.0 (版本太新不行)
     5. pyserial-asyncio
+[Package] GUI
+    1. testresources
+    2. websockets
+    3. PyQt6
+    4. PyQt6-WebEngine
 
-
+- ROS2
-ROS2
 1. source ~/ros2_humble/install/setup.bash
-2.
+2. geographic_info (https://github.com/ros-geographic-info/geographic_info.git) 已經作為 submodule 放在 external
+3. angles (https://github.com/ros/angles.git) 已經作為 submodule 放在 external
+4. mavros_msgs (https://github.com/mavlink/mavros) 這個專案中的一個資料夾 這邊手動複製的
 
-===
+### 開發用輔助專案
-開發用輔助專案
 1. Gazebo Garden
 2. Ardupilot
 
 ===
-依賴的 ROS 庫
+## 使用說明
-1. https://github.com/ros-geographic-info/geographic_info.git 記得要搞 ros2 版本的
-2. https://github.com/ros/angles.git
-3. mavros_msgs 是 https://github.com/mavlink/mavros 這個專案中的一個資料夾 這邊手動複製的
-
 Clone 專案後 請先執行這些指令
 ```bash
 # 1.同步 submodule
@@ -41,34 +50,55 @@ git submodule update
 # 2. build 需要的 package
 colcon build --packages-select angles geographic_msgs
 colcon build --packages-select mavros_msgs # 這個依賴前面的
+colcon build --packages-select fc_interfaces # 自己定義的
+
+```
+1. 
+主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 路徑下 以模組化啟動程式 
+```bash
+# 記得先開啟 依賴 Package 到 overlay
+. ./install/local_setup.bash
+# 範例
+cd ~/AirTrapMine/src/ # 這是範例!!!
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.tests.demo_integration
+python -m someotherpkg.src.example_wholeMoving
 ```
 
+2. 
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
 
 ===
-Package 簡述
+## 資料夾說明
 1. unitdev 為各自協作者做開發時的測試區
     01 -> 晉凱(ken)
     02 -> 其宇(chiyu)
     03 -> 文鈞
     04 -> 倫渝
-2. fc_network_adapter 
+2. fc_network_adapter (核心)
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
     --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-
+3. fc_interfaces (重要)
+    自定義的 ROS2 介面檔
+4. fc_network_apps
+    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
+    使用者的外層包裝
+5. someotherpkg
+    如何使用 fc_network_apps 的範例檔案
+6. GUI 
+    由 PyQt6 開發的互動式介面
 N. logs 是執行時期的記錄檔
 
 ===
-主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式
 
-例如 在 ~/AirTrapMine/src/ 資料夾下 
-```bash
-# 記得先開啟 依賴 Package 到 overlay
-. ./install/local_setup.bash
-# 範例
-python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
-python -m fc_network_adapter.tests.demo_integration
 
-```
 
+簡述0328
+主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。

src/GUI/comm_panel.py

@@ -0,0 +1,687 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+                            QPushButton, QLineEdit, QComboBox)
+from PyQt6.QtCore import pyqtSignal
+import glob
+import os
+
+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
+    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):
+        """初始化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("14550")
+        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)
+        
+        # ========== 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("""
+            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)
+        
+        # 只清空 port 輸入框，保留 IP
+        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 _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()
+        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)
+    
+    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/command_sender.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+"""
+指令發送模組
+負責將目標位置轉成具體的通訊指令發送到飛控
+
+現階段: MavlinkSender (直接 pymavlink 發送)
+未來:   替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
+"""
+from abc import ABC, abstractmethod
+from pymavlink import mavutil
+
+
+class CommandSender(ABC):
+    """指令發送抽象介面"""
+
+    @abstractmethod
+    def send_position_global(self, sysid, lat, lon, alt):
+        """
+        發送全球座標位置指令
+
+        Args:
+            sysid: 目標無人機的 MAVLink system ID
+            lat: 緯度 (度)
+            lon: 經度 (度)
+            alt: 高度 (公尺)
+        """
+        pass
+
+    @abstractmethod
+    def close(self):
+        """關閉連線"""
+        pass
+
+
+class MavlinkSender(CommandSender):
+    """
+    MAVLink 直接發送 (驗證階段用)
+    透過 SET_POSITION_TARGET_GLOBAL_INT 發送目標位置
+
+    使用方式:
+        sender = MavlinkSender("udpout:127.0.0.1:14550")
+        sender.send_position_global(sysid=1, lat=24.123, lon=120.567, alt=10.0)
+    """
+
+    # type_mask: 只使用位置 (lat, lon, alt)
+    # 忽略速度 (bit 3,4,5)、加速度 (bit 6,7,8)、yaw (bit 10)、yaw_rate (bit 11)
+    TYPE_MASK_POSITION_ONLY = (
+        0b0000_1101_1111_1000  # = 0x0DF8
+    )
+
+    def __init__(self, connection_string="udpout:127.0.0.1:14550"):
+        """
+        Args:
+            connection_string: MAVLink 連線字串
+                SITL 範例: "udpout:127.0.0.1:14550"
+        """
+        self.connection_string = connection_string
+        self.mav = mavutil.mavlink_connection(connection_string)
+        print(f"MavlinkSender 已建立連線: {connection_string}")
+
+    def send_position_global(self, sysid, lat, lon, alt):
+        """
+        發送 SET_POSITION_TARGET_GLOBAL_INT
+
+        注意:
+            - coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
+              高度是相對起飛點的高度 (公尺)
+            - 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
+              需要在外部先減去起飛點高度再傳入
+        """
+        self.mav.mav.set_position_target_global_int_send(
+            0,                          # time_boot_ms (不使用)
+            sysid,                      # target_system
+            1,                          # target_component (autopilot)
+            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            self.TYPE_MASK_POSITION_ONLY,
+            int(lat * 1e7),             # lat_int
+            int(lon * 1e7),             # lon_int
+            float(alt),                 # alt
+            0, 0, 0,                    # vx, vy, vz (忽略)
+            0, 0, 0,                    # afx, afy, afz (忽略)
+            0, 0                        # yaw, yaw_rate (忽略)
+        )
+
+    def close(self):
+        """關閉 MAVLink 連線"""
+        if self.mav:
+            self.mav.close()
+            self.mav = None
+            print("MavlinkSender 已關閉")

src/GUI/communication.py

@@ -0,0 +1,905 @@
+from rclpy.node import Node
+from PyQt6.QtCore import QObject, pyqtSignal
+import math
+import re
+import threading
+from threading import Lock
+from concurrent.futures import ThreadPoolExecutor
+import asyncio
+import websockets
+import json
+import socket
+import sys
+import os
+import traceback
+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, String
+from mavros_msgs.msg import State, VfrHud
+from mavros_msgs.srv import CommandBool, CommandTOL
+
+# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
+_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _src_path not in sys.path:
+    sys.path.insert(0, _src_path)
+
+# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
+try:
+    from fc_network_apps.longCommand import CommandLongClient
+except ImportError as e:
+    import traceback
+    print(f"⚠️  警告: 無法導入 CommandLongClient")
+    print(f"   错误: {e}")
+    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+    print(f"   完整堆栈跟踪:")
+    traceback.print_exc()
+    CommandLongClient = None
+
+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, monitor=None):
+        super().__init__(daemon=True)
+        self.ip = ip
+        self.port = port
+        self.signals = signals
+        self.connection_name = connection_name
+        self.monitor = monitor  # 保存 monitor 引用
+        self.socket_id = monitor.get_next_socket_id() if monitor else 0
+        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"s{self.socket_id}_{system_id}"
+            
+            if msg_type == "HEARTBEAT":
+                # 先發送連接類型資訊
+                self.signals.update_signal.emit('connection_type', drone_id, {
+                    'type': 'UDP'
+                })
+                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":
+                # 從 MAVLink 訊息中提取並轉為角度
+                pitch = math.degrees(msg.pitch)
+                roll = math.degrees(msg.roll)
+                yaw = math.degrees(msg.yaw)
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'pitch': pitch,
+                    'roll': roll,
+                    'yaw': yaw,
+                    'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
+                })
+
+            elif msg_type == "VFR_HUD":
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'airspeed': msg.airspeed,
+                    'groundspeed': msg.groundspeed,
+                    'heading': msg.heading,
+                    'throttle': msg.throttle,
+                    'alt': msg.alt,
+                    'climb': msg.climb
+                })
+                
+        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, monitor=None):
+        super().__init__(daemon=True)
+        self.port = port
+        self.baudrate = baudrate
+        self.signals = signals
+        self.connection_name = connection_name
+        self.monitor = monitor  # 保存 monitor 引用
+        self.socket_id = monitor.get_next_socket_id() if monitor else 0
+        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"s{self.socket_id}_{system_id}"
+            
+            if msg_type == "HEARTBEAT":
+                # 先發送連接類型資訊
+                self.signals.update_signal.emit('connection_type', drone_id, {
+                    'type': 'Serial'
+                })
+                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":
+                # 從 MAVLink 訊息中提取並轉為角度
+                pitch = math.degrees(msg.pitch)
+                roll = math.degrees(msg.roll)
+                yaw = math.degrees(msg.yaw)
+                self.signals.update_signal.emit('attitude', drone_id, {
+                    'pitch': pitch,
+                    'roll': roll,
+                    'yaw': yaw,
+                    'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
+                })
+
+            elif msg_type == "VFR_HUD":
+                self.signals.update_signal.emit('hud', drone_id, {
+                    'airspeed': msg.airspeed,
+                    'groundspeed': msg.groundspeed,
+                    'heading': msg.heading,
+                    'throttle': msg.throttle,
+                    'alt': msg.alt,
+                    'climb': msg.climb
+                })
+                
+        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, monitor=None):
+        super().__init__(daemon=True)
+        self.url = url
+        self.signals = signals
+        self.connection_name = connection_name
+        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
+        
+    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:
+            system_id = data.get('system_id')
+            if not system_id:
+                return
+            drone_id = f"s{self.socket_id}_{system_id}"
+
+            # 模式
+            if 'mode' in data:
+                # 先發送連接類型資訊
+                self.signals.update_signal.emit('connection_type', drone_id, {
+                    'type': 'WS'
+                })
+                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):
+    # Subscribe to drone ROS2 topics
+    _instance = None  # Singleton pattern to prevent duplicate nodes
+    
+    def __init__(self):
+        # Use a unique node name with timestamp to avoid conflicts on restart
+        import time
+        node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
+        super().__init__(node_name)
+        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 = []
+        
+        # 串口接收器列表
+        
+        # ================================================================================
+        # 【新增】儲存 GPS 資料的字典
+        # ================================================================================
+        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 = []
+        
+        # ================================================================================
+        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # ================================================================================
+        self.command_long_client = None
+        try:
+            self.command_long_client = CommandLongClient()
+        except Exception as e:
+            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
+            self.command_long_client = None
+        # ================================================================================
+        
+        # 主题检测定时器
+        self.create_timer(1.0, self.scan_topics)
+    
+    def get_next_socket_id(self):
+        """获取下一个可用的 socket_id（从 0 开始连续分配）"""
+        with self.socket_id_lock:
+            current_id = self.socket_id_counter
+            self.socket_id_counter += 1
+            return current_id
+    
+    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
+                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'/fc_network/vehicle/(sys\d+)/(\w+)')
+        
+        found_drones = set()
+        for topic_name, _ in topics:
+            if match := drone_pattern.match(topic_name):
+                sys_id, topic_type = match.groups()
+                found_drones.add(sys_id)
+                with self.lock:
+                    self.drone_topics.setdefault(sys_id, set()).add(topic_type)
+        
+        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 = 0
+                self.sys_to_socket_id[sys_id] = 0
+            
+            if not hasattr(self, f'drone_{sys_id}_subs'):
+                self.setup_drone(sys_id)
+    
+    def setup_drone(self, sys_id):
+        # sys_id 格式： sys11, sys12, ...
+        base_topic = f'/fc_network/vehicle/{sys_id}'
+        
+        # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
+        self.arm_clients[sys_id] = self.create_client(
+            CommandBool,
+            f'{base_topic}/cmd/arming'
+        )
+        self.takeoff_clients[sys_id] = self.create_client(
+            CommandTOL,
+            f'{base_topic}/cmd/takeoff'
+        )
+
+        # Add setpoint publisher
+        self.setpoint_pubs[sys_id] = self.create_publisher(
+            Point,
+            f'{base_topic}/setpoint_position/local',
+            10
+        )
+
+        subs = {
+            'battery': self.create_subscription(
+                BatteryState,
+                f'{base_topic}/battery',
+                lambda msg, sid=sys_id: self.battery_callback(sid, msg),
+                10
+            ),
+            'position': self.create_subscription(
+                NavSatFix,
+                f'{base_topic}/position',
+                lambda msg, sid=sys_id: self.gps_callback(sid, msg),
+                10
+            ),
+            '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, sid=sys_id: self.hud_callback(sid, msg),
+                10
+            )
+        }
+        
+        setattr(self, f'drone_{sys_id}_subs', subs)
+
+    # ================================================================================
+    # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
+    # ================================================================================
+    MODE_MAPPING = {
+        "STABILIZE": 0,
+        "ACRO": 1,
+        "ALT_HOLD": 2,
+        "AUTO": 3,
+        "GUIDED": 4,
+        "LOITER": 5,
+        "RTL": 6,
+        "CIRCLE": 7,
+        "POSITION": 8,
+        "LAND": 9,
+        "OF_LOITER": 10,
+        "DRIFT": 11,
+        "SPORT": 13,
+        "FLIP": 14,
+        "AUTOTUNE": 15,
+        "POSHOLD": 16,
+        "BRAKE": 17,
+        "THROW": 18,
+        "AVOID_ADSB": 19,
+        "GUIDED_NOGPS": 20,
+        "SMART_RTL": 21,
+    }
+    # ================================================================================
+
+    async def set_mode(self, drone_id, mode_name):
+        """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            # 獲取模式對應的 custom_mode 值
+            custom_mode = self.MODE_MAPPING.get(mode_name)
+            if custom_mode is None:
+                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+                return False
+            
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+            
+            if not self.command_long_client:
+                print(f"❌ [SET_MODE] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.change_mode_async(
+                target_sysid=sysid,
+                custom_mode=float(custom_mode),
+                target_compid=0,
+                base_mode=1.0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [SET_MODE] 模式切換成功")
+                return True
+            else:
+                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+                return False
+        except Exception as e:
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
+    async def arm_drone(self, drone_id, arm):
+        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            action_name = "解鎖" if arm else "上鎖"
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
+            
+            if not self.command_long_client:
+                print(f"❌ [ARM] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.arm_disarm_async(
+                target_sysid=sysid,
+                arm=arm,
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [ARM] {action_name}成功")
+                return True
+            else:
+                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+                return False
+        except Exception as e:
+            print(f"❌ [ARM] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude):
+        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+            
+            if not self.command_long_client:
+                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.takeoff_async(
+                target_sysid=sysid,
+                altitude_m=float(altitude),
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [TAKEOFF] 起飛成功")
+                return True
+            else:
+                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                return False
+        except Exception as e:
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            traceback.print_exc()
+            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, 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
+        }
+
+    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 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.signals.update_signal.emit('connection_type', actual_drone_id, {
+                'type': 'ROS2'
+            })
+            
+            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
+        }
+        
+        # ================================================================================
+        # 【新增】儲存 GPS 資料到 drone_gps 字典
+        # ================================================================================
+        self.drone_gps[actual_drone_id] = {
+            '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, 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,
+            '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, self)
+        receiver.start()
+        self.serial_receivers.append(receiver)
+        print(f"Started serial connection on {port} at {baudrate} baud")
+        return receiver

src/GUI/drone_panel.py

@@ -0,0 +1,586 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
+from PyQt6.QtCore import pyqtSignal
+from PyQt6.QtGui import QPainter, QPen, QColor, QFont, QPolygonF
+from PyQt6.QtCore import QPointF, Qt
+import math
+
+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
+        
+        # 提取資訊 (格式: s{socket_seq}_{system_id}, 如 s0_11, s1_12)
+        parts = drone_id.split('_')
+        if len(parts) >= 2:
+            self.socket_seq = parts[0][1:]  # socket 序號 (移除 's' 前綴)
+            self.system_id = parts[1]       # system ID
+            self.display_id = f"ID:{self.system_id}"  # 顯示為 ID:11, ID:12
+        else:
+            self.socket_seq = "?"
+            self.system_id = "?"
+            self.display_id = drone_id
+        
+        self.attitude_indicator = None
+        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()
+        
+        # 右側態度指示器
+        attitude_widget = self._create_attitude_indicator()
+        
+        # 將 info 和 attitude 加入內容容器
+        content_layout.addWidget(info_widget, 1)
+        content_layout.addWidget(attitude_widget, 0)
+        
+        # 將內容容器加入主佈局
+        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;
+            }
+            QCheckBox::indicator {
+                width: 16px;
+                height: 16px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }
+            QCheckBox::indicator:checked {
+                background-color: #7FFFD4;
+                border: 2px solid #888888;
+            }
+        """)
+        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)
+        
+        # 第三行：高度
+        altitude_row = self._create_altitude_row()
+        info_layout.addWidget(altitude_row)
+        
+        # 第四行：航向 + 速度
+        nav_row = self._create_nav_row()
+        info_layout.addWidget(nav_row)
+        
+        return info_widget
+    
+    def _create_attitude_indicator(self):
+        """創建態度指示器（ADI 人工地平儀）"""
+        self.attitude_indicator = AttitudeIndicator(self.drone_id)
+        self.attitude_indicator.setFixedSize(90, 100)
+        return self.attitude_indicator
+    
+    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_connection_row(self):
+        """創建連接資訊行 (Socket Seq + 連接方式)"""
+        connection_row = QWidget()
+        connection_layout = QHBoxLayout(connection_row)
+        connection_layout.setContentsMargins(0, 0, 0, 0)
+        
+        connection_title = QLabel("Socket：")
+        connection_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        # 根據解析的 drone_id 資訊設定初始值
+        self.socket_seq_label = QLabel(self.socket_seq)
+        self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq")
+        self.socket_seq_label.setStyleSheet("color: #DDD;")
+        
+        connection_sep = QLabel(" - ")
+        connection_sep.setStyleSheet("color: #DDD;")
+        
+        # 設定連接方式顯示
+        connection_type_map = {
+            'r': 'ROS2',
+            'u': 'UDP',
+            's': 'Serial',
+            'w': 'WS'
+        }
+        connection_type = connection_type_map.get(self.type_prefix, 'Unknown')
+        
+        self.connection_type_label = QLabel(connection_type)
+        self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
+        self.connection_type_label.setStyleSheet("color: #DDD;")
+        
+        connection_layout.addWidget(connection_title)
+        connection_layout.addWidget(self.socket_seq_label)
+        connection_layout.addWidget(connection_sep)
+        connection_layout.addWidget(self.connection_type_label)
+        connection_layout.addStretch()
+        
+        return connection_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_pct_label = QLabel("--")
+        self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
+        self.battery_pct_label.setStyleSheet("color: #DDD;")
+
+        # 分隔符
+        separator1 = QLabel(" - ")
+        separator1.setStyleSheet("color: #DDD;")
+
+        # 顯示電壓
+        self.battery_vol_label = QLabel("--")
+        self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
+        self.battery_vol_label.setStyleSheet("color: #DDD;")
+
+        # 分隔符
+        separator2 = QLabel(" - ")
+        separator2.setStyleSheet("color: #DDD;")
+
+        # 顯示電池節數 (S count)
+        self.battery_cells_label = QLabel("--")
+        self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
+        self.battery_cells_label.setStyleSheet("color: #DDD;")
+
+        battery_layout.addWidget(battery_title)
+        battery_layout.addWidget(self.battery_pct_label)
+        battery_layout.addWidget(separator1)
+        battery_layout.addWidget(self.battery_vol_label)
+        battery_layout.addWidget(separator2)
+        battery_layout.addWidget(self.battery_cells_label)
+        battery_layout.addStretch()
+        
+        return battery_row
+    
+    def _create_altitude_row(self):
+        """創建高度和速度行"""
+        altitude_row = QWidget()
+        altitude_layout = QHBoxLayout(altitude_row)
+        altitude_layout.setContentsMargins(0, 0, 0, 0)
+        
+        altitude_title = QLabel("高度：")
+        altitude_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        self.altitude_label = QLabel("--")
+        self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
+        self.altitude_label.setStyleSheet("color: #DDD;")
+        
+        speed_title = QLabel("速度：")
+        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+        
+        self.speed_label = QLabel("--")
+        self.speed_label.setObjectName(f"{self.drone_id}_speed")
+        self.speed_label.setStyleSheet("color: #DDD;")
+        
+        altitude_layout.addWidget(altitude_title)
+        altitude_layout.addWidget(self.altitude_label)
+        altitude_layout.addWidget(speed_title)
+        altitude_layout.addWidget(self.speed_label)
+        altitude_layout.addStretch()
+        
+        return altitude_row
+    
+    def _create_position_row(self):
+        """位置行已移除（位置座標不再顯示於面板）。"""
+        return QWidget()
+    
+    def _create_nav_row(self):
+        """創建導航行（已移除，不再顯示）"""
+        return QWidget()
+    
+    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 set_connection_info(self, socket_seq, connection_type):
+        """設定連接資訊（Socket Seq 和連接方式）
+        connection_type: 'UDP' | 'Serial' | 'WS'
+        """
+        self.socket_seq_label.setText(str(socket_seq))
+        
+        # 顯示友善的連接方式
+        type_display = {
+            'UDP': 'UDP',
+            'Serial': 'Serial',
+            'WS': 'WS'
+        }.get(connection_type, connection_type)
+        
+        self.connection_type_label.setText(type_display)
+
+    
+    def update_attitude(self, heading, roll, pitch):
+        """更新態度指示器"""
+        if self.attitude_indicator:
+            self.attitude_indicator.update_attitude(heading, roll, pitch)
+    
+    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', socket_type=None, parent=None):
+        super().__init__(parent)
+        self.socket_id = socket_id
+        self.color = color
+        self.socket_type = socket_type
+        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 分組標題
+        if self.socket_type:
+            title_text = f"{self.socket_type} {self.socket_id}"
+        else:
+            title_text = f"Socket {self.socket_id}"
+        self.title_label = QLabel(title_text)
+        self.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(self.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_socket_type(self, conn_type):
+        """設置 socket 類型並更新標題"""
+        self.title_label.setText(f"{conn_type} {self.socket_id}")
+    
+    def set_check_state(self, state):
+        """設置分組勾選狀態（支持半選）"""
+        self.group_checkbox.setCheckState(state)
+
+
+class AttitudeIndicator(QWidget):
+    """
+    人工地平儀 (ADI) — 仿 Mission Planner 風格
+    上半部顯示 roll/pitch 人工地平儀，下方細條顯示航向
+    """
+
+    def __init__(self, drone_id, parent=None):
+        super().__init__(parent)
+        self.drone_id = drone_id
+        self.heading = 0.0   # 航向 yaw (0–360)
+        self.roll    = 0.0   # 滾轉 (deg, left- negative)
+        self.pitch   = 0.0   # 俯仰 (deg, nose-up positive)
+        self.setStyleSheet("background-color: transparent;")
+
+    def update_attitude(self, heading, roll, pitch):
+        self.heading = heading % 360
+        self.roll    = roll
+        self.pitch   = pitch
+        self.update()
+
+    # ------------------------------------------------------------------ helpers
+    def _adi_rect(self):
+        """Returns the square rect used for the ADI ball."""
+        w, h = self.width(), self.height()
+        side  = min(w, h - 14)   # leave 14 px at bottom for heading strip
+        x = (w - side) / 2
+        y = 0
+        return x, y, side, side
+
+    # ------------------------------------------------------------------ paint
+    def paintEvent(self, event):
+        p = QPainter(self)
+        p.setRenderHint(QPainter.RenderHint.Antialiasing)
+        self._draw_adi(p)
+        self._draw_heading_strip(p)
+
+    # ---- artificial horizon ------------------------------------------------
+    def _draw_adi(self, p):
+        from PyQt6.QtGui import QPainterPath
+        x0, y0, side, _ = self._adi_rect()
+        cx = x0 + side / 2
+        cy = y0 + side / 2
+        r  = side / 2 - 1
+
+        # clip to circle
+        clip_path = QPainterPath()
+        clip_path.addEllipse(QPointF(cx, cy), r, r)
+        p.setClipPath(clip_path)
+
+        # pixels-per-degree for pitch (10 deg ≈ side/5)
+        ppd = side / 50.0
+
+        # ---- rotate + translate canvas for roll & pitch
+        p.save()
+        p.translate(cx, cy)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
+        pitch_offset = self.pitch * ppd
+
+        # sky (above horizon)
+        sky_color = QColor(30, 100, 180)
+        p.fillRect(int(-r*2), int(-r*2 + pitch_offset), int(r*4), int(r*4), sky_color)
+
+        # ground (below horizon)
+        ground_color = QColor(140, 90, 40)
+        p.fillRect(int(-r*2), int(pitch_offset), int(r*4), int(r*4), ground_color)
+
+        # horizon line
+        p.setPen(QPen(QColor("#FFFFFF"), 2))
+        p.drawLine(int(-r), int(pitch_offset), int(r), int(pitch_offset))
+
+        # pitch ladder (every 10°, ±30°)
+        p.setPen(QPen(QColor(255, 255, 255, 180), 1))
+        p.setFont(QFont("Arial", 6))
+        for deg in range(-30, 31, 10):
+            if deg == 0:
+                continue
+            yy = int(pitch_offset - deg * ppd)
+            half = int(r * (0.35 if deg % 20 == 0 else 0.22))
+            p.drawLine(-half, yy, half, yy)
+
+        p.restore()
+        p.setClipping(False)
+
+        # ---- roll arc & tick marks (outside clip, fixed frame) ----
+        p.save()
+        p.translate(cx, cy)
+
+        arc_r = r - 2
+        p.setPen(QPen(QColor("#FFFFFF"), 1))
+        # draw arc from -60° to +60°  (Qt arc: 0=3o'clock, CCW, 16ths of deg)
+        p.drawArc(int(-arc_r), int(-arc_r), int(2*arc_r), int(2*arc_r),
+                  (90 - 60) * 16, 120 * 16)
+
+        # tick marks at 0, ±10, ±20, ±30, ±45, ±60
+        for deg in [0, 10, 20, 30, 45, 60, -10, -20, -30, -45, -60]:
+            rad = math.radians(deg - 90)
+            tick = 6 if deg % 30 == 0 else 4
+            x1 = arc_r * math.cos(rad)
+            y1 = arc_r * math.sin(rad)
+            x2 = (arc_r - tick) * math.cos(rad)
+            y2 = (arc_r - tick) * math.sin(rad)
+            p.drawLine(QPointF(x1, y1), QPointF(x2, y2))
+
+        # roll pointer triangle (rotates with roll)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
+        ptr_r = arc_r - 1
+        tri = QPolygonF([
+            QPointF(0, -ptr_r),
+            QPointF(-5, -ptr_r + 9),
+            QPointF(5,  -ptr_r + 9),
+        ])
+        p.setBrush(QColor("#FFFFFF"))
+        p.setPen(Qt.PenStyle.NoPen)
+        p.drawPolygon(tri)
+        p.restore()
+
+        # ---- fixed aircraft symbol ----
+        p.save()
+        p.translate(cx, cy)
+        p.setPen(QPen(QColor("#FFD700"), 2))
+        # left wing
+        p.drawLine(int(-r*0.5), 0, int(-r*0.15), 0)
+        p.drawLine(int(-r*0.15), 0, int(-r*0.15), int(r*0.12))
+        # right wing
+        p.drawLine(int(r*0.15), 0, int(r*0.5), 0)
+        p.drawLine(int(r*0.15), 0, int(r*0.15), int(r*0.12))
+        # centre dot
+        p.setBrush(QColor("#FFD700"))
+        p.drawEllipse(QPointF(0, 0), 2.5, 2.5)
+        p.restore()
+
+        # ---- outer ring ----
+        p.setPen(QPen(QColor("#888888"), 1))
+        p.setBrush(Qt.BrushStyle.NoBrush)
+        p.drawEllipse(QPointF(cx, cy), r, r)
+
+    # ---- heading strip at bottom ------------------------------------------
+    def _draw_heading_strip(self, p):
+        w         = self.width()
+        x0, y0, side, _ = self._adi_rect()
+        strip_y   = y0 + side
+        strip_h   = self.height() - strip_y
+        if strip_h < 4:
+            return
+
+        # background
+        p.fillRect(0, int(strip_y), w, strip_h, QColor(30, 30, 30))
+
+        # heading text centred (bigger)
+        p.setPen(QPen(QColor("#FFFFFF")))
+        p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
+        hdg_str = f"{int(self.heading)}°"
+        p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)

src/GUI/mission_executor.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+"""
+任務執行模組
+管理多架無人機的 GUIDED 模式飛行控制迴圈
+
+設計:
+    - 每架無人機持有一個航點序列，逐點推進
+    - 各自到達就各自切換到下一個航點
+    - 用 QTimer 驅動，在 Qt 主線程執行
+    - 暫停 = 停止發送 setpoint，飛控自動懸停
+    - 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep)
+"""
+import math
+from enum import Enum
+from PyQt6.QtCore import QObject, QTimer, pyqtSignal
+
+
+class MissionState(Enum):
+    """整體任務狀態"""
+    IDLE = "idle"
+    RUNNING = "running"
+    PAUSED = "paused"
+
+
+class DroneTask:
+    """單架無人機的任務資料"""
+    __slots__ = ('drone_id', 'sysid', 'waypoints', 'wp_index', 'done')
+
+    def __init__(self, drone_id, sysid, waypoints):
+        """
+        Args:
+            drone_id: GUI 用的 ID (如 's0_11')
+            sysid: MAVLink system ID (如 11)
+            waypoints: 航點序列 [(lat, lon, alt), ...]
+        """
+        self.drone_id = drone_id
+        self.sysid = sysid
+        self.waypoints = waypoints
+        self.wp_index = 0
+        self.done = len(waypoints) == 0
+
+    @property
+    def current_target(self):
+        if self.done or self.wp_index >= len(self.waypoints):
+            return None
+        return self.waypoints[self.wp_index]
+
+    @property
+    def total_waypoints(self):
+        return len(self.waypoints)
+
+
+class MissionExecutor(QObject):
+    """
+    任務執行器
+
+    planned_waypoints 格式:
+        {
+            'drone_ids': ['s0_1', 's0_2', ...],
+            'waypoints': [
+                [(lat,lon,alt), ...],   # drone 0
+                [(lat,lon,alt), ...],   # drone 1
+            ]
+        }
+    """
+
+    # 信號
+    drone_waypoint_reached = pyqtSignal(str, int, int)  # (drone_id, wp_index, total)
+    mission_completed = pyqtSignal()
+
+    def __init__(self, sender, drone_gps,
+                 arrival_radius=2.0, send_rate_hz=2.0):
+        super().__init__()
+        self.sender = sender
+        self.drone_gps = drone_gps
+        self.arrival_radius = arrival_radius
+        self.state = MissionState.IDLE
+        self.tasks = {}
+
+        self._interval_ms = int(1000 / send_rate_hz)
+        self._timer = QTimer()
+        self._timer.timeout.connect(self._tick)
+
+    # ------------------------------------------------------------------ 公開方法
+
+    def start(self, planned_waypoints):
+        """啟動任務"""
+        if self.state == MissionState.RUNNING:
+            print("任務已在執行中")
+            return
+
+        self.tasks.clear()
+
+        drone_ids = planned_waypoints['drone_ids']
+        waypoints_list = planned_waypoints['waypoints']
+
+        for i, drone_id in enumerate(drone_ids):
+            sysid = int(drone_id.split('_')[1])
+            self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i])
+
+        self.state = MissionState.RUNNING
+        self._timer.start(self._interval_ms)
+
+        total_wps = sum(t.total_waypoints for t in self.tasks.values())
+        print(f"任務啟動: {len(self.tasks)} 架無人機, "
+              f"共 {total_wps} 個航點, "
+              f"到達半徑={self.arrival_radius}m, "
+              f"發送週期={self._interval_ms}ms")
+
+    def pause(self):
+        """暫停任務"""
+        if self.state == MissionState.RUNNING:
+            self._timer.stop()
+            self.state = MissionState.PAUSED
+            print("任務暫停")
+
+    def resume(self):
+        """恢復任務"""
+        if self.state == MissionState.PAUSED:
+            self._timer.start(self._interval_ms)
+            self.state = MissionState.RUNNING
+            print("任務恢復")
+
+    def stop(self):
+        """停止任務"""
+        self._timer.stop()
+        self.tasks.clear()
+        self.state = MissionState.IDLE
+        print("任務停止")
+
+    # ------------------------------------------------------------------ 控制迴圈
+
+    def _tick(self):
+        """控制迴圈"""
+        all_done = True
+
+        for task in self.tasks.values():
+            if task.done:
+                continue
+
+            all_done = False
+            target = task.current_target
+            if target is None:
+                continue
+
+            # 讀取當前 GPS
+            gps = self.drone_gps.get(task.drone_id)
+            if gps is None:
+                continue
+
+            tgt_lat, tgt_lon, tgt_alt = target
+            distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
+
+            # 到達判定
+            if distance < self.arrival_radius:
+                task.wp_index += 1
+                if task.wp_index >= task.total_waypoints:
+                    task.done = True
+                    self.drone_waypoint_reached.emit(
+                        task.drone_id, task.wp_index, task.total_waypoints
+                    )
+                    print(f"  {task.drone_id} → DONE "
+                          f"({task.total_waypoints}/{task.total_waypoints})")
+                    continue
+                else:
+                    self.drone_waypoint_reached.emit(
+                        task.drone_id, task.wp_index, task.total_waypoints
+                    )
+                    print(f"  {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
+                          f"(距離: {distance:.1f}m)")
+                    # 更新目標
+                    tgt_lat, tgt_lon, tgt_alt = task.current_target
+
+            # 發送 setpoint
+            self.sender.send_position_global(
+                task.sysid, tgt_lat, tgt_lon, tgt_alt
+            )
+
+        # 全部完成檢查
+        if all_done and self.tasks:
+            self._timer.stop()
+            self.state = MissionState.IDLE
+            self.mission_completed.emit()
+            print("===== 任務全部完成 =====")
+
+
+# ------------------------------------------------------------------ 工具函式
+
+def _haversine(lat1, lon1, lat2, lon2):
+    """計算兩個 GPS 座標間的水平距離 (公尺)"""
+    R = 6371000.0
+    lat1_r = math.radians(lat1)
+    lat2_r = math.radians(lat2)
+    dlat = math.radians(lat2 - lat1)
+    dlon = math.radians(lon2 - lon1)
+
+    a = (math.sin(dlat / 2) ** 2 +
+         math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
+    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

src/GUI/mission_group.py

@@ -0,0 +1,519 @@
+#!/usr/bin/env python3
+"""
+任務群組模組
+管理多任務群組的資料結構與無人機分配對話框
+"""
+from PyQt6.QtWidgets import (
+    QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton,
+    QComboBox, QDialog, QCheckBox, QScrollArea, QFrame, QLineEdit
+)
+from PyQt6.QtCore import Qt, pyqtSignal
+from mission_executor import MissionExecutor, MissionState
+
+
+# 群組顏色（循環使用）
+GROUP_COLORS = [
+    '#4A9EFF',  # 藍
+    '#FF8C42',  # 橘
+    '#56C87A',  # 綠
+    '#E85D75',  # 紅
+    '#B07CED',  # 紫
+    '#F5C542',  # 黃
+    '#42C9C9',  # 青
+    '#FF6B9D',  # 粉
+]
+
+
+class MissionGroup:
+    """單一任務群組的資料"""
+
+    def __init__(self, group_id, color):
+        self.group_id = group_id          # 'A', 'B', 'C', ...
+        self.color = color                # 顏色 hex
+        self.drone_ids = set()            # 已分配的無人機 ID
+        self.mission_type = 'M_FORMATION' # 預設任務類型
+        self.planned_waypoints = None     # 規劃結果 dict
+        self.executor = None              # MissionExecutor 實例（延遲建立）
+        self.center_origin = None         # 規劃原點
+
+    @property
+    def state(self):
+        if self.executor is None:
+            return MissionState.IDLE
+        return self.executor.state
+
+    @property
+    def display_name(self):
+        return f"Group {self.group_id}"
+
+
+class DroneAssignDialog(QDialog):
+    """無人機分配對話框"""
+
+    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned):
+        """
+        Args:
+            parent: 父 widget
+            all_drone_ids: 所有可用無人機 ID 列表
+            current_assigned: 當前群組已分配的無人機 set
+            other_assigned: 其他群組已佔用的無人機 dict {drone_id: group_id}
+        """
+        super().__init__(parent)
+        self.setWindowTitle("分配無人機")
+        self.setMinimumWidth(280)
+        self.setStyleSheet("""
+            QDialog { background-color: #2B2B2B; }
+            QLabel { color: #DDD; }
+            QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
+            QCheckBox::indicator { width: 16px; height: 16px; }
+            QCheckBox:disabled { color: #666; }
+        """)
+
+        layout = QVBoxLayout(self)
+
+        title = QLabel("選擇要分配到此群組的無人機：")
+        title.setStyleSheet("font-size: 13px; font-weight: bold; padding-bottom: 6px;")
+        layout.addWidget(title)
+
+        # 滾動區域
+        scroll = QScrollArea()
+        scroll.setWidgetResizable(True)
+        scroll.setMaximumHeight(300)
+        scroll_widget = QWidget()
+        scroll_layout = QVBoxLayout(scroll_widget)
+        scroll_layout.setSpacing(2)
+
+        self.checkboxes = {}
+        sorted_ids = sorted(all_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+
+        for drone_id in sorted_ids:
+            cb = QCheckBox(drone_id)
+
+            if drone_id in current_assigned:
+                cb.setChecked(True)
+            elif drone_id in other_assigned:
+                cb.setEnabled(False)
+                cb.setToolTip(f"已分配到 Group {other_assigned[drone_id]}")
+                cb.setText(f"{drone_id}  (Group {other_assigned[drone_id]})")
+
+            self.checkboxes[drone_id] = cb
+            scroll_layout.addWidget(cb)
+
+        scroll_layout.addStretch()
+        scroll.setWidget(scroll_widget)
+        layout.addWidget(scroll)
+
+        # 按鈕
+        btn_layout = QHBoxLayout()
+        ok_btn = QPushButton("確定")
+        ok_btn.setStyleSheet("""
+            QPushButton { background-color: #4A9EFF; color: white; border: none;
+                padding: 8px 20px; border-radius: 4px; font-weight: bold; }
+            QPushButton:hover { background-color: #3A8EEF; }
+        """)
+        ok_btn.clicked.connect(self.accept)
+
+        cancel_btn = QPushButton("取消")
+        cancel_btn.setStyleSheet("""
+            QPushButton { background-color: #555; color: #DDD; border: none;
+                padding: 8px 20px; border-radius: 4px; }
+            QPushButton:hover { background-color: #666; }
+        """)
+        cancel_btn.clicked.connect(self.reject)
+
+        btn_layout.addStretch()
+        btn_layout.addWidget(cancel_btn)
+        btn_layout.addWidget(ok_btn)
+        layout.addLayout(btn_layout)
+
+    def get_selected(self):
+        """回傳勾選的無人機 ID set"""
+        return {did for did, cb in self.checkboxes.items() if cb.isChecked() and cb.isEnabled()}
+
+
+class GroupPanel(QWidget):
+    """單一群組的 UI 面板（嵌入到 tab 中）— 三欄式佈局"""
+
+    # 信號
+    assign_drones_requested = pyqtSignal(str)          # group_id
+    mission_type_changed = pyqtSignal(str, str)        # group_id, mission_type
+    start_requested = pyqtSignal(str)                  # group_id
+    pause_requested = pyqtSignal(str)                  # group_id
+    stop_requested = pyqtSignal(str)                   # group_id
+    mode_change_requested = pyqtSignal(str, str)       # group_id, mode
+    arm_requested = pyqtSignal(str)                    # group_id
+    takeoff_requested = pyqtSignal(str, float)         # group_id, altitude
+    box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
+    select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
+    clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
+    add_group_requested = pyqtSignal()                 # 新增群組
+    delete_group_requested = pyqtSignal(str)           # group_id — 刪除群組
+
+    BUTTON_STYLE = """
+        QPushButton {{ background-color: {bg}; color: {fg}; border: none;
+            padding: 5px 8px; border-radius: 4px; font-size: 11px; }}
+        QPushButton:hover {{ background-color: {hover}; }}
+        QPushButton:disabled {{ background-color: #444; color: #666; }}
+    """
+
+    def __init__(self, group: MissionGroup, parent=None):
+        super().__init__(parent)
+        self.group = group
+        self._is_all_selected = False  # 追蹤全選狀態
+        self.all_btn_ref = None  # 保存全選按鈕的參考
+        self._build_ui()
+
+    def _make_sep(self):
+        """建立垂直分隔線"""
+        sep = QFrame()
+        sep.setFrameShape(QFrame.Shape.VLine)
+        sep.setStyleSheet("color: #444;")
+        return sep
+
+    def _build_ui(self):
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(6, 4, 6, 4)
+        layout.setSpacing(0)
+
+        COMBO = ("QComboBox { background-color: #333; color: #DDD; "
+                 "border-radius: 3px; padding: 2px 6px; font-size: 11px; }")
+        BTN = self.BUTTON_STYLE
+        LBL = "color: #AAA; font-size: 11px;"
+        TITLE = "color: #DDD; font-size: 11px; font-weight: bold; padding-bottom: 2px;"
+
+        # ── 三欄主佈局 ──
+        cols = QHBoxLayout()
+        cols.setSpacing(6)
+
+        # ============================
+        # 左欄：控制指令
+        # ============================
+        left = QVBoxLayout()
+        left.setSpacing(3)
+
+        left_title = QLabel("控制指令")
+        left_title.setStyleSheet(TITLE)
+        left.addWidget(left_title)
+
+        # 模式切換
+        mode_row = QHBoxLayout()
+        mode_row.setSpacing(3)
+        self.mode_combo = QComboBox()
+        self.mode_combo.addItems([
+            "GUIDED", "AUTO", "LAND", "LOITER",
+            "STABILIZE", "ALT_HOLD", "RTL", "POSHOLD", "SMART_RTL"
+        ])
+        self.mode_combo.setStyleSheet(COMBO)
+        mode_btn = QPushButton("切換")
+        mode_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        mode_btn.clicked.connect(
+            lambda: self.mode_change_requested.emit(
+                self.group.group_id, self.mode_combo.currentText()))
+        mode_row.addWidget(self.mode_combo, 1)
+        mode_row.addWidget(mode_btn)
+        left.addLayout(mode_row)
+
+        # 解鎖
+        arm_btn = QPushButton("解鎖")
+        arm_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        arm_btn.clicked.connect(
+            lambda: self.arm_requested.emit(self.group.group_id))
+        left.addWidget(arm_btn)
+
+        # 起飛
+        takeoff_row = QHBoxLayout()
+        takeoff_row.setSpacing(3)
+        self.alt_input = QComboBox()
+        self.alt_input.setEditable(True)
+        self.alt_input.addItems(["5", "10", "15", "20"])
+        self.alt_input.setCurrentText("10")
+        self.alt_input.setStyleSheet(COMBO)
+        alt_lbl = QLabel("m")
+        alt_lbl.setStyleSheet(LBL)
+        takeoff_btn = QPushButton("起飛")
+        takeoff_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        takeoff_btn.clicked.connect(self._on_takeoff)
+        takeoff_row.addWidget(self.alt_input, 1)
+        takeoff_row.addWidget(alt_lbl)
+        takeoff_row.addWidget(takeoff_btn)
+        left.addLayout(takeoff_row)
+
+        left.addStretch()
+
+        # ============================
+        # 中欄：任務規劃（左右分割）
+        # ============================
+        mid = QVBoxLayout()
+        mid.setSpacing(2)
+
+        mid_title = QLabel("任務規劃")
+        mid_title.setStyleSheet(TITLE)
+        mid.addWidget(mid_title)
+
+        mid_body = QHBoxLayout()
+        mid_body.setSpacing(4)
+
+        # 左側：類型 + 狀態 + 座標
+        mid_left = QVBoxLayout()
+        mid_left.setSpacing(2)
+        self.type_combo = QComboBox()
+        self.type_combo.addItems([
+            "M_FORMATION", "CIRCLE_FORMATION", "LEADER_FOLLOWER", "GRID_SWEEP"
+        ])
+        self.type_combo.setStyleSheet(COMBO)
+        self.type_combo.currentTextChanged.connect(
+            lambda t: self.mission_type_changed.emit(self.group.group_id, t))
+        mid_left.addWidget(self.type_combo)
+        self.status_label = QLabel("○ 未規劃")
+        self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+        mid_left.addWidget(self.status_label)
+        self.center_label = QLabel("中心: --")
+        self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        mid_left.addWidget(self.center_label)
+        self.target_label = QLabel("目標: --")
+        self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        mid_left.addWidget(self.target_label)
+        mid_left.addStretch()
+
+        # 右側：執行 / 暫停 / 停止（垂直排列）
+        mid_right = QVBoxLayout()
+        mid_right.setSpacing(3)
+        self.start_btn = QPushButton("▶ 執行")
+        self.start_btn.setStyleSheet(BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
+        self.start_btn.clicked.connect(
+            lambda: self.start_requested.emit(self.group.group_id))
+        self.pause_btn = QPushButton("⏸ 暫停")
+        self.pause_btn.setStyleSheet(BTN.format(bg='#F57F17', fg='white', hover='#F9A825'))
+        self.pause_btn.clicked.connect(
+            lambda: self.pause_requested.emit(self.group.group_id))
+        self.stop_btn = QPushButton("■ 停止")
+        self.stop_btn.setStyleSheet(BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
+        self.stop_btn.clicked.connect(
+            lambda: self.stop_requested.emit(self.group.group_id))
+        mid_right.addWidget(self.start_btn)
+        mid_right.addWidget(self.pause_btn)
+        mid_right.addWidget(self.stop_btn)
+        mid_right.addStretch()
+
+        mid_body.addLayout(mid_left, 1)
+        mid_body.addLayout(mid_right)
+        mid.addLayout(mid_body)
+
+        # ============================
+        # 選取與分組（3x2 按鈕）
+        # ============================
+        right = QVBoxLayout()
+        right.setSpacing(3)
+
+        right_title = QLabel("選取與分組")
+        right_title.setStyleSheet(TITLE)
+        right.addWidget(right_title)
+
+        self.drone_list_label = QLabel("尚未分配")
+        self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        self.drone_list_label.setWordWrap(True)
+        right.addWidget(self.drone_list_label)
+
+        # 3x2 按鈕網格：第一行 框選 全選 新增群組
+        grid_r1 = QHBoxLayout()
+        grid_r1.setSpacing(3)
+        box_btn = QPushButton("框選")
+        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        box_btn.clicked.connect(
+            lambda: self.box_select_requested.emit(self.group.group_id))
+        all_btn = QPushButton("全選/取消")
+        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        all_btn.clicked.connect(self._on_all_select_clicked)
+        self.all_btn_ref = all_btn  # 保存按鈕參考（備用）
+        add_group_btn = QPushButton("+ 新增群組")
+        add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
+        add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
+        grid_r1.addWidget(box_btn)
+        grid_r1.addWidget(all_btn)
+        grid_r1.addWidget(add_group_btn)
+        right.addLayout(grid_r1)
+
+        # 第二行 編輯分配 清除分組 刪除群組
+        grid_r2 = QHBoxLayout()
+        grid_r2.setSpacing(3)
+        assign_btn = QPushButton("編輯分配")
+        assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        assign_btn.clicked.connect(
+            lambda: self.assign_drones_requested.emit(self.group.group_id))
+        clear_btn = QPushButton("清除分組")
+        clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
+        clear_btn.clicked.connect(
+            lambda: self.clear_group_requested.emit(self.group.group_id))
+        delete_group_btn = QPushButton("刪除群組")
+        delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        delete_group_btn.clicked.connect(
+            lambda: self.delete_group_requested.emit(self.group.group_id))
+        grid_r2.addWidget(assign_btn)
+        grid_r2.addWidget(clear_btn)
+        grid_r2.addWidget(delete_group_btn)
+        right.addLayout(grid_r2)
+
+        right.addStretch()
+
+        # ============================
+        # 第四欄：任務參數
+        # ============================
+        param_col = QVBoxLayout()
+        param_col.setSpacing(2)
+
+        param_title = QLabel("任務參數")
+        param_title.setStyleSheet(TITLE)
+        param_col.addWidget(param_title)
+
+        INPUT = ("QLineEdit { background-color: #333; color: #DDD; "
+                 "border: 1px solid #555; border-radius: 3px; "
+                 "padding: 1px 4px; font-size: 11px; }")
+
+        # 每種任務類型的參數定義: (key, label, default_value)
+        self._param_defs = {
+            'M_FORMATION': [
+                ('spacing', '間距 (m)', '5.0'),
+                ('base_altitude', '基準高度 (m)', '10.0'),
+                ('altitude_diff', '高低差 (m)', '2.0'),
+            ],
+            'CIRCLE_FORMATION': [
+                ('radius', '半徑 (m)', '10.0'),
+                ('altitude', '高度 (m)', '10.0'),
+                ('start_angle', '起始角 (°)', '0'),
+            ],
+            'LEADER_FOLLOWER': [
+                ('lateral_offset', '橫向偏移 (m)', '3.0'),
+                ('longitudinal_spacing', '縱向間距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
+            ],
+            'GRID_SWEEP': [
+                ('line_spacing', '掃描線距 (m)', '5.0'),
+                ('altitude', '高度 (m)', '10.0'),
+            ],
+        }
+
+        # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
+        self._param_widgets = {}  # key → (label_widget, input_widget)
+        self._param_rows = []     # 所有 row layout 對應的 container widget
+
+        for mission_type, defs in self._param_defs.items():
+            for key, label_text, default in defs:
+                if key in self._param_widgets:
+                    continue  # 同名參數只建一次
+                row_w = QWidget()
+                row_l = QHBoxLayout(row_w)
+                row_l.setContentsMargins(0, 0, 0, 0)
+                row_l.setSpacing(3)
+                lbl = QLabel(label_text)
+                lbl.setStyleSheet(LBL)
+                inp = QLineEdit(default)
+                inp.setStyleSheet(INPUT)
+                inp.setFixedWidth(50)
+                row_l.addWidget(lbl, 1)
+                row_l.addWidget(inp)
+                param_col.addWidget(row_w)
+                self._param_widgets[key] = (row_w, inp)
+                self._param_rows.append(row_w)
+
+        param_col.addStretch()
+
+        # 初始顯示對應的參數
+        self._update_param_visibility()
+
+        # 當任務類型切換時更新參數顯示
+        self.type_combo.currentTextChanged.connect(self._update_param_visibility)
+
+        # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
+        # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+        cols.addLayout(left, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(mid, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(param_col, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(right, 0)
+        cols.addStretch()  # 填充剩餘空間，使四列置左
+
+        layout.addLayout(cols)
+
+    def update_drone_list(self):
+        """更新無人機列表顯示"""
+        if not self.group.drone_ids:
+            self.drone_list_label.setText("尚未分配")
+            self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        else:
+            sorted_ids = sorted(self.group.drone_ids,
+                                key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+            self.drone_list_label.setText("、".join(sorted_ids))
+            self.drone_list_label.setStyleSheet(
+                f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+
+    def update_status(self):
+        """更新任務狀態顯示"""
+        state = self.group.state
+        if self.group.planned_waypoints is None:
+            self.status_label.setText("○ 未規劃")
+            self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+        elif state == MissionState.RUNNING:
+            self.status_label.setText("▶ 執行中")
+            self.status_label.setStyleSheet("color: #4CAF50; font-size: 11px; font-weight: bold;")
+        elif state == MissionState.PAUSED:
+            self.status_label.setText("⏸ 已暫停")
+            self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
+        else:
+            n = len(self.group.drone_ids)
+            total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
+            self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
+            self.status_label.setStyleSheet(
+                f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+
+    def _on_all_select_clicked(self):
+        """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
+        self.select_all_requested.emit(self.group.group_id)
+    
+    def set_all_select_state(self, is_selected):
+        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
+        self._is_all_selected = is_selected
+
+    def _update_param_visibility(self, _=None):
+        """根據當前任務類型，顯示/隱藏對應的參數列"""
+        mission_type = self.type_combo.currentText()
+        visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        for key, (row_w, _inp) in self._param_widgets.items():
+            row_w.setVisible(key in visible_keys)
+
+    def get_mission_params(self):
+        """讀取當前顯示的參數值，回傳 dict"""
+        mission_type = self.type_combo.currentText()
+        params = {}
+        for key, _label, default in self._param_defs.get(mission_type, []):
+            if key in self._param_widgets:
+                _row_w, inp = self._param_widgets[key]
+                try:
+                    params[key] = float(inp.text())
+                except ValueError:
+                    params[key] = float(default)
+        return params
+
+    def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
+        """更新中心點 / 目標點顯示"""
+        info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
+        self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
+        self.center_label.setStyleSheet(info_style)
+        self.target_label.setText(f"目標: {target_lat:.6f}°, {target_lon:.6f}°")
+        self.target_label.setStyleSheet(info_style)
+
+    def clear_mission_info(self):
+        """清除中心點 / 目標點顯示"""
+        self.center_label.setText("中心: --")
+        self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        self.target_label.setText("目標: --")
+        self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+
+    def _on_takeoff(self):
+        try:
+            alt = float(self.alt_input.currentText())
+        except ValueError:
+            alt = 10.0
+        self.takeoff_requested.emit(self.group.group_id, alt)
+

src/GUI/mission_planner.py

@@ -0,0 +1,538 @@
+#!/usr/bin/env python3
+"""
+飛行任務規劃模組
+支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep
+"""
+import math
+from typing import List, Tuple, Optional, Dict, Any
+from enum import Enum
+
+
+class MissionType(Enum):
+    """任務類型"""
+    M_FORMATION = "m_formation"
+    CIRCLE_FORMATION = "circle_formation"
+    LEADER_FOLLOWER = "leader_follower"
+    GRID_SWEEP = "grid_sweep"
+
+
+class CoordinateConverter:
+    """GPS 座標與 Local 座標的轉換器"""
+
+    def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
+        self.origin_lat = origin_lat
+        self.origin_lon = origin_lon
+        self.origin_alt = origin_alt
+        self.R = 6371000.0
+
+    def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
+        lat_rad = math.radians(lat)
+        lon_rad = math.radians(lon)
+        origin_lat_rad = math.radians(self.origin_lat)
+        origin_lon_rad = math.radians(self.origin_lon)
+
+        dlat = lat_rad - origin_lat_rad
+        dlon = lon_rad - origin_lon_rad
+
+        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
+        y = self.R * dlat
+        z = alt - self.origin_alt
+
+        return x, y, z
+
+    def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
+        origin_lat_rad = math.radians(self.origin_lat)
+        origin_lon_rad = math.radians(self.origin_lon)
+
+        lat_rad = origin_lat_rad + (y / self.R)
+        lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
+
+        lat = math.degrees(lat_rad)
+        lon = math.degrees(lon_rad)
+        alt = self.origin_alt + z
+
+        return lat, lon, alt
+
+
+class FormationPlanner:
+    """隊形規劃器"""
+
+    def __init__(self, spacing: float = 5.0,
+                 base_altitude: float = 10.0,
+                 altitude_diff: float = 2.0):
+        self.spacing = spacing
+        self.base_altitude = base_altitude
+        self.altitude_diff = altitude_diff
+        self.current_origin = None
+        self.converter = None
+
+    def plan_formation_mission(self,
+                               drone_gps_positions: List[Tuple[float, float, float]],
+                               target_gps: Tuple[float, float, float],
+                               mission_type: MissionType = MissionType.M_FORMATION,
+                               params: Optional[Dict[str, Any]] = None
+                               ) -> Tuple[List[List[Tuple[float, float, float]]],
+                                          Tuple[float, float, float]]:
+        if len(drone_gps_positions) == 0:
+            raise ValueError("無人機位置列表不能為空")
+
+        center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
+        center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
+        center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
+
+        self.current_origin = (center_lat, center_lon, center_alt)
+        self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
+
+        drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
+        target_local = self.converter.gps_to_local(*target_gps)
+
+        if mission_type == MissionType.M_FORMATION:
+            s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+
+        elif mission_type == MissionType.CIRCLE_FORMATION:
+            s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+
+        elif mission_type == MissionType.LEADER_FOLLOWER:
+            params = params or {}
+            route_wps_gps = params.get('route_waypoints')
+            if route_wps_gps is None or len(route_wps_gps) < 2:
+                raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
+            route_wps_local = [
+                self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
+                for wp in route_wps_gps
+            ]
+            waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params)
+
+        elif mission_type == MissionType.GRID_SWEEP:
+            params = params or {}
+            rect_corners_gps = params.get('rect_corners')
+            if rect_corners_gps is None or len(rect_corners_gps) != 4:
+                raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
+            rect_corners_local = [
+                self.converter.gps_to_local(c[0], c[1], 0)[:2]
+                for c in rect_corners_gps
+            ]
+            waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params)
+        else:
+            raise ValueError(f"不支援的任務類型: {mission_type}")
+
+        waypoints_gps = []
+        for drone_wps in waypoints_local:
+            gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
+            waypoints_gps.append(gps_wps)
+
+        return waypoints_gps, self.current_origin
+
+    # ------------------------------------------------------------------ M-Formation
+
+    def _calculate_m_formation(self, drone_positions, target_point, params):
+        params = params or {}
+        N = len(drone_positions)
+        spacing = params.get('spacing', self.spacing)
+        base_altitude = params.get('base_altitude', self.base_altitude)
+        altitude_diff = params.get('altitude_diff', self.altitude_diff)
+
+        C_x = sum(pos[0] for pos in drone_positions) / N
+        C_y = sum(pos[1] for pos in drone_positions) / N
+
+        T_x, T_y, T_z = target_point
+        V_x, V_y = T_x - C_x, T_y - C_y
+        P_x, P_y = -V_y, V_x
+
+        length = math.sqrt(P_x ** 2 + P_y ** 2)
+        P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
+
+        projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
+                       for i, pos in enumerate(drone_positions)]
+        projections.sort()
+
+        stage1_positions = [None] * N
+        stage2_positions = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            offset = (rank - (N - 1) / 2) * spacing
+            altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
+            stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
+            stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
+
+        return stage1_positions, stage2_positions
+
+    # ------------------------------------------------------------------ Circle
+
+    def _calculate_circle_formation(self, drone_positions, target_point, params):
+        params = params or {}
+        N = len(drone_positions)
+        radius = params.get('radius', 10.0)
+        altitude = params.get('altitude', 10.0)
+        start_angle = params.get('start_angle', 0.0)
+
+        center_x, center_y, center_z = target_point
+        stage1_positions = []
+        stage2_positions = []
+        angle_step = 360.0 / N
+
+        for i in range(N):
+            angle_rad = math.radians(start_angle + angle_step * i)
+            final_x = center_x + radius * math.cos(angle_rad)
+            final_y = center_y + radius * math.sin(angle_rad)
+            final_z = altitude
+
+            current_x, current_y, current_z = drone_positions[i]
+            stage1_positions.append((
+                current_x + (final_x - current_x) * 0.5,
+                current_y + (final_y - current_y) * 0.5,
+                current_z + (final_z - current_z) * 0.5
+            ))
+            stage2_positions.append((final_x, final_y, final_z))
+
+        return stage1_positions, stage2_positions
+
+    # ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎)
+
+    def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
+        """
+        路徑跟隨編隊 — Bezier 曲線轉彎版
+
+        步驟:
+            1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
+            2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
+
+        隊形 (俯視):
+            |     (前進方向)         |
+            |           D1          |  ← 左偏移, 後 0m
+            |                D2     |  ← 右偏移, 後 5m
+            |           D3          |  ← 左偏移, 後 10m
+            |                D4     |  ← 右偏移, 後 15m
+        """
+        N = len(drone_positions)
+        lateral_offset = params.get('lateral_offset', 3.0)
+        longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
+        altitude = params.get('altitude', self.base_altitude)
+        turn_margin = params.get('turn_margin', 0.35)      # 轉彎切入距離佔段長比例
+        curve_resolution = params.get('curve_resolution', 8)  # 每個彎道的插值點數
+
+        # Step 1: 建立帶 Bezier 轉彎的中心路徑
+        center_path = self._build_center_path(
+            route_wps_local, turn_margin, curve_resolution
+        )
+
+        # Step 2: 固定排序
+        first_dir = (center_path[0][2], center_path[0][3])
+        first_perp = (-first_dir[1], first_dir[0])
+        C_x = sum(p[0] for p in drone_positions) / N
+        C_y = sum(p[1] for p in drone_positions) / N
+
+        projections = [
+            ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
+            for i, pos in enumerate(drone_positions)
+        ]
+        projections.sort()
+
+        # Step 3: 偏移
+        all_waypoints = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            lat_sign = -1 if rank % 2 == 0 else 1
+            lat = lat_sign * lateral_offset
+            lon = rank * longitudinal_spacing
+
+            waypoints = []
+            for (cx, cy, dx, dy) in center_path:
+                perp_x, perp_y = -dy, dx
+                ox = cx + lat * perp_x - lon * dx
+                oy = cy + lat * perp_y - lon * dy
+                waypoints.append((ox, oy, altitude))
+
+            all_waypoints[original_idx] = waypoints
+
+        return all_waypoints
+
+    def _build_center_path(self, waypoints, turn_margin, curve_resolution):
+        """
+        建立帶 Bezier 曲線轉彎的中心路徑
+
+        在每個轉折 WP 處:
+            1. 計算 pre_turn = WP - d_in × T
+            2. 計算 post_turn = WP + d_out × T
+            3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post
+            4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP)
+
+        Args:
+            waypoints: 路徑航點 [(x, y), ...]
+            turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
+            curve_resolution: 每個彎道的 Bezier 插值點數
+
+        Returns:
+            [(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向
+        """
+        num_wps = len(waypoints)
+
+        if num_wps < 2:
+            return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)]
+
+        # 計算每段方向和長度
+        segments = []
+        for i in range(num_wps - 1):
+            dx = waypoints[i + 1][0] - waypoints[i][0]
+            dy = waypoints[i + 1][1] - waypoints[i][1]
+            length = math.sqrt(dx ** 2 + dy ** 2)
+            if length < 0.01:
+                segments.append((0.0, 1.0, length))
+            else:
+                segments.append((dx / length, dy / length, length))
+
+        path = []
+
+        # 第一個航點
+        path.append((waypoints[0][0], waypoints[0][1],
+                      segments[0][0], segments[0][1]))
+
+        # 中間航點 (轉彎處)
+        for i in range(1, num_wps - 1):
+            d_in_x, d_in_y, len_in = segments[i - 1]
+            d_out_x, d_out_y, len_out = segments[i]
+
+            # 切入距離 T: 取相鄰兩段中較短的 × turn_margin
+            T = min(len_in, len_out) * turn_margin
+
+            if T < 0.5:
+                # 段太短，不插彎，直接加一個平均方向點
+                avg_dx = d_in_x + d_out_x
+                avg_dy = d_in_y + d_out_y
+                avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
+                if avg_len > 0.01:
+                    avg_dx /= avg_len
+                    avg_dy /= avg_len
+                else:
+                    avg_dx, avg_dy = d_in_x, d_in_y
+                path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy))
+                continue
+
+            # P0 = pre_turn (弧線起始)
+            p0_x = waypoints[i][0] - d_in_x * T
+            p0_y = waypoints[i][1] - d_in_y * T
+
+            # P1 = WP 本身 (控制點)
+            p1_x = waypoints[i][0]
+            p1_y = waypoints[i][1]
+
+            # P2 = post_turn (弧線結束)
+            p2_x = waypoints[i][0] + d_out_x * T
+            p2_y = waypoints[i][1] + d_out_y * T
+
+            # 加入 pre_turn 點 (方向 = incoming)
+            path.append((p0_x, p0_y, d_in_x, d_in_y))
+
+            # 加入 Bezier 插值點
+            for j in range(1, curve_resolution):
+                t = j / curve_resolution
+
+                # B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2
+                one_minus_t = 1.0 - t
+                bx = one_minus_t * one_minus_t * p0_x + \
+                     2 * t * one_minus_t * p1_x + \
+                     t * t * p2_x
+                by = one_minus_t * one_minus_t * p0_y + \
+                     2 * t * one_minus_t * p1_y + \
+                     t * t * p2_y
+
+                # B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向
+                tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x)
+                tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y)
+
+                # 正規化
+                t_len = math.sqrt(tdx ** 2 + tdy ** 2)
+                if t_len > 0.01:
+                    tdx /= t_len
+                    tdy /= t_len
+                else:
+                    tdx, tdy = d_in_x, d_in_y
+
+                path.append((bx, by, tdx, tdy))
+
+            # 加入 post_turn 點 (方向 = outgoing)
+            path.append((p2_x, p2_y, d_out_x, d_out_y))
+
+        # 最後一個航點
+        path.append((waypoints[-1][0], waypoints[-1][1],
+                      segments[-1][0], segments[-1][1]))
+
+        return path
+
+    # ------------------------------------------------------------------ 柵狀掃描
+
+    def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
+        """柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊"""
+        N = len(drone_positions)
+        line_spacing = params.get('line_spacing', 5.0)
+        altitude = params.get('altitude', self.base_altitude)
+
+        c0, c1, c2, c3 = rect_corners_local
+
+        edge_a = (c1[0] - c0[0], c1[1] - c0[1])
+        len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
+        edge_b = (c3[0] - c0[0], c3[1] - c0[1])
+        len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)
+
+        if len_a >= len_b:
+            sweep_vec = edge_a
+            sweep_len = len_a
+            cut_vec = edge_b
+            cut_len = len_b
+            sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
+            sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
+            cut_start_corner = c0
+        else:
+            sweep_vec = edge_b
+            sweep_len = len_b
+            cut_vec = edge_a
+            cut_len = len_a
+            sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
+            sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
+            cut_start_corner = c0
+
+        sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
+        cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)
+
+        C_x = sum(p[0] for p in drone_positions) / N
+        C_y = sum(p[1] for p in drone_positions) / N
+
+        dist_to_start = math.sqrt(
+            (C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
+        )
+        dist_to_end = math.sqrt(
+            (C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
+        )
+
+        if dist_to_start <= dist_to_end:
+            near_corner_a = cut_start_corner
+        else:
+            sweep_dir = (-sweep_dir[0], -sweep_dir[1])
+            near_corner_a = (cut_start_corner[0] + sweep_vec[0],
+                            cut_start_corner[1] + sweep_vec[1])
+
+        projections = [
+            ((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
+            for i, pos in enumerate(drone_positions)
+        ]
+        projections.sort()
+
+        strip_width = cut_len / N
+        drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
+        near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
+        gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2
+
+        all_waypoints = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            strip_center_offset = (rank + 0.5) * strip_width
+            base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
+            base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset
+
+            waypoints_list = []
+
+            gather_offset = gather_sweep_proj - near_sweep_proj
+            gx = base_x + sweep_dir[0] * gather_offset
+            gy = base_y + sweep_dir[1] * gather_offset
+            waypoints_list.append((gx, gy, altitude))
+
+            num_lines = max(1, int(strip_width / line_spacing))
+            actual_spacing = strip_width / num_lines
+            first_line_offset = (rank * strip_width) + actual_spacing / 2
+
+            entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
+            entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
+            waypoints_list.append((entry_x, entry_y, altitude))
+
+            current_cut_offset = first_line_offset
+
+            for line_idx in range(num_lines):
+                line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
+                line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
+                line_far_x = line_near_x + sweep_dir[0] * sweep_len
+                line_far_y = line_near_y + sweep_dir[1] * sweep_len
+
+                if line_idx % 2 == 0:
+                    waypoints_list.append((line_far_x, line_far_y, altitude))
+                else:
+                    waypoints_list.append((line_near_x, line_near_y, altitude))
+
+                if line_idx < num_lines - 1:
+                    next_cut_offset = current_cut_offset + actual_spacing
+                    next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
+                    next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
+                    next_far_x = next_near_x + sweep_dir[0] * sweep_len
+                    next_far_y = next_near_y + sweep_dir[1] * sweep_len
+
+                    if line_idx % 2 == 0:
+                        waypoints_list.append((next_far_x, next_far_y, altitude))
+                    else:
+                        waypoints_list.append((next_near_x, next_near_y, altitude))
+                    current_cut_offset = next_cut_offset
+
+            all_waypoints[original_idx] = waypoints_list
+
+        return all_waypoints
+
+
+# ================================================================================
+# 測試
+# ================================================================================
+if __name__ == "__main__":
+    planner = FormationPlanner()
+
+    drones = [
+        (24.123450, 120.567800, 0.0),
+        (24.123470, 120.567820, 0.0),
+        (24.123440, 120.567810, 0.0),
+        (24.123460, 120.567830, 0.0),
+    ]
+    target = (24.12400, 120.56795, 10.0)
+
+    # M-Formation
+    wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+    print("M-Formation:")
+    for i, wp_list in enumerate(wps):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
+
+    # Leader-Follower (Bezier 轉彎)
+    route = [
+        (24.12345, 120.56780),
+        (24.12370, 120.56800),
+        (24.12390, 120.56820),
+        (24.12400, 120.56800),
+        (24.12420, 120.56790),
+    ]
+    wps_lf, origin_lf = planner.plan_formation_mission(
+        drones, target, MissionType.LEADER_FOLLOWER,
+        params={
+            'route_waypoints': route,
+            'lateral_offset': 3.0,
+            'longitudinal_spacing': 5.0,
+            'turn_margin': 0.35,
+            'curve_resolution': 8,
+            'altitude': 10.0
+        }
+    )
+    print(f"\nLeader-Follower (Bezier turns):")
+    for i, wp_list in enumerate(wps_lf):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
+        for j, wp in enumerate(wp_list):
+            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
+
+    # Grid Sweep
+    rect = [
+        (24.1237, 120.5679),
+        (24.1237, 120.5683),
+        (24.1240, 120.5683),
+        (24.1240, 120.5679)
+    ]
+    wps2, origin2 = planner.plan_formation_mission(
+        drones, target, MissionType.GRID_SWEEP,
+        params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
+    )
+    print(f"\nGrid Sweep:")
+    for i, wp_list in enumerate(wps2):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")

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", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
+                          "空速", "油門", "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/GUI/validation/test_mission.py

@@ -0,0 +1,299 @@
+#!/usr/bin/env python3
+"""
+獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作
+
+使用方式:
+    1. 啟動 SITL
+    2. 修改下方 CONFIG 區塊
+    3. python3 test_mission.py
+"""
+import sys, os
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..'))
+
+import time
+from pymavlink import mavutil
+from PyQt6.QtWidgets import QApplication
+from PyQt6.QtCore import QTimer
+
+from mission_planner import FormationPlanner, MissionType
+from command_sender import MavlinkSender
+from mission_executor import MissionExecutor, MissionState
+
+
+# ================================================================================
+# CONFIG
+# ================================================================================
+
+# 接收用連線 (讀取無人機狀態)
+RECV_CONNECTION = "udp:127.0.0.1:14550"
+
+# 發送用連線 (發送 setpoint 指令)
+SEND_CONNECTION = "udpout:127.0.0.1:14550"
+
+# 要控制的無人機 sysid 列表
+DRONE_SYSIDS = [1]
+
+# 起飛高度 (公尺)
+TAKEOFF_ALT = 10.0
+
+# 任務規劃參數
+FORMATION_SPACING = 5.0
+BASE_ALTITUDE = 10.0
+ALTITUDE_DIFF = 2.0
+ARRIVAL_RADIUS = 2.0
+
+# 測試模式: "formation" 或 "grid_sweep"
+TEST_MODE = "formation"
+
+# Grid Sweep 專用設定
+GRID_LINE_SPACING = 5.0
+
+# ================================================================================
+
+
+class SITLDroneManager:
+    """管理 SITL 無人機的連線、起飛前置作業"""
+
+    def __init__(self, connection_string, sysids):
+        self.connection_string = connection_string
+        self.sysids = sysids
+        self.mav = None
+        self.drone_gps = {}
+
+    def connect(self):
+        """建立 MAVLink 連線並等待心跳"""
+        print(f"連線到 {self.connection_string} ...")
+        self.mav = mavutil.mavlink_connection(self.connection_string)
+        self.mav.wait_heartbeat()
+        print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}")
+
+    def set_guided_and_arm(self, sysid):
+        """切換到 GUIDED 模式並解鎖"""
+        print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---")
+
+        # 切換 GUIDED 模式
+        self.mav.mav.command_long_send(
+            sysid, 1,
+            mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+            0,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            4,   # GUIDED = 4
+            0, 0, 0, 0, 0
+        )
+        time.sleep(1)
+
+        # 解鎖
+        self.mav.mav.command_long_send(
+            sysid, 1,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0, 1, 0, 0, 0, 0, 0, 0
+        )
+        time.sleep(1)
+        print(f"  sysid={sysid}: GUIDED + ARMED")
+
+    def takeoff(self, sysid, altitude):
+        """起飛到指定高度"""
+        print(f"  sysid={sysid}: 起飛到 {altitude}m ...")
+        self.mav.mav.command_long_send(
+            sysid, 1,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0, 0, 0, 0, 0, 0, 0, altitude
+        )
+
+    def wait_for_altitude(self, sysid, target_alt, timeout=30):
+        """等待無人機到達指定高度"""
+        print(f"  sysid={sysid}: 等待到達 {target_alt}m ...")
+        start = time.time()
+        while time.time() - start < timeout:
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            if msg and msg.get_srcSystem() == sysid:
+                alt = msg.relative_alt / 1000.0
+                if alt >= target_alt * 0.9:
+                    print(f"  sysid={sysid}: 已到達 {alt:.1f}m")
+                    return True
+        print(f"  sysid={sysid}: 等待超時!")
+        return False
+
+    def update_gps_once(self):
+        """讀取一輪 GPS 資料更新 drone_gps"""
+        deadline = time.time() + 3
+        received = set()
+        while time.time() < deadline and len(received) < len(self.sysids):
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            if msg is None:
+                continue
+            sid = msg.get_srcSystem()
+            if sid in self.sysids:
+                drone_id = f"s0_{sid}"
+                self.drone_gps[drone_id] = {
+                    'lat': msg.lat / 1e7,
+                    'lon': msg.lon / 1e7,
+                    'alt': msg.relative_alt / 1000.0
+                }
+                received.add(sid)
+
+        for sid in self.sysids:
+            drone_id = f"s0_{sid}"
+            if drone_id in self.drone_gps:
+                gps = self.drone_gps[drone_id]
+                print(f"  {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)")
+            else:
+                print(f"  {drone_id}: 尚未收到 GPS")
+
+    def start_gps_polling(self, interval_ms=200):
+        """啟動定時 GPS 輪詢 (用 QTimer)"""
+        self._gps_timer = QTimer()
+        self._gps_timer.timeout.connect(self._poll_gps)
+        self._gps_timer.start(interval_ms)
+
+    def _poll_gps(self):
+        """非阻塞方式讀取最新 GPS"""
+        while True:
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False)
+            if msg is None:
+                break
+            sid = msg.get_srcSystem()
+            if sid in self.sysids:
+                drone_id = f"s0_{sid}"
+                self.drone_gps[drone_id] = {
+                    'lat': msg.lat / 1e7,
+                    'lon': msg.lon / 1e7,
+                    'alt': msg.relative_alt / 1000.0
+                }
+
+
+def main():
+    # 建立 Qt 應用 (MissionExecutor 需要 QTimer)
+    app = QApplication(sys.argv)
+
+    # 連線 + 起飛前置作業
+    manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS)
+    manager.connect()
+
+    for sysid in DRONE_SYSIDS:
+        manager.set_guided_and_arm(sysid)
+        manager.takeoff(sysid, TAKEOFF_ALT)
+
+    # 等待所有無人機到達起飛高度
+    for sysid in DRONE_SYSIDS:
+        manager.wait_for_altitude(sysid, TAKEOFF_ALT)
+
+    time.sleep(2)
+
+    # 讀取當前 GPS 位置
+    print("\n讀取當前 GPS 位置 ...")
+    manager.update_gps_once()
+
+    drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS]
+    drone_gps_positions = []
+    for drone_id in drone_ids:
+        gps = manager.drone_gps.get(drone_id)
+        if gps is None:
+            print(f"錯誤: 讀不到 {drone_id} 的 GPS")
+            return
+        drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt']))
+
+    # 規劃任務
+    print(f"\n規劃任務 (模式: {TEST_MODE}) ...")
+    planner = FormationPlanner(
+        spacing=FORMATION_SPACING,
+        base_altitude=BASE_ALTITUDE,
+        altitude_diff=ALTITUDE_DIFF
+    )
+
+    center_lat = drone_gps_positions[0][0]
+    center_lon = drone_gps_positions[0][1]
+
+    if TEST_MODE == "formation":
+        target_lat = center_lat + 30.0 / 111000.0
+        target_lon = center_lon
+        target_gps = (target_lat, target_lon, BASE_ALTITUDE)
+        print(f"  目標點: ({target_lat:.6f}, {target_lon:.6f})")
+
+        waypoints_per_drone, origin = planner.plan_formation_mission(
+            drone_gps_positions, target_gps, MissionType.M_FORMATION
+        )
+
+    elif TEST_MODE == "grid_sweep":
+        # 在無人機前方 30m 處建立 40m x 30m 的矩形
+        offset_lat = 30.0 / 111000.0
+        half_w = 20.0 / (111000.0 * 0.9)
+        half_h = 15.0 / 111000.0
+
+        rect_center_lat = center_lat + offset_lat
+        rect_center_lon = center_lon
+
+        rect_corners = [
+            (rect_center_lat - half_h, rect_center_lon - half_w),
+            (rect_center_lat - half_h, rect_center_lon + half_w),
+            (rect_center_lat + half_h, rect_center_lon + half_w),
+            (rect_center_lat + half_h, rect_center_lon - half_w),
+        ]
+        target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE)
+        print(f"  矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})")
+
+        waypoints_per_drone, origin = planner.plan_formation_mission(
+            drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
+            params={
+                'rect_corners': rect_corners,
+                'line_spacing': GRID_LINE_SPACING,
+                'altitude': BASE_ALTITUDE
+            }
+        )
+    else:
+        print(f"未知測試模式: {TEST_MODE}")
+        return
+
+    planned_waypoints = {
+        'drone_ids': drone_ids,
+        'waypoints': waypoints_per_drone
+    }
+
+    # 印出規劃結果
+    for i, did in enumerate(drone_ids):
+        wps = waypoints_per_drone[i]
+        print(f"  {did}: {len(wps)} 個航點")
+        for j, wp in enumerate(wps):
+            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)")
+
+    # 啟動任務
+    print("\n啟動任務 ...")
+    manager.start_gps_polling(interval_ms=200)
+
+    sender = MavlinkSender(SEND_CONNECTION)
+    executor = MissionExecutor(
+        sender=sender,
+        drone_gps=manager.drone_gps,
+        arrival_radius=ARRIVAL_RADIUS,
+        send_rate_hz=2.0
+    )
+
+    executor.drone_waypoint_reached.connect(
+        lambda did, idx, total: print(f"\n  >> {did} 到達 WP {idx}/{total}")
+    )
+    executor.mission_completed.connect(
+        lambda: (print("\n===== 任務全部完成 ====="), app.quit())
+    )
+
+    # 設定超時自動退出
+    timeout_timer = QTimer()
+    timeout_timer.setSingleShot(True)
+    timeout_timer.timeout.connect(lambda: (
+        print("\n⚠ 測試超時，強制退出"),
+        executor.stop(),
+        app.quit()
+    ))
+    timeout_timer.start(180_000)  # 180 秒超時
+
+    executor.start(planned_waypoints)
+
+    print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n")
+    app.exec()
+
+    executor.stop()
+    sender.close()
+    print("測試結束")
+
+
+if __name__ == "__main__":
+    main()

src/GUI/validation/verify_waypoints.py

@@ -0,0 +1,482 @@
+#!/usr/bin/env python3
+"""
+任務規劃視覺化驗證工具（含動畫模擬）
+
+使用方式:
+    1. 由 GUI 自動觸發:  python3 verify_waypoints.py --file /tmp/mission_plan.json
+    2. 獨立手動執行:      python3 verify_waypoints.py
+
+操作:
+    - 啟動後先顯示靜態航點圖
+    - 點擊圖下方的按鈕控制動畫
+"""
+import sys, os
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..'))
+
+import json
+import argparse
+import math
+import matplotlib
+matplotlib.use('TkAgg')
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+from matplotlib.widgets import Button
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+from mission_planner import FormationPlanner, MissionType, CoordinateConverter
+
+
+# ================================================================================
+# 色彩定義
+# ================================================================================
+COLORS = ['#378ADD', '#1D9E75', '#BA7517', '#D85A30', '#7F77DD', '#D4537E',
+          '#E24B4A', '#639922', '#00BFFF', '#FF69B4']
+
+# 動畫參數
+FRAMES_PER_SEGMENT = 40    # 每段航點之間的畫面數
+TRAIL_LENGTH = 60          # 拖尾長度（畫面數）
+INTERVAL_MS = 50           # 每幀間隔 (ms)
+
+
+# ================================================================================
+# 靜態繪圖
+# ================================================================================
+
+def plot_grid_sweep(ax, data, conv):
+    """畫 Grid Sweep 俯視圖"""
+    if 'rect_corners' in data:
+        rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']]
+        xs = [p[0] for p in rect_local] + [rect_local[0][0]]
+        ys = [p[1] for p in rect_local] + [rect_local[0][1]]
+        ax.plot(xs, ys, 'k--', linewidth=1.5, label='Task area')
+        ax.fill(xs, ys, alpha=0.05, color='gray')
+
+    _draw_waypoint_paths(ax, data, conv, show_sweep_labels=True)
+
+    total_wps = sum(len(wps) for wps in data['waypoints'])
+    ax.set_title(f'Grid Sweep - {len(data["drone_ids"])} drones, {total_wps} waypoints')
+    ax.set_xlabel('East (m)')
+    ax.set_ylabel('North (m)')
+    ax.set_aspect('equal')
+    ax.grid(True, alpha=0.3)
+
+
+def plot_formation(ax, data, conv):
+    """畫 M-Formation / Circle / Leader-Follower 俯視圖"""
+    _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False)
+
+    if 'target' in data:
+        t = data['target']
+        tx, ty, _ = conv.gps_to_local(t[0], t[1], t[2] if len(t) > 2 else 0)
+        ax.plot(tx, ty, 'r*', markersize=18, zorder=5)
+        ax.annotate('Target', (tx, ty + 1), fontsize=9, color='red', ha='center', va='bottom')
+
+    if 'route_waypoints' in data:
+        route_local = [conv.gps_to_local(wp[0], wp[1], 0)[:2] for wp in data['route_waypoints']]
+        rxs = [p[0] for p in route_local]
+        rys = [p[1] for p in route_local]
+        ax.plot(rxs, rys, 'r--', linewidth=1.5, alpha=0.5, label='Route center')
+        for i, (rx, ry) in enumerate(route_local):
+            ax.plot(rx, ry, 'ro', markersize=6, alpha=0.5)
+            ax.annotate(f'R{i+1}', (rx, ry + 0.8), fontsize=7, color='red',
+                       ha='center', va='bottom', alpha=0.7)
+
+    mission_type = data.get('mission_type', 'formation')
+    total_wps = sum(len(wps) for wps in data['waypoints'])
+    ax.set_title(f'{mission_type} - {len(data["drone_ids"])} drones, {total_wps} waypoints')
+    ax.set_xlabel('East (m)')
+    ax.set_ylabel('North (m)')
+    ax.set_aspect('equal')
+    ax.grid(True, alpha=0.3)
+
+
+def _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False):
+    """共用的航點路徑繪圖"""
+    drone_ids = data['drone_ids']
+    waypoints = data['waypoints']
+    drones_gps = data.get('drones_gps', [])
+
+    for i, pos in enumerate(drones_gps):
+        x, y, _ = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0)
+        c = COLORS[i % len(COLORS)]
+        ax.plot(x, y, 'o', color=c, markersize=10, zorder=5)
+        ax.annotate(f'{drone_ids[i]}', (x, y + 1), fontsize=8, fontweight='bold',
+                    ha='center', va='bottom', color=c)
+
+    for i, wps in enumerate(waypoints):
+        c = COLORS[i % len(COLORS)]
+        local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
+        xs = [p[0] for p in local_wps]
+        ys = [p[1] for p in local_wps]
+        ax.plot(xs, ys, '-', color=c, linewidth=1.5, alpha=0.7)
+
+        for j, (x, y, z) in enumerate(local_wps):
+            if show_sweep_labels:
+                if j == 0:
+                    ax.plot(x, y, 's', color=c, markersize=8, zorder=4)
+                    ax.annotate('gather', (x, y), fontsize=6, ha='right', va='top')
+                elif j == 1:
+                    ax.plot(x, y, '^', color=c, markersize=8, zorder=4)
+                    ax.annotate('entry', (x, y), fontsize=6, ha='right', va='top')
+                else:
+                    ax.plot(x, y, '.', color=c, markersize=4)
+            else:
+                marker = 's' if j == 0 else '*'
+                ax.plot(x, y, marker, color=c, markersize=10, zorder=4)
+                ax.annotate(f'WP{j}\n({z:.0f}m)', (x, y), fontsize=6, ha='center', va='bottom')
+
+        if local_wps:
+            lx, ly, _ = local_wps[-1]
+            ax.plot(lx, ly, 'X', color=c, markersize=10, markeredgewidth=2, zorder=4)
+
+
+# ================================================================================
+# 動畫模擬
+# ================================================================================
+
+class MissionAnimator:
+    """任務動畫控制器"""
+
+    def __init__(self, fig, ax, data, conv):
+        self.fig = fig
+        self.ax = ax
+        self.data = data
+        self.conv = conv
+        self.is_playing = False
+        self.anim = None
+        self.current_frame = 0
+
+        drone_ids = data['drone_ids']
+        waypoints = data['waypoints']
+        drones_gps = data.get('drones_gps', [])
+        self.num_drones = len(drone_ids)
+
+        # 建立完整航點序列：初始位置 → WP0 → WP1 → ...
+        self.all_local_wps = []
+        for i, wps in enumerate(waypoints):
+            local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
+
+            # 把初始位置插入最前面
+            if i < len(drones_gps):
+                pos = drones_gps[i]
+                start = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0)
+                local_wps.insert(0, start)
+
+            self.all_local_wps.append(local_wps)
+
+        # 計算最大航點段數
+        self.max_segments = max(len(wps) - 1 for wps in self.all_local_wps) if self.all_local_wps else 0
+        self.total_frames = self.max_segments * FRAMES_PER_SEGMENT
+
+        # 預計算位置
+        self.positions = self._precompute_positions()
+
+        # 動畫元素
+        self.drone_dots = []
+        self.trail_lines = []
+        self.trail_data = [[] for _ in range(self.num_drones)]
+        self.status_text = None
+
+    def _precompute_positions(self):
+        """預計算所有幀的位置 — 等時間步進"""
+        positions = []
+
+        for frame in range(self.total_frames + 1):
+            seg_idx = frame // FRAMES_PER_SEGMENT
+            seg_progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT
+
+            frame_positions = []
+            for drone_idx in range(self.num_drones):
+                wps = self.all_local_wps[drone_idx]
+                num_segs = len(wps) - 1
+
+                if seg_idx >= num_segs:
+                    frame_positions.append((wps[-1][0], wps[-1][1]))
+                else:
+                    x0, y0, _ = wps[seg_idx]
+                    x1, y1, _ = wps[seg_idx + 1]
+                    x = x0 + (x1 - x0) * seg_progress
+                    y = y0 + (y1 - y0) * seg_progress
+                    frame_positions.append((x, y))
+
+            positions.append(frame_positions)
+
+        return positions
+
+    def setup(self):
+        """建立動畫元素和按鈕"""
+        for i in range(self.num_drones):
+            c = COLORS[i % len(COLORS)]
+            dot, = self.ax.plot([], [], 'o', color=c, markersize=12,
+                               markeredgecolor='white', markeredgewidth=1.5, zorder=10)
+            self.drone_dots.append(dot)
+            trail, = self.ax.plot([], [], '-', color=c, linewidth=2.5, alpha=0.4, zorder=9)
+            self.trail_lines.append(trail)
+
+        self.status_text = self.ax.text(
+            0.02, 0.98, 'Ready',
+            transform=self.ax.transAxes, fontsize=10,
+            verticalalignment='top',
+            bbox=dict(boxstyle='round,pad=0.3', facecolor='white', alpha=0.8)
+        )
+
+        # 按鈕放在右上角圖內，避免擋到軸標籤
+        ax_play = self.fig.add_axes([0.72, 0.91, 0.08, 0.035])
+        self.btn_play = Button(ax_play, 'Play', color='#4CAF50', hovercolor='#66BB6A')
+        self.btn_play.label.set_color('white')
+        self.btn_play.label.set_fontweight('bold')
+        self.btn_play.on_clicked(self._on_play)
+
+        ax_pause = self.fig.add_axes([0.81, 0.91, 0.08, 0.035])
+        self.btn_pause = Button(ax_pause, 'Pause', color='#FF9800', hovercolor='#FFB74D')
+        self.btn_pause.label.set_color('white')
+        self.btn_pause.label.set_fontweight('bold')
+        self.btn_pause.on_clicked(self._on_pause)
+
+        ax_reset = self.fig.add_axes([0.90, 0.91, 0.08, 0.035])
+        self.btn_reset = Button(ax_reset, 'Reset', color='#F44336', hovercolor='#EF5350')
+        self.btn_reset.label.set_color('white')
+        self.btn_reset.label.set_fontweight('bold')
+        self.btn_reset.on_clicked(self._on_reset)
+
+    def _on_play(self, event):
+        if self.is_playing:
+            return
+        if self.anim is None:
+            self.anim = animation.FuncAnimation(
+                self.fig, self._update_frame,
+                frames=range(self.current_frame, self.total_frames + 1),
+                interval=INTERVAL_MS,
+                blit=False,
+                repeat=False
+            )
+        else:
+            self.anim.resume()
+        self.is_playing = True
+        self.fig.canvas.draw_idle()
+
+    def _on_pause(self, event):
+        if not self.is_playing or self.anim is None:
+            return
+        self.anim.pause()
+        self.is_playing = False
+        self.status_text.set_text(f'Paused (frame {self.current_frame}/{self.total_frames})')
+        self.fig.canvas.draw_idle()
+
+    def _on_reset(self, event):
+        if self.anim is not None:
+            self.anim.event_source.stop()
+            self.anim = None
+        self.is_playing = False
+        self.current_frame = 0
+        self.trail_data = [[] for _ in range(self.num_drones)]
+        for dot in self.drone_dots:
+            dot.set_data([], [])
+        for trail in self.trail_lines:
+            trail.set_data([], [])
+        self.status_text.set_text('Ready')
+        self.fig.canvas.draw_idle()
+
+    def _update_frame(self, frame):
+        self.current_frame = frame
+
+        if frame >= len(self.positions):
+            self.is_playing = False
+            self.status_text.set_text('Done')
+            return self.drone_dots + self.trail_lines + [self.status_text]
+
+        # seg_idx - 1 是因為第 0 段是 start→WP0
+        seg_idx = frame // FRAMES_PER_SEGMENT
+        progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT
+
+        # 顯示時把第 0 段標為 "Start -> WP0"
+        if seg_idx == 0:
+            label = f'Start -> WP0  Progress {progress:.0%}'
+        else:
+            label = f'WP{seg_idx-1} -> WP{seg_idx}  Progress {progress:.0%}'
+        self.status_text.set_text(
+            f'{label}  Frame {frame}/{self.total_frames}'
+        )
+
+        for i in range(self.num_drones):
+            x, y = self.positions[frame][i]
+            self.drone_dots[i].set_data([x], [y])
+
+            self.trail_data[i].append((x, y))
+            if len(self.trail_data[i]) > TRAIL_LENGTH:
+                self.trail_data[i] = self.trail_data[i][-TRAIL_LENGTH:]
+            trail_x = [p[0] for p in self.trail_data[i]]
+            trail_y = [p[1] for p in self.trail_data[i]]
+            self.trail_lines[i].set_data(trail_x, trail_y)
+
+        return self.drone_dots + self.trail_lines + [self.status_text]
+
+
+# ================================================================================
+# 主流程
+# ================================================================================
+
+def visualize_from_file(filepath):
+    """從 JSON 檔案讀取並視覺化"""
+    with open(filepath, 'r') as f:
+        data = json.load(f)
+
+    origin = data['origin']
+    conv = CoordinateConverter(origin[0], origin[1], 0)
+    mission_type = data.get('mission_type', 'formation')
+    is_sweep = mission_type == 'grid_sweep'
+
+    fig, ax = plt.subplots(1, 1, figsize=(10, 8))
+    fig.suptitle(f'Mission Verification - {mission_type}', fontsize=13, fontweight='bold')
+
+    if is_sweep:
+        plot_grid_sweep(ax, data, conv)
+    else:
+        plot_formation(ax, data, conv)
+
+    _print_summary(data)
+
+    animator = MissionAnimator(fig, ax, data, conv)
+    animator.setup()
+
+    plt.tight_layout(rect=[0, 0, 1, 0.95])
+    plt.show()
+
+
+def visualize_standalone():
+    """獨立執行：使用內建測試資料"""
+    drones = [
+        (24.123450, 120.567800, 0.0),
+        (24.123470, 120.567820, 0.0),
+        (24.123440, 120.567810, 0.0),
+        (24.123460, 120.567830, 0.0),
+    ]
+    rect_corners = [
+        (24.12380, 120.56775),
+        (24.12380, 120.56810),
+        (24.12420, 120.56810),
+        (24.12420, 120.56775),
+    ]
+    target = (24.12400, 120.56795, 10.0)
+
+    planner = FormationPlanner(spacing=5.0, base_altitude=10.0, altitude_diff=2.0)
+
+    fig = plt.figure(figsize=(16, 10))
+    fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold')
+
+    # M-Formation
+    wps_m, origin_m = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+    conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0)
+    data_m = {
+        'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
+        'waypoints': wps_m,
+        'drones_gps': drones,
+        'target': target,
+        'mission_type': 'M_FORMATION'
+    }
+    ax1 = fig.add_subplot(2, 2, 1)
+    plot_formation(ax1, data_m, conv_m)
+
+    # Grid Sweep 5m
+    target_gs = (sum(c[0] for c in rect_corners) / 4,
+                 sum(c[1] for c in rect_corners) / 4, 10.0)
+    wps_g, origin_g = planner.plan_formation_mission(
+        drones, target_gs, MissionType.GRID_SWEEP,
+        params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0}
+    )
+    conv_g = CoordinateConverter(origin_g[0], origin_g[1], 0)
+    data_g = {
+        'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
+        'waypoints': wps_g,
+        'drones_gps': drones,
+        'rect_corners': rect_corners,
+        'mission_type': 'grid_sweep'
+    }
+    ax2 = fig.add_subplot(2, 2, 2)
+    plot_grid_sweep(ax2, data_g, conv_g)
+
+    # Leader-Follower
+    route = [
+        (24.12360, 120.56780),
+        (24.12380, 120.56800),
+        (24.12400, 120.56820),
+        (24.12410, 120.56800),
+        (24.12420, 120.56790),
+    ]
+    wps_lf, origin_lf = planner.plan_formation_mission(
+        drones, target, MissionType.LEADER_FOLLOWER,
+        params={'route_waypoints': route, 'lateral_offset': 3.0,
+                'longitudinal_spacing': 5.0, 'altitude': 10.0}
+    )
+    conv_lf = CoordinateConverter(origin_lf[0], origin_lf[1], 0)
+    data_lf = {
+        'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
+        'waypoints': wps_lf,
+        'drones_gps': drones,
+        'route_waypoints': route,
+        'target': target,
+        'mission_type': 'LEADER_FOLLOWER'
+    }
+    ax3 = fig.add_subplot(2, 2, 3)
+    plot_formation(ax3, data_lf, conv_lf)
+
+    # 3D
+    ax4 = fig.add_subplot(2, 2, 4, projection='3d')
+    _plot_3d(ax4, data_g, conv_g)
+
+    plt.tight_layout()
+    plt.show()
+
+
+def _plot_3d(ax, data, conv):
+    """3D 視角"""
+    if 'rect_corners' in data:
+        rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']]
+        xs = [p[0] for p in rect_local] + [rect_local[0][0]]
+        ys = [p[1] for p in rect_local] + [rect_local[0][1]]
+        ax.plot(xs, ys, [0] * len(xs), 'k--', linewidth=1, alpha=0.4)
+
+    for i, wps in enumerate(data['waypoints']):
+        c = COLORS[i % len(COLORS)]
+        local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
+        xs = [p[0] for p in local_wps]
+        ys = [p[1] for p in local_wps]
+        zs = [p[2] for p in local_wps]
+        ax.plot(xs, ys, zs, '-', color=c, linewidth=1.5)
+        if local_wps:
+            ax.scatter(xs[0], ys[0], zs[0], color=c, s=50, marker='s')
+            ax.scatter(xs[-1], ys[-1], zs[-1], color=c, s=50, marker='X')
+
+    ax.set_title('3D view')
+    ax.set_xlabel('East (m)')
+    ax.set_ylabel('North (m)')
+    ax.set_zlabel('Alt (m)')
+
+
+def _print_summary(data):
+    """終端印出摘要"""
+    drone_ids = data['drone_ids']
+    waypoints = data['waypoints']
+    mission_type = data.get('mission_type', 'unknown')
+    print(f"\n{'=' * 50}")
+    print(f"Mission: {mission_type}")
+    print(f"Drones: {len(drone_ids)}")
+    print(f"{'=' * 50}")
+    for i, did in enumerate(drone_ids):
+        wps = waypoints[i]
+        print(f"  {did}: {len(wps)} waypoints")
+        for j, wp in enumerate(wps):
+            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)")
+    print(f"{'=' * 50}\n")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Mission waypoint visualizer')
+    parser.add_argument('--file', '-f', type=str, default=None,
+                        help='JSON file from GUI (auto-generated)')
+    args = parser.parse_args()
+
+    if args.file:
+        visualize_from_file(args.file)
+    else:
+        visualize_standalone()

src/fc_interfaces/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.8)
+project(fc_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/MavPing.srv"
+  "srv/MavCommandLong.srv"
+  "srv/MavPositionTargetGlobalInt.srv"
+  )
+
+ament_package()

src/fc_interfaces/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fc_interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/fc_interfaces/srv/MavCommandLong.srv

@@ -0,0 +1,18 @@
+# Request
+uint8 target_sysid
+uint8 target_compid
+uint16 command
+uint8 confirmation
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+float32 param5
+float32 param6
+float32 param7
+float32 timeout_sec
+---
+# Response
+bool success
+string message
+uint8 ack_result

src/fc_interfaces/srv/MavPing.srv

@@ -0,0 +1,9 @@
+# Request
+uint8 target_sysid
+uint8 target_compid
+uint8 ping_seq
+---
+# Response
+bool success
+string message
+float32 rtt_ms

src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv

@@ -0,0 +1,36 @@
+# Request
+uint8  target_sysid
+uint8  target_compid
+
+uint32 time_boot_ms        # ms since system boot, 一般用 0 讓飛控自己填
+uint8  coordinate_frame    # MAV_FRAME_*
+uint16 type_mask           # POSITION_TARGET_TYPEMASK bitmask
+int32  lat_int             # 1e7 * latitude (deg)
+int32  lon_int             # 1e7 * longitude (deg)
+float32 alt                # altitude (m)
+float32 vx                 # X velocity (m/s)
+float32 vy                 # Y velocity (m/s)
+float32 vz                 # Z velocity (m/s)
+float32 afx                # X acceleration or force (m/s^2)
+float32 afy                # Y acceleration or force (m/s^2)
+float32 afz                # Z acceleration or force (m/s^2)
+float32 yaw                # yaw (rad)
+float32 yaw_rate           # yaw rate (rad/s)
+float32 timeout_sec        # 等待 POSITION_TARGET_GLOBAL_INT 回應的時間上限
+---
+# Response
+string message
+uint8  ack_result  
+uint32 r_time_boot_ms  
+uint16 r_type_mask           
+int32  r_lat_int            
+int32  r_lon_int            
+float32 r_alt                
+float32 r_vx 
+float32 r_vy 
+float32 r_vz 
+float32 r_afx          
+float32 r_afy      
+float32 r_afz              
+float32 r_yaw        
+float32 r_yaw_rate     

src/fc_network_adapter/fc_network_adapter/ackEnum.py

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

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -6,6 +6,7 @@
 # 開發此專案的注意事項
 - 預設 autopilot 的 component id = 1
 - 不允許 system id 重複
+- 預設同一載具僅會使用相同的 socket
 - 增加一個固定數值監控然後要到 ros2 topic 
   - mavlinkROS2Node.py 檔案內 
     - PublishRateController.topic_intervals 建立
@@ -126,6 +127,7 @@
 - *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) }
 - *self.mavlink_socket* 從 pymavlink 繼承的socket物件
 - *self.state* 描述這個 socket 物件的狀態
+- *self.MAVLink* pymavlink 的一個裝置物件 模擬自己是某一個裝置 用以對於該 mavlink bus 負責封包發送 
 ---
 - *process_data()* [async method] 核心方法
 - *remove_target_socket()* *add_target_socket()*
@@ -170,6 +172,45 @@
 ## mavlinkVehicleView.py
     這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用
 
+## mavlinkROS2Nodes.py
+    這裡支持了所有 ROS2 框架的對應功能 包涵 
+    1. 將載具的串流更新的狀態更新到對應的 topic 
+    2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
+
+### **[Class]** fc_ros_manager
+    MAVLink ROS2 節點管理器 管理兩個獨立的 Node
+    會開一個執行緒 讓兩個 Node 都跑在裡面
+    然後用 spin_once 保持 Node 的活性
+- *self.status_publisher* 物件實例
+- *self.command_service* 物件實例
+- *self.spin_thread* 執行緒
+---
+- *start()* 啟動自己
+- *stop()* 停下自己
+- *shutdown()* 關閉自己並清理
+
+### **[Class]** VehicleStatusPublisher
+    這整個組件都是自動的 目前沒有什麼需要 runtime 設定的地方
+- *self.fc_publishers* 儲存所有 publisher 的資料結構
+- *self.rate_controller* 儲存頻率參數的地方 也可以關掉某個 topic 的發佈
+---
+- *timer_callback()* 自動的抓出現有的 vehicle 中有的資訊 然後固定的將訊息丟到負責發佈的方法中
+- *_publish_vehicle_status()* 接 timer_callback 後去分配到各項發佈中
+- *_get_or_create_publisher()* 實際創建 topic 的地方
+- *_publish_XXX()* 各項發佈的方法
+
+### **[Class]** MavlinkCommandService
+    提供 ros2 service 讓 serial_object 去丟出 mavlink 封包
+    然後會從回應封包中挑出期待的回應 再傳給 ros2 request 
+    其中 PendingEntry 是很關鍵的東西
+    它的每個物件 代表需要等待的封包種類與必需包含的內容
+- *self._pending_by_sysid* 儲存 PendingEntry 的地方
+---
+- *return_router()* 檢查並消耗 return_packet_ring 然後將封包分配到 pending 去
+- *__init__()* 這邊登入要創建的 service 到 ros2 系統
+- *handle_XXX()* 這邊是實現 service 的具體方法
+
+
 # 開發記錄
 
 ## 已實現功能
@@ -182,9 +223,9 @@
 7. 終端機介面控制
 8. 基礎載具流量觀測
 9. 載具狀態收集與彙整
-10. a. ros2 topic 應用開發介面
+10. a. ros2 topic 應用開發介面 (基礎框架)
+    b. ros2 service 應用開發介面 (基礎框架)
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令
 5-2. 模組化 serial 連線機制 以利後期擴容其他模組
-10. a. ros2  應用開發介面

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-VERSION_NO = "v0.59"
+PROJECT_VER = "v0.61"
 
 class PanelState:
     def __init__(self):
@@ -304,7 +304,7 @@ class ControlPanel:
                 # help_line = start_line + len(current_menu.children) + 2
                 help_line = height - 2
                 stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
-                stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM)
+                stdscr.addstr(height-1 , width-12, f" {PROJECT_VER} ", curses.A_DIM)
 
                 stdscr.refresh()
 
@@ -1138,11 +1138,11 @@ class Orchestrator:
 
         # === 4) ros 部分的準備 ===
         self.fc_ros_manager = mros.ros2_manager
-        if not self.fc_ros_manager.initialized:
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
             'position': 1.0,
-            'attitude': 0.0,    
+            'position_ned': 1.0,
+            'attitude': 1.0,
             'velocity': 0.0,
             'battery': 1.0,
             'vfr_hud': 1.0,
@@ -1311,7 +1311,7 @@ class Orchestrator:
 
             if self.fc_ros_manager.spin_thread.is_alive():
                 if self.fc_ros_manager.running:
-                    self.fc_ros_manager.stop()
+                    self.fc_ros_manager.shutdown()
                 self.fc_ros_manager.spin_thread.join(timeout=2)
 
             # 關閉面板執行緒
@@ -1340,6 +1340,7 @@ class Orchestrator:
                 self.occupied_ip_ports[ip].append(port)
             else:
                 self.occupied_ip_ports[ip] = [port]
+
         # 外放資訊部分
         elif self.panelState.udp_info_temp["direction"] == "outbound":
             connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
@@ -1559,8 +1560,27 @@ class Orchestrator:
 
 def main():
 
-    stop_evt = threading.Event()
+    # =========== 各項模組的版本先驗 ===========
+    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
+    version_check = True
+    if mo.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkObkect")
+        version_check = False
+    if mros.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkROS2Nodes")
+        version_check = False
+    if mvv.MODULE_VER != "1.00":
+        print("Module Version Error! : mavlinkVehicleView")
+        version_check = False
+    if sm.MODULE_VER != "0.60":
+        print("Module Version Error! : serialManager")
+        version_check = False
+    if version_check == False:
+        print("Environment Obstacle! Check YOUR Execution System Path First!!")
+        return
+    # ========================================
 
+    stop_evt = threading.Event()
     def signal_handler(signum, frame):
         """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組"""
         stop_evt.set()
@@ -1594,4 +1614,7 @@ if __name__ == "__main__":
 4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊
 5. 系統納入 mavlink ROS2 Manager 模組
 
+2025.04.13
+1. 加入各項模組的版本先驗 檢驗失敗就直接離開
+
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -28,9 +28,6 @@ mavlink_bridge:
     專門處理 stream_bridge_ring 裡面的訊息流
     會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖
 
-
-
-
 '''
 
 # 基礎功能的 import
@@ -45,6 +42,8 @@ from collections import deque
 
 # mavlink 的 import
 from pymavlink import mavutil
+from pymavlink.dialects.v20 import common as mav_common
+from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
@@ -59,6 +58,8 @@ from .utils import RingBuffer, setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.50"
+
 
 stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255)
 return_packet_ring = RingBuffer(capacity=256, buffer_id=254)
@@ -342,6 +343,19 @@ class mavlink_object:
     mavlinkObjects = {}  # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) }
     socket_num = 0  # 用來記錄目前的 socket 數量
 
+    # 用來銜接 pymavlink 的 MavLink 實例的管道
+    class CapturingPipeline:
+        def __init__(self, target_deque):
+            self.target_deque = target_deque
+            self.last_data = b''
+        def write(self, data):
+            self.last_data = bytes(data)
+            # logger.debug(f'CapturingPipeline write data: {data.hex()}') # developer debug
+            # 把收到的資料 bytes 放到 target_deque
+            self.target_deque.append(data)
+        def seek(self, pos): pass
+        def tell(self): return 0
+
     def __new__(cls, *args, **kwargs):
         # 每創建一個實例 就將其添加到 mavlinkObjects 列表中
         # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 
@@ -359,16 +373,20 @@ class mavlink_object:
         cls.mavlinkObjects[socket_id] = instance
         return instance
 
-    def __init__(self, socket):
+    def __init__(self, socket, socket_dialect = 'common'):
         # 登入所需的 socket
         self.mavlink_socket = socket
 
         # 用於主線程發送消息的緩衝區
         self.outgoing_msgs = deque()
+        self.mavlinkPipeline = self.CapturingPipeline(self.outgoing_msgs)
+
+        if socket_dialect == 'common':
+            self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
-        self.return_msg_types = set()
+        self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表
         self.target_sockets = set()

src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py

@@ -1,212 +0,0 @@
-
-'''
-這個檔案只有一個 class
-是作為 mavlinkObject.py 中 mavlink_analyzer 類別的功能衍生
-
-主要概念是將 "離散的" mavlink 參數轉換成 ROS topic
-包含了創建 publisher 和 以及包裝並丟到 ros2 topic 的 packEmit 
-
-publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic>
-'''
-
-import os
-
-# ROS2 的 import
-import std_msgs.msg
-import sensor_msgs.msg
-import geometry_msgs.msg
-import mavros_msgs.msg
-
-import math
-
-# 自定義的 import
-from .utils import setup_logger
-
-logger = setup_logger(os.path.basename(__file__))
-
-class mavlink_publisher():
-
-    prefix_path = 'MavLinkBus'
-
-    def create_flightMode(self, sysid, component_obj):
-        # target topic name # 請跟這個 method 的名稱保持一致
-        target_topic = 'flightMode'
-
-        # 這邊要檢查 flight_mode 是否存在
-        try:
-            _ = component_obj.emitParams['flightMode_mode']
-        except KeyError:
-            # 這個 component id 還不存在
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        
-        # 若存在則 建立 publisher object 並回傳 true
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode]
-
-        return True
-
-    def packEmit_flightMode(cls, emitParams, publisher):
-        msg_str = emitParams['flightMode_mode']
-        msg = std_msgs.msg.String()
-        msg.data = msg_str
-        publisher.publish(msg)
-        pass
-
-    # ↓↓↓↓↓↓↓↓↓↓↓↓  處理不同 ros2 topic 訊息 請放在這裡  ↓↓↓↓↓↓↓↓↓↓↓↓ 
-    def euler_to_quaternion(cls,roll, pitch, yaw):
-        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 create_attitude(self, sysid, component_obj):
-        target_topic = 'attitude'
-
-        try:
-            _ = component_obj.emitParams['attitude']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude]
-
-    def packEmit_attitude(self, emitParams, publisher):
-        mav_msg = emitParams['attitude']
-        msg = sensor_msgs.msg.Imu()
-        x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw)
-        msg.orientation.x = x
-        msg.orientation.y = y
-        msg.orientation.z = z
-        msg.orientation.w = w
-        msg.angular_velocity.x = mav_msg.rollspeed
-        msg.angular_velocity.y = mav_msg.pitchspeed
-        msg.angular_velocity.z = mav_msg.yawspeed
-        publisher.publish(msg)
-        pass
-    
-    def create_local_position_pose(self, sysid, component_obj):
-        target_topic = 'local_position/pose'
-        try:
-            _ = component_obj.emitParams['local_position']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose]
-
-    def packEmit_local_pose(cls, emitParams, publisher):
-        mav_msg = emitParams['local_position']
-        msg = geometry_msgs.msg.Point()
-        msg.x = mav_msg.x
-        msg.y = mav_msg.y
-        msg.z = mav_msg.z
-        publisher.publish(msg)
-        pass
-
-    def create_local_position_velocity(self, sysid, component_obj):
-        target_topic = 'local_position/velocity'
-        try:
-            _ = component_obj.emitParams['local_position']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel]
-
-    def packEmit_local_vel(cls, emitParams, publisher):
-        mav_msg = emitParams['local_position']
-        msg = geometry_msgs.msg.Vector3()
-        msg.x = mav_msg.vx
-        msg.y = mav_msg.vy
-        msg.z = mav_msg.vz
-        publisher.publish(msg)
-        pass
-
-    def create_global_global(self, sysid, component_obj):
-        target_topic = 'global_position/global'
-        try:
-            _ = component_obj.emitParams['global_position']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global]
-
-    def packEmit_global_global(cls, emitParams, publisher):
-        mav_msg = emitParams['global_position']
-        msg = sensor_msgs.msg.NavSatFix()
-        msg.latitude = mav_msg.lat/1e7
-        msg.longitude = mav_msg.lon/1e7
-        msg.altitude = mav_msg.alt/1e3
-        publisher.publish(msg)
-        pass
-        
-    def create_global_rel(self, sysid, component_obj):
-        target_topic = 'global_position/rel_alt'
-        try:
-            _ = component_obj.emitParams['global_position']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel]
-
-    def packEmit_global_rel(cls, emitParams, publisher):
-        mav_msg = emitParams['global_position']
-        msg = std_msgs.msg.Float64()
-        msg.data = float(mav_msg.relative_alt/1e3)
-        publisher.publish(msg)
-        pass
-
-    def create_vfr_hud(self, sysid, component_obj):
-        target_topic = 'vfr_hud'
-        try:
-            _ = component_obj.emitParams['vfr_hud']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud]
-
-    def packEmit_vfr_hud(cls, emitParams, publisher):
-        mav_msg = emitParams['vfr_hud']
-        msg = mavros_msgs.msg.VfrHud()
-        msg.airspeed = mav_msg.airspeed
-        msg.groundspeed = mav_msg.groundspeed
-        msg.heading = mav_msg.heading
-        msg.throttle = float(mav_msg.throttle)
-        msg.altitude = mav_msg.alt
-        msg.climb = mav_msg.climb
-        publisher.publish(msg)
-        pass
-
-    def create_battery(self, sysid, component_obj):
-        target_topic = 'battery'
-        try:
-            _ = component_obj.emitParams['battery']
-        except KeyError:
-            logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
-            return False
-        topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery]
-
-    def packEmit_battery(cls, emitParams, publisher):
-        mav_msg = emitParams['battery']
-        msg = sensor_msgs.msg.BatteryState()
-        msg.voltage = mav_msg.voltages[0]/1e3
-        publisher.publish(msg)
-        pass
-
-        
-    # ↑↑↑↑↑↑↑↑↑↑↑↑  處理不同 ros2 topic 訊息 請放在這裡  ↑↑↑↑↑↑↑↑↑↑↑↑

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,10 +1,15 @@
 """
 MAVLink ROS2 Nodes
-包含兩個獨立的 ROS2 Node：
+主要包含兩個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
-2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
-
     從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
+2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
+    以最基礎的 mavlink microservice 會實現目標 
+    並不會包含額外的功能
+
+與一個節點管理器
+- fc_ros_manager
+
 """
 
 import os
@@ -22,19 +27,26 @@ from rclpy.executors import MultiThreadedExecutor
 import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
+import nav_msgs.msg
 import mavros_msgs.msg
 
+# ROS2 Service imports
+import fc_interfaces.srv as fcsrv
+from .ackEnum import serviceAckResult
+
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
+from . import mavlinkObject as mo
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-
+MODULE_VER = "1.50"
 
 # ============================================================================
+#                       VehicleStatusPublisher Node
+# ============================================================================
+
 # 頻率控制器
-# ============================================================================
-
 class PublishRateController:
     """發布頻率控制器 - 按時間間隔控制發布頻率"""
     
@@ -45,14 +57,17 @@ class PublishRateController:
         # 例如：'ekf_status': 1.0,  # EKF 狀態
         # ═══════════════════════════════════════════════════════════════
         # 各 topic 的發布間隔（秒）
+        # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
+        # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.5,       # GPS位置
+            'position': 0.0,       # GNSS位置
-            'attitude': 0.5,       # 姿態
+            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
-            'velocity': 0.5,       # 速度
+            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
-            'battery': 1.0,        # 電池
+            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
-            'vfr_hud': 0.5,        # VFR HUD
+            'battery': 0.0,        # 電池
-            'mode': 1.0,           # 飛行模式
+            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
-            'summary': 1.0,        # 載具摘要
+            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -93,11 +108,6 @@ class PublishRateController:
         """重置所有計時器"""
         self.last_publish_time.clear()
 
-
-# ============================================================================
-#                       VehicleStatusPublisher Node
-# ============================================================================
-
 class VehicleStatusPublisher(Node):
     """
     載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
@@ -134,10 +144,8 @@ class VehicleStatusPublisher(Node):
         if not self.running:
             return
 
-        # 從 vehicle_registry 獲取所有載具
+        # 高頻快路徑：讀取 registry 的 immutable 快照
-        all_vehicles = mvv.vehicle_registry.get_all()
+        for sysid, vehicle in mvv.vehicle_registry.read_all():
-        
-        for sysid, vehicle in all_vehicles.items():
             self._publish_vehicle_status(vehicle)
     
     def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
@@ -163,6 +171,7 @@ class VehicleStatusPublisher(Node):
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
         self._publish_position(sysid, status)
+        self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
@@ -219,6 +228,42 @@ class VehicleStatusPublisher(Node):
         
         publisher.publish(msg)
 
+    def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
+        if not self.rate_controller.should_publish(sysid, 'position_ned'):
+            return
+
+        local_pos = status.custom_status.get('local_position')
+        if not local_pos:
+            return
+
+        required_keys = ('x', 'y', 'z', 'vx', 'vy', 'vz')
+        if any(local_pos.get(k) is None for k in required_keys):
+            return
+
+        publisher = self._get_or_create_publisher(
+            sysid, 'position_ned', nav_msgs.msg.Odometry
+        )
+
+        if publisher.get_subscription_count() == 0:
+            return
+
+        msg = nav_msgs.msg.Odometry()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'local_ned'
+        msg.child_frame_id = 'base_link_ned'
+
+        # 保持 MAVLink LOCAL_POSITION_NED 座標定義，不進行 ENU 轉換
+        msg.pose.pose.position.x = float(local_pos['x'])
+        msg.pose.pose.position.y = float(local_pos['y'])
+        msg.pose.pose.position.z = float(local_pos['z'])
+
+        msg.twist.twist.linear.x = float(local_pos['vx'])
+        msg.twist.twist.linear.y = float(local_pos['vy'])
+        msg.twist.twist.linear.z = float(local_pos['vz'])
+
+        publisher.publish(msg)
+    
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
         """發布姿態（IMU）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
@@ -227,7 +272,6 @@ class VehicleStatusPublisher(Node):
         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:
@@ -450,194 +494,364 @@ class VehicleStatusPublisher(Node):
 #                       MavlinkCommandService Node
 # ============================================================================
 
+class PendingEntry():
+    def __init__(self, event, match_fn):
+        # self.match_msgid = match_msgid # 只會在初始時設定
+        self.match_fn = match_fn
+
+        self.event = event # 只會在外迴圈 set()
+        self.result_mav_msg = None #只會在外迴圈 被寫入
+        self.error = ""
+
 class MavlinkCommandService(Node):
     """
     MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令
     
     職責:
-    - 作為 service server，等待 client 請求
+    - 作為 service server 等待 client 請求
     - 接收請求，組裝 MAVLink 封包
     - 調用 mavlinkObject 發送封包
-    - 處理 ACK 等待和超時（未來實現）
+    - 處理 ACK 等待和超時
     
     設計理念：回歸 MAVLink 純粹結構
     - 只負責將 ROS2 請求轉換為 MAVLink 封包
-    - 不預設功能（如 ARM/DISARM），保持模組化
+    - 不預設功能（如 ARM/DISARM) 保持模組化
     - 高層應用可透過此 service 實現各種功能
+
+    講白話一點就是
+    每次接到一個 service 請求 要整個系統丟某種指令給載具時
+    會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+
     """
 
+    serviceString_prefix = '/fc_network/vehicle'
+    
     def __init__(self):
         super().__init__('mavlink_command_service')
         
-        # ═══════════════════════════════════════════════════════════════════
+        # 狀態標記
-        # ROS2 Service 架構說明：
+        self.running = True
-        # 
-        # 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 檔案編譯
+        # # mavlinkObject 的引用（將由外部設置） 用不到
-        # 這裡先使用標準 String service 作為簡化實現
+        # self.mavlink_analyzer = None
-        # TODO: 未來可創建專門的 .srv 檔案
+
-        from std_srvs.srv import Trigger
+        # pending 旗標物件的儲存庫
-        from example_interfaces.srv import SetBool
+        self._pending_by_sysid = {} # sysid(int) : PendingEntry
 
         # ═══════════════════════════════════════════════════════════════════
-        # Service 1: 發送 MAVLink Message（通用介面）
+        # Service : ADD_TWO 測試用，未來刪除
-        # 使用 Trigger 作為臨時實現，未來應使用自定義 service
         # ═══════════════════════════════════════════════════════════════════
-        # TODO: 創建 SendMavlinkMessage.srv
+        from example_interfaces.srv import AddTwoInts
-        # Request:
+        self.srv_test = self.create_service(
-        #   uint8 target_sysid
+            AddTwoInts,
-        #   uint8 target_compid
+            'mavlink/add_two_ints',
-        #   uint16 message_id
+            self.handle_add_two_ints
-        #   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
+        # Service : TIMESYNC 可以作為模板
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_mav_ping = self.create_service(
+            fcsrv.MavPing,
+            self.serviceString_prefix + '/mav_ping',
+            self.handle_mav_timesync_ping
+        )
+
+        # ═══════════════════════════════════════════════════════════════════
+        # Service : 發送 COMMAND_LONG
         # ═══════════════════════════════════════════════════════════════════
         self.srv_command_long = self.create_service(
-            Trigger,
+            fcsrv.MavCommandLong,
-            '/mavlink/send_command_long',
+            self.serviceString_prefix + '/send_command_long',
             self.handle_command_long
         )
 
         # ═══════════════════════════════════════════════════════════════════
-        # Service 3: 參數請求
+        # Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
         # ═══════════════════════════════════════════════════════════════════
-        self.srv_param_request = self.create_service(
+        self.srv_pos_global_int = self.create_service(
-            Trigger,
+            fcsrv.MavPositionTargetGlobalInt,
-            '/mavlink/param_request',
+            self.serviceString_prefix + '/pos_global_int',
-            self.handle_param_request
+            self.handle_set_position_target_global_int
         )
 
-        # 狀態標記
-        self.running = True
-        
-        # mavlinkObject 的引用（將由外部設置）
-        self.mavlink_analyzer = None
-        
         logger.info("MavlinkCommandService initialized")
 
-    def set_mavlink_analyzer(self, mavlink_analyzer):
+    def _index(self, target_sysid, target_compid):
-        """
-        設置 mavlink_analyzer 引用
 
-        Args:
+        # 找到對應的 vehicle
-            mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例
+        vehicle = mvv.vehicle_registry.get(target_sysid)
-        """
+        if not vehicle:
-        self.mavlink_analyzer = mavlink_analyzer
+            return None
-        logger.info("MavlinkCommandService: mavlink_analyzer set")
+
+        socket_id = vehicle.custom_meta.get("socket_id")
+        if socket_id is None:
+            return None
+
+        # 提取主要的 socket_id
+        mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
+        if mav_obj is None:
+            return None
+        return mav_obj
+
+    def return_router(self):
+        '''
+        在節點管理器哪邊被呼叫
+        消耗 return_packet_ring 裡接收到的 mavlink 封包
+        分送到各自的 pending 中 
+        藉由 event.set() 解開 service 中的 block 
+        '''
+        return_tuple = mo.return_packet_ring.get()
+        # 確認 return_packet_ring 有資料
+        if return_tuple == None: return
+
+        socketid, timestamp, msg = return_tuple
+        sysid = msg.get_srcSystem()
+
+        _pending = self._pending_by_sysid.get(sysid)
+        # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
+        if _pending == None:
+            return
+
+        if _pending.match_fn(msg):
+            _pending.result_mav_msg = msg
+            _pending.event.set()
 
     # ═══════════════════════════════════════════════════════════════════════
-    # Service Handler: 發送 MAVLink Message
+    # Service Handler: 發送 TIMESYNC 可以作為模板
     # ═══════════════════════════════════════════════════════════════════════
-    def handle_send_message(self, request, response):
+    def handle_mav_timesync_ping(self, request, response):
-        """
+        '''
-        處理發送 MAVLink 訊息的請求
+        用 timesync 封包驗證來回的時間
-        
+        隸屬於 MavLink PING Protocol
-        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"
+        response.message = "Unknown error"
-            logger.error(response.message)
+        response.rtt_ms = 0.0
+        timeout_sec = 2.0
+
+        # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+        if request.target_sysid in self._pending_by_sysid:
+            response.message = f"sysid {request.target_sysid} already has pending request"
             return response
 
-        # TODO: 實際實現
+        # 2) 找到 socket 標地
-        # 1. 從 request 解析參數（target_sysid, message_id, fields 等）
+        socketObject = self._index(request.target_sysid, request.target_compid)
-        # 2. 使用 pymavlink 組裝 MAVLink 封包
+        if socketObject is None:
-        # 3. 調用 mavlink_analyzer.send_message() 發送
+            response.message = "This system id not found."
-        # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應
+            return response
+
+        # 3) 接收封包系統 的設定
+        # 在 socket 那邊先把要的封包種類導流進來
+        expect_recieve_msg_id = 111 # TIMESYNC
+        socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+        evt = threading.Event()
+        # 設定封包檢驗 
+        # 這邊是設定 tc1 不為 0 
+        def match_fn(compare_msg):
+            if compare_msg.get_msgId() != expect_recieve_msg_id : 
+                return False
+            # logger.debug(f"mark A : {compare_msg.get_srcSystem()} ,{compare_msg.get_msgId()}, {compare_msg.tc1}, {compare_msg.ts1}")
+            if compare_msg.tc1 == 0 :
+                return False
+            return True
+
+        _pending = PendingEntry(event = evt, match_fn = match_fn)
+
+        self._pending_by_sysid[request.target_sysid] = _pending
+
+        # 4) 送出封包
+        now_us = int(time.monotonic() * 1e6)
+        socketObject.MAVLink.timesync_send(0, now_us)
+
+        fail_skip = False
+        # 5) 等待回應封包
+        if not evt.wait(timeout_sec):
+            response.message = "mavlink timeout - TIMESYNC"
+            fail_skip = True
+
+        # 6) 處理回應封包
+        if not fail_skip:
+            ack_msg = _pending.result_mav_msg
+
+            current_time = int(time.monotonic() * 1e6)
+            response.rtt_ms = (current_time - now_us) / 1e3
+            response.message = ""
+
+        # 7) 接收封包系統 的重置
+        msg_types = list(socketObject.return_msg_types)
+        if expect_recieve_msg_id in msg_types:
+            msg_types.remove(expect_recieve_msg_id)
+        socketObject.set_return_message_types(msg_types)
+        del evt
+        self._pending_by_sysid.pop(request.target_sysid, None)
 
-        # 暫時返回成功（示範用）
-        response.success = True
-        response.message = "Message sent (placeholder implementation)"
         return response
 
     # ═══════════════════════════════════════════════════════════════════════
-    # Service Handler: 發送 COMMAND_LONG
+    # Service Handler: 發送 COMMAND_LONG (76)
     # ═══════════════════════════════════════════════════════════════════════
     def handle_command_long(self, request, response):
-        """
+        '''
-        處理發送 COMMAND_LONG 的請求
+        用 command long 丟出  MAV_CMD 
-        
+        隸屬於 MavLink Command Protocol
-        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"
+        response.message = "Unknown error"
+        response.ack_result = serviceAckResult.UNKNOWN
+        timeout_sec = request.timeout_sec
+
+        # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+        if request.target_sysid in self._pending_by_sysid:
+            response.message = f"sysid {request.target_sysid} already has pending request"
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
             return response
 
-        # TODO: 實際實現
+        # 2) 找到 socket 標地
-        # 1. 從 request 解析 COMMAND_LONG 參數
+        socketObject = self._index(request.target_sysid, request.target_compid)
-        #    - target_sysid, target_compid
+        if socketObject is None:
-        #    - command (MAV_CMD_xxx)
+            response.message = "This system id not found."
-        #    - param1~param7
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
-        # 2. 組裝 COMMAND_LONG 封包
+            return response
-        # 3. 發送並等待 COMMAND_ACK (message ID=77)
+
-        # 4. 解析 ACK 結果（ACCEPTED/FAILED 等）
+        # 3) 接收封包系統 的設定
+        expect_recieve_msg_id = 77 # mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK 
+        socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+        evt = threading.Event()
+
+        def match_fn(compare_msg):
+            if compare_msg.get_msgId() != expect_recieve_msg_id : 
+                return False
+            return True
+
+        _pending = PendingEntry(event = evt, match_fn = match_fn)
+
+        self._pending_by_sysid[request.target_sysid] = _pending
+
+        # 4) 送出封包
+        socketObject.MAVLink.command_long_send(
+            request.target_sysid,
+            request.target_compid,
+            request.command,
+            request.confirmation,
+            request.param1, request.param2, request.param3, request.param4,
+            request.param5, request.param6, request.param7
+        )
+
+        fail_skip = False
+        # 5) 等待回應封包
+        if not evt.wait(timeout_sec):
+            response.message = "mavlink timeout - CommLONG"
+            response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
+            fail_skip = True
+
+        # 6) 處理回應封包
+        if not fail_skip:
+            ack_msg = _pending.result_mav_msg
+            response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
+            response.message = "" # 沒有消息就是好消息
+            response.ack_result = ack_msg.result
+
+        # 7) 接收封包系統 的重置
+        msg_types = list(socketObject.return_msg_types)
+        if expect_recieve_msg_id in msg_types:
+            msg_types.remove(expect_recieve_msg_id)
+        socketObject.set_return_message_types(msg_types)
+        del evt
+        self._pending_by_sysid.pop(request.target_sysid, None)
+
+        return response
+
+    # ═══════════════════════════════════════════════════════════════════════
+    # Service Handler: 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+    # ═══════════════════════════════════════════════════════════════════════
+    def handle_set_position_target_global_int(self, request, response):
+        '''
+        隸屬於 MavLink Offboard Control Protocol
+        '''
+        # 設定失效回應
+        response.message = "Unknown error"
+        response.ack_result = serviceAckResult.UNKNOWN
+        timeout_sec = request.timeout_sec
+
+        # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+        if request.target_sysid in self._pending_by_sysid:
+            response.message = f"sysid {request.target_sysid} already has pending request"
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
+            return response
+
+        # 2) 找到 socket 標地
+        socketObject = self._index(request.target_sysid, request.target_compid)
+        if socketObject is None:
+            response.message = "This system id not found."
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+            return response
+
+        # 3) 接收封包系統 的設定
+        # 在 socket 那邊先把要的封包種類導流進來
+        expect_recieve_msg_id = 87 # POSITION_TARGET_GLOBAL_INT 
+        socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+        evt = threading.Event()
+        # 設定封包檢驗 
+        def match_fn(compare_msg):
+            if compare_msg.get_msgId() != expect_recieve_msg_id : 
+                return False
+            return True
+
+        # 4) 送出封包
+        socketObject.MAVLink.set_position_target_global_int_send(
+            request.time_boot_ms, 
+            request.target_sysid,
+            request.target_compid,
+            request.coordinate_frame,
+            request.type_mask,
+            request.lat_int, request.lon_int, request.alt,
+            request.vx, request.vy, request.vz,
+            request.afx, request.afy, request.afz,
+            request.yaw, request.yaw_rate
+        )
+
+        time.sleep(0.01) # 因應 mavlink 的廣播式回應 後置回應監聽 並且故意延遲10ms 讓系統代謝掉舊的87封包
+        _pending = PendingEntry(event = evt, match_fn = match_fn)
+        self._pending_by_sysid[request.target_sysid] = _pending
+
+        fail_skip = False
+        # 5) 等待回應封包
+        if not evt.wait(timeout_sec):
+            response.message = "mavlink timeout - SET POSITION 86"
+            response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
+            fail_skip = True
+
+        # 6) 處理回應封包
+        if not fail_skip:
+            ack_msg = _pending.result_mav_msg
+
+            response.message = ""
+            response.ack_result = 0
+            response.r_time_boot_ms = ack_msg.time_boot_ms 
+            response.r_type_mask = ack_msg.type_mask
+            response.r_lat_int = ack_msg.lat_int
+            response.r_lon_int = ack_msg.lon_int
+            response.r_alt = ack_msg.alt
+            response.r_vx = ack_msg.vx
+            response.r_vy = ack_msg.vy
+            response.r_vz = ack_msg.vz
+            response.r_afx = ack_msg.afx
+            response.r_afy = ack_msg.afy
+            response.r_afz = ack_msg.afz
+            response.r_yaw = ack_msg.yaw
+            response.r_yaw_rate = ack_msg.yaw_rate
+
+        # 7) 接收封包系統 的重置
+        msg_types = list(socketObject.return_msg_types)
+        if expect_recieve_msg_id in msg_types:
+            msg_types.remove(expect_recieve_msg_id)
+        socketObject.set_return_message_types(msg_types)
+        del evt
+        self._pending_by_sysid.pop(request.target_sysid, None)
 
-        response.success = True
-        response.message = "Command sent (placeholder implementation)"
         return response
 
     # ═══════════════════════════════════════════════════════════════════════
@@ -719,11 +933,19 @@ class MavlinkCommandService(Node):
         response.message = "Param request sent (placeholder implementation)"
         return response
 
-    # ═══════════════════════════════════════════════════════════════════════
+
-    # 【新增 Service 位置 4/4】
+    def handle_add_two_ints(self, request, response):
-    # 若要新增 service，請在此處添加新的 handler 方法
+        """測試用 Service Handler 未來刪除"""
-    # 並在 __init__ 中創建對應的 service server
+        logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
-    # ═══════════════════════════════════════════════════════════════════════
+        response.sum = request.a + request.b
+        logger.info(
+            f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
+        )
+        time.sleep(1)
+
+        return response
+
+
 
     def stop(self):
         """停止服務"""
@@ -739,6 +961,11 @@ class fc_ros_manager:
     """
     MAVLink ROS2 節點管理器
     
+    因為物件創建在 mavlinkROS2Nodes.py 中，所以會分為 __init__ 跟 initialize 兩個階段。
+
+    stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
+    shutdown 是完全關閉 ROS2 並銷毀節點實例。
+
     管理兩個獨立的 ROS2 Node：
     - VehicleStatusPublisher
     - MavlinkCommandService
@@ -750,7 +977,7 @@ class fc_ros_manager:
         self.initialized = False
         self.running = False
         
-        # 两个 node 实例
+        # 两个 node 實例
         self.status_publisher: Optional[VehicleStatusPublisher] = None
         self.command_service: Optional[MavlinkCommandService] = None
         
@@ -766,13 +993,14 @@ class fc_ros_manager:
         
         try:
             # 初始化 ROS2
-            rclpy.init()
+            if not rclpy.ok():
+                rclpy.init(args=None)
             
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
             self.command_service = MavlinkCommandService()
     
-            # 創建執行者 MultiThreadedExecutor
+            # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
             self.executor = MultiThreadedExecutor()
             self.executor.add_node(self.status_publisher)
             self.executor.add_node(self.command_service)
@@ -813,12 +1041,14 @@ class fc_ros_manager:
             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)
+                self.command_service.return_router()
         except Exception as e:
             logger.error(f"fc_ros_manager error in spinning executor: {e}")
     
@@ -899,4 +1129,16 @@ ros2_manager = fc_ros_manager()
 4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
 5. 預留 MavlinkCommandService 結構（稍後實現）
 
+2026.03.27
+1. 完成 ros2 service 結構與 timesync 與 command_long 協議
+
+2026.04.07
+1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
+2. 優化 response.ack_result 回傳值的資訊
+
+
+TODO 
+1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
+2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
 '''
+

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,14 +5,14 @@ VehicleView - Pure State Container
 """
 
 import os
-from typing import Dict, Optional, Any
+from typing import Dict, Optional, Any, Tuple
 from dataclasses import dataclass, field
 from enum import Enum
 
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-
+MODULE_VER = "1.00"
 
 # ====================== Enums =====================
 
@@ -402,11 +402,18 @@ class VehicleViewRegistry:
     
     def __init__(self):
         self._vehicles: Dict[int, VehicleView] = {}
+        # 高頻讀取用 snapshot（弱一致性）
+        self._vehicles_snapshot_items: Tuple[Tuple[int, VehicleView], ...] = tuple()
+
+    def _refresh_snapshot(self) -> None:
+        """重建快照（僅在 key 集合變動時觸發）"""
+        self._vehicles_snapshot_items = tuple(self._vehicles.items())
     
     def register(self, sysid: int) -> VehicleView:
         """註冊新的載具視圖"""
         if sysid not in self._vehicles:
             self._vehicles[sysid] = VehicleView(sysid)
+            self._refresh_snapshot()
             logger.info(f"Registered new VehicleView for system {sysid}")
         return self._vehicles[sysid]
     
@@ -418,6 +425,7 @@ class VehicleViewRegistry:
         """註銷載具視圖"""
         if sysid in self._vehicles:
             del self._vehicles[sysid]
+            self._refresh_snapshot()
             logger.info(f"Unregistered VehicleView for system {sysid}")
             return True
         return False
@@ -426,9 +434,21 @@ class VehicleViewRegistry:
         """取得所有載具視圖"""
         return self._vehicles.copy()
 
+    def read_all(self) -> Tuple[Tuple[int, VehicleView], ...]:
+        """
+        高效讀取所有載具視圖快照（弱一致性）
+
+        注意:
+        - 回傳 immutable tuple 快照，適合高頻迭代
+        - 僅在 register/unregister/clear 後更新，可能不是最新資料
+        - 適合高頻讀取且可接受弱一致性的情境
+        """
+        return self._vehicles_snapshot_items
+    
     def clear(self) -> None:
         """清空所有載具視圖"""
         self._vehicles.clear()
+        self._refresh_snapshot()
         logger.info("Cleared all VehicleViews")
     
     def __len__(self) -> int:
@@ -449,5 +469,8 @@ vehicle_registry = VehicleViewRegistry()
 2026.01.16
 1. 新增 重置指定組件的封包統計 功能
 
+2026.04.13
+1. 新增 VehicleViewRegistry.read_all 方法 讓 ROS2 Node 組件更有效率的讀取其中的資料
+
 '''
 

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -15,6 +15,7 @@ import time
 import threading
 import struct
 from enum import Enum, auto
+from abc import ABC, abstractmethod
 
 # # XBee 模組
 # from xbee.frame import APIFrame
@@ -25,8 +26,108 @@ from .utils import setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "0.60"
 
 # ====================== 分割線 =====================
+
+# 定義 serial 連線的模式
+class SerialMode(Enum):
+    """連接類型"""
+    STRAIGHT = auto()      # 原始數據直通
+    XBEEAPI2AT = auto()    # XBee API 模式
+    NOT_USE = auto()       # 不使用
+
+
+# ====================== Frame Processor 基類與實現 =====================
+
+class FrameProcessor(ABC):
+    """協議處理器基類"""
+    
+    def __init__(self):
+        self.buffer = bytearray()
+    
+    @abstractmethod
+    def process_incoming(self, data: bytes):
+        """
+        處理接收到的數據
+        返回：已完整解析的 payload 列表
+        """
+        pass
+    
+    @abstractmethod
+    def process_outgoing(self, data: bytes) -> bytes:
+        """
+        封裝要發送的數據
+        返回：封裝後的完整幀
+        """
+        pass
+
+
+class RawFrameProcessor(FrameProcessor):
+    """原始數據直通處理器"""
+    
+    def process_incoming(self, data: bytes):
+        """直接返回原始數據，不進行緩衝"""
+        return [data] if data else []
+    
+    def process_outgoing(self, data: bytes) -> bytes:
+        """直接返回原始數據，不進行封裝"""
+        return data
+
+
+class XBeeFrameProcessor(FrameProcessor):
+    """XBee API 協議處理器"""
+    
+    def process_incoming(self, data: bytes):
+        """處理 XBee API 幀並提取 payload"""
+        self.buffer.extend(data)
+        payloads = []
+        
+        while len(self.buffer) >= 3:
+            # 尋找幀頭
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            
+            # 讀取 payload 長度
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1  # 起始符(1) + 長度(2) + payload + 校驗和(1)
+            
+            # 等待完整幀
+            if len(self.buffer) < full_length:
+                break
+            
+            # 提取完整 frame 並從緩衝區移除
+            frame = bytes(self.buffer[:full_length])
+            del self.buffer[:full_length]
+            
+            # 判斷 frame 類型並處理
+            frame_type = frame[3]
+            
+            if frame_type == 0x90:  # RX Packet
+                payload = XBeeFrameHandler.decapsulate_data(frame)
+                if payload:
+                    payloads.append(payload)
+            elif frame_type == 0x88:  # AT Response
+                # 可以在這裡處理 AT 指令回應
+                # response = XBeeFrameHandler.parse_at_command_response(frame)
+                # 目前忽略
+                pass
+            elif frame_type == 0x8B:  # Transmit Status
+                # 傳輸狀態，目前忽略
+                pass
+            else:
+                logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+        
+        return payloads
+    
+    def process_outgoing(self, data: bytes) -> bytes:
+        """將數據封裝為 XBee API 傳輸幀"""
+        return XBeeFrameProcessor.encapsulate_data(data)
+
+
+# ====================== XBee Frame Handler =====================
+
 class XBeeFrameHandler:
     """XBee API Frame 處理器"""
     
@@ -55,7 +156,7 @@ class XBeeFrameHandler:
     
     @staticmethod
     def parse_receive_packet(frame: bytes) -> dict:
-        # """解析 RX Packet (0x90) - 未來擴展用"""
+        """解析 RX Packet (0x90) - 未來擴展用"""
         # if len(frame) < 15 or frame[3] != 0x90:
         #     return None
         
@@ -91,34 +192,12 @@ class XBeeFrameHandler:
 
     @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:
@@ -137,7 +216,7 @@ class ATCommandHandler:
         """根據 AT 指令類型分派處理"""
         if not response or not response['is_ok']:
             if response:
-                print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+                logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
             return
         
         command = response['command']
@@ -146,189 +225,122 @@ class ATCommandHandler:
         if handler:
             handler(response['data'])
         else:
-            print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
+            logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
     
     def _handle_rssi(self, data: bytes):
         """處理 DB (RSSI) 回應"""
-        if not data:
+        # 未來可實現 RSSI 處理邏輯
-            return
+        pass
-        
-        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) - 範例"""
+        """處理 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) - 範例"""
+        """處理 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 收發
+# ====================== Serial Handler =====================
-    def __init__(self, udp_handler, serial_port_str):
+
+class SerialHandler(asyncio.Protocol):
+    """asyncio.Protocol 用於處理 Serial 收發"""
+    
+    def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode):
         self.udp_handler = udp_handler  # UDP 的傳輸物件
         self.serial_port_str = serial_port_str
-        self.at_handler = ATCommandHandler(serial_port_str)
+        self.serial_mode = serial_mode
-
-        self.buffer = bytearray()  # 用於緩存接收到的資料
         self.transport = None  # Serial 自己的傳輸物件
-        # self.first_data = True  # 標記是否為第一次收到資料
+        
-        # self.has_processed = False # 測試模式用 處理數據旗標 # debug
+        # 根據模式創建對應的 processor
+        self.processor = self._create_processor(serial_mode)
+        
+        # AT 指令處理器（僅 XBee 模式使用）
+        if serial_mode == SerialMode.XBEEAPI2AT:
+            self.at_handler = ATCommandHandler(serial_port_str)
+        else:
+            self.at_handler = None
+
+    def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor:
+        """工廠方法：根據模式創建處理器"""
+        if serial_mode == SerialMode.STRAIGHT:
+            return RawFrameProcessor()
+        elif serial_mode == SerialMode.XBEEAPI2AT:
+            return XBeeFrameProcessor()
+        else:
+            logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
+            return RawFrameProcessor()
 
     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
+        logger.debug(f"Serial port {self.serial_port_str} connected")
 
-    # Serial 收到資料的處理過程
     def data_received(self, data):
-        # 1. 把收到的資料加入緩衝區
+        """Serial 收到資料的處理過程"""
-        self.buffer.extend(data) 
+        # 使用 processor 處理接收到的數據
+        payloads = self.processor.process_incoming(data)
         
-        # 2. 需要完整的 header 才能解析
+        # 將所有解析完成的 payload 轉發到 UDP
-        while len(self.buffer) >= 3:
+        for payload in payloads:
-            # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) 
+            self.udp_handler.transport.sendto(
-            if self.buffer[0] != 0x7E:
+                payload,
-                self.buffer.pop(0) # 如果不是就丟掉
+                (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)
-                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 收發
+# ====================== UDP Handler =====================
+
+class UDPHandler(asyncio.DatagramProtocol):
+    """asyncio.DatagramProtocol 用於處理 UDP 收發"""
     
     LOCAL_HOST_IP = '127.0.0.1'  # 只送給本地端 IP
 
-    def __init__(self, target_port):
+    def __init__(self, target_port, serial_mode: SerialMode):
         self.target_port = target_port  # 目標 UDP 端口
-
+        self.serial_mode = serial_mode
         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
+        logger.debug(f"UDP transport ready for port {self.target_port}")
 
     def set_serial_handler(self, serial_handler):
+        """設置對應的 Serial Handler"""
         self.serial_handler = serial_handler
 
-    # UDP 收到資料的處理過程
     def datagram_received(self, data, addr):
-        # 儲存對方的地址（這樣就能向同一個來源回傳數據）
+        """UDP 收到資料的處理過程"""
-        # self.remote_addr = addr # debug
+        if not self.serial_handler:
-        # print(f"Received UDP data from {addr}, setting as remote address")
+            logger.warning("Serial handler not set, dropping UDP packet")
+            return
         
-        processed_data = XBeeFrameHandler.encapsulate_data(data)
+        # 使用 processor 封裝數據
+        processed_data = self.serial_handler.processor.process_outgoing(data)
         
-        if self.serial_handler:
+        # 發送到串口
         self.serial_handler.transport.write(processed_data)
 
-#==================================================================
-
-class SerialReceiverType(Enum):
-    """連接類型"""
-    TELEMETRY = auto() 
-    XBEEAPI2AT = auto() 
-    OTHER = auto() 
 
+# ====================== Serial Manager =====================
 
 class serial_manager:
+    """串口管理器"""
 
     class serial_object:
-        def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
+        """串口物件"""
+        def __init__(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
             self.serial_port = serial_port  # /dev/ttyUSB or COM3 ...etc
             self.baudrate = baudrate
-            self.receiver_type = receiver_type
+            self.serial_mode = serial_mode
-            self.target_port = target_port # 指向的 UPD 端口
+            self.target_port = target_port  # 指向的 UDP 端口
 
-            self.transport = None # TODO 這個變數可能沒有作用
+            self.transport = None
-            self.protocol = None # TODO 這個變數可能沒有作用
+            self.protocol = None
             self.udp_handler = None
             self.serial_handler = None
 
@@ -344,14 +356,14 @@ class serial_manager:
         self.thread = None
 
     def start(self):
-
+        """啟動 serial_manager"""
         if self.running:
             logger.warning("serial_manager already running")
-            return
+            return False
         
         self.running = True
 
-        # 啟動獨立線程 命名為 SerialManager
+        # 啟動獨立線程，命名為 SerialManager
         self.thread = threading.Thread(
             target=self._run_event_loop, 
             name="SerialManager"
@@ -375,7 +387,6 @@ class serial_manager:
 
     def shutdown(self):
         """停止 serial_manager 和其管理的所有 serial_object"""
-        # 自己在 running 狀態下才執行停止程序
         if not self.running:
             logger.warning("serial_manager is not running")
             return
@@ -404,37 +415,25 @@ class serial_manager:
         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):
+    def create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
-
+        """創建串口連接"""
         if not self.running or not self.loop:
             logger.error("Event loop not running, cannot create serial link")
             return False
 
-        # 檢查 serial port 有效
+        # 檢查 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._async_create_serial_link(serial_port, baudrate, target_port, serial_mode),
             self.loop
         )
 
@@ -442,8 +441,8 @@ class serial_manager:
             # 等待結果，設定合理的超時時間
             result = future.result(timeout=5.0)
             if result:
-                logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}")
+                logger.info(f"Create Serial Link: {serial_port} ({serial_mode.name}) -> UDP {target_port}")
-            return True
+            return result
         except asyncio.TimeoutError:
             logger.error(f"Timeout creating serial link for {serial_port}")
             return False
@@ -451,27 +450,28 @@ class serial_manager:
             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):
+    async def _async_create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
         """在事件循環線程中執行實際的連接創建"""
         try:
             # 創建 serial_object 實例
-            serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type)
+            serial_obj = self.serial_object(serial_port, baudrate, target_port, serial_mode)
 
             # 建立 UDP 處理器並指定目標端口位置
-            serial_obj.udp_handler = UDPHandler(target_port)
+            serial_obj.udp_handler = UDPHandler(target_port, serial_mode)
             
             # 建立 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
+            logger.debug(f"UDP endpoint created for {serial_port}")
 
-            # 建立 Serial 處理器，將 UDP 處理器傳給它
+            # 建立 Serial 處理器，將 UDP 處理器和模式傳給它
-            serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port)
+            serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port, serial_mode)
 
             # 建立 Serial 連接
             serial_transport, _ = await serial_asyncio.create_serial_connection(
@@ -481,14 +481,14 @@ class serial_manager:
                 baudrate=baudrate
             )
             
-            # logger.info(f"Serial connection created for {serial_port}") # debug
+            logger.debug(f"Serial connection created for {serial_port}")
 
             # 將 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
+            logger.debug(f"Serial object {serial_id} added to manager")
             return True
             
         except Exception as e:
@@ -501,12 +501,10 @@ class serial_manager:
 
     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
@@ -549,7 +547,7 @@ class serial_manager:
             
             # 從管理列表中移除
             del self.serial_objects[serial_id]
-            # logger.info(f"Serial object {serial_id} removed from manager") # debug
+            logger.debug(f"Serial object {serial_id} removed from manager")
             return True
             
         except Exception as e:
@@ -557,6 +555,7 @@ class serial_manager:
             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
@@ -593,6 +592,8 @@ class serial_manager:
             return False
 
 
+# ====================== 測試代碼 =====================
+
 if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
@@ -600,11 +601,16 @@ if __name__ == '__main__':
     SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
     SERIAL_BAUDRATE = 115200
     UDP_REMOTE_PORT = 14571
-    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT)
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
+
+    # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
+    # SERIAL_BAUDRATE = 115200
+    # UDP_REMOTE_PORT = 14571
+    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
 
     linked_serial = sm.get_serial_link()
     print(linked_serial)
-    time.sleep(10)
+    time.sleep(60)
 
     sm.remove_serial_link(1)
     time.sleep(3)

src/fc_network_adapter/tests/example_mavlink_client.py

@@ -0,0 +1,180 @@
+"""
+MavlinkCommandService 使用範例
+
+展示如何從其他 ROS2 節點調用 MAVLink 指令服務
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+import time
+
+
+class MavlinkClientExample(Node):
+    """
+    範例：MAVLink Service Client
+    
+    這個節點展示如何調用 MavlinkCommandService 提供的服務
+    """
+    
+    def __init__(self):
+        super().__init__('mavlink_client_example')
+        
+        # 創建 service client
+        self.client = self.create_client(
+            Trigger,
+            '/mavlink/send_command_long'
+        )
+        
+        # 等待服務可用
+        self.get_logger().info('等待 service 可用...')
+        self.client.wait_for_service()
+        self.get_logger().info('Service 已連接！')
+    
+    def send_arm_command(self):
+        """
+        範例：發送 ARM 指令
+        
+        實際使用時應該發送：
+        - message_id = 76 (COMMAND_LONG)
+        - command = 400 (MAV_CMD_COMPONENT_ARM_DISARM)
+        - param1 = 1 (ARM)
+        """
+        self.get_logger().info('發送 ARM 指令...')
+        
+        request = Trigger.Request()
+        # TODO: 實際使用時應該是自定義的 SendCommandLong.Request
+        # request.target_sysid = 1
+        # request.target_compid = 1
+        # request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
+        # request.param1 = 1.0   # 1 = ARM, 0 = DISARM
+        # request.param2 = 0.0
+        # ... param3-7
+        # request.timeout = 3.0
+        
+        # 異步調用
+        future = self.client.call_async(request)
+        
+        # 等待結果
+        rclpy.spin_until_future_complete(self, future)
+        
+        if future.done():
+            response = future.result()
+            if response.success:
+                self.get_logger().info('✓ ARM 指令發送成功')
+            else:
+                self.get_logger().error(f'✗ ARM 指令失敗: {response.message}')
+        else:
+            self.get_logger().error('✗ Service 調用超時')
+
+
+def main():
+    """主函數"""
+    rclpy.init()
+    
+    # 創建客戶端節點
+    client = MavlinkClientExample()
+    
+    # 發送指令
+    client.send_arm_command()
+    
+    # 清理
+    client.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
+
+
+"""
+═══════════════════════════════════════════════════════════════════════════
+進階使用範例：高層應用程式
+
+假設我們要創建一個 "任務控制器"，通過 MavlinkCommandService 來控制載具：
+
+```python
+class MissionController(Node):
+    def __init__(self):
+        super().__init__('mission_controller')
+        
+        # 創建各種 service clients
+        self.client_arm = self.create_client(
+            SendCommandLong, '/mavlink/send_command_long')
+        self.client_mode = self.create_client(
+            SendCommandLong, '/mavlink/send_command_long')
+        self.client_takeoff = self.create_client(
+            SendCommandLong, '/mavlink/send_command_long')
+        self.client_goto = self.create_client(
+            SendCommandInt, '/mavlink/send_command_int')
+    
+    def arm_vehicle(self, sysid=1):
+        \"\"\"解鎖載具\"\"\"
+        request = SendCommandLong.Request()
+        request.target_sysid = sysid
+        request.target_compid = 1
+        request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
+        request.param1 = 1.0   # ARM
+        
+        future = self.client_arm.call_async(request)
+        # 處理回應...
+    
+    def set_mode(self, sysid=1, mode='GUIDED'):
+        \"\"\"設置飛行模式\"\"\"
+        # 實現模式切換邏輯...
+        pass
+    
+    def takeoff(self, sysid=1, altitude=10.0):
+        \"\"\"起飛\"\"\"
+        request = SendCommandLong.Request()
+        request.target_sysid = sysid
+        request.target_compid = 1
+        request.command = 22  # MAV_CMD_NAV_TAKEOFF
+        request.param7 = altitude
+        
+        future = self.client_takeoff.call_async(request)
+        # 處理回應...
+    
+    def goto_position(self, sysid=1, lat=0, lon=0, alt=10):
+        \"\"\"前往指定位置\"\"\"
+        request = SendCommandInt.Request()
+        request.target_sysid = sysid
+        request.target_compid = 1
+        request.command = 192  # MAV_CMD_DO_REPOSITION
+        request.x = int(lat * 1e7)
+        request.y = int(lon * 1e7)
+        request.z = alt
+        
+        future = self.client_goto.call_async(request)
+        # 處理回應...
+    
+    def execute_mission(self):
+        \"\"\"執行完整任務\"\"\"
+        # 1. 解鎖
+        self.arm_vehicle()
+        time.sleep(1)
+        
+        # 2. 切換到 GUIDED 模式
+        self.set_mode(mode='GUIDED')
+        time.sleep(1)
+        
+        # 3. 起飛到 10 公尺
+        self.takeoff(altitude=10.0)
+        time.sleep(10)
+        
+        # 4. 前往目標點
+        self.goto_position(lat=25.033, lon=121.565, alt=10)
+        time.sleep(30)
+        
+        # 5. 返回並降落
+        # ...
+```
+
+這樣的設計讓高層應用可以：
+1. 完全不需要知道 MAVLink 協議細節
+2. 通過 ROS2 service 與載具互動
+3. 模組化開發不同功能
+4. 易於測試和維護
+
+═══════════════════════════════════════════════════════════════════════════
+"""

src/fc_network_apps/__init__.py

@@ -0,0 +1,10 @@
+from .longCommand import CommandLongClient, CommandLongResult
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
+
+
+__all__ = [
+    "CommandLongClient",
+    "PositionTargetGlobalIntClient",
+    "CommandLongResult",
+    "PositionTargetGlobalIntResult",
+]

src/fc_network_apps/longCommand.py

@@ -0,0 +1,303 @@
+"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令)."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional
+import asyncio
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavCommandLong
+
+COMMAND_DO_SET_MODE = 176
+COMMAND_NAV_LAND = 21
+COMMAND_NAV_TAKEOFF = 22
+COMMAND_COMPONENT_ARM_DISARM = 400
+
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+
+@dataclass
+class CommandLongResult:
+    success: bool
+    message: str
+    ack_result: int
+
+class CommandLongClient(Node):
+    """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
+
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        super().__init__("fc_command_long_client")
+        self._client = self.create_client(MavCommandLong, service_name)
+
+    def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
+        return self._client.wait_for_service(timeout_sec=timeout_sec)
+
+    def _send_command_long(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        command: int,
+        confirmation: int,
+        param1: float,
+        param2: float,
+        param3: float,
+        param4: float,
+        param5: float,
+        param6: float,
+        param7: float,
+        timeout_sec: float,
+    ) -> CommandLongResult:
+        req = MavCommandLong.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.command = command
+        req.confirmation = confirmation
+        req.param1 = param1
+        req.param2 = param2
+        req.param3 = param3
+        req.param4 = param4
+        req.param5 = param5
+        req.param6 = param6
+        req.param7 = param7
+        req.timeout_sec = float(timeout_sec)
+
+        future = self._client.call_async(req)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
+        if not future.done() or future.result() is None:
+            return CommandLongResult(
+                success=False,
+                message="Service timeout.",
+                ack_result=-1,
+            )
+
+        response = future.result()
+        return CommandLongResult(
+            success=response.success,
+            message=response.message,
+            ack_result=response.ack_result,
+        )
+
+    def change_mode(
+        self,
+        *,
+        target_sysid: int,
+        custom_mode: float,
+        target_compid: int = 0,
+        base_mode: float = 1.0,
+        confirmation: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_DO_SET_MODE,
+            confirmation=confirmation,
+            param1=float(base_mode),
+            param2=float(custom_mode),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    def takeoff(
+        self,
+        *,
+        target_sysid: int,
+        altitude_m: float,
+        target_compid: int = 0,
+        min_pitch_deg: float = 0.0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_TAKEOFF,
+            confirmation=0,
+            param1=float(min_pitch_deg),
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
+        )
+
+    def land(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        altitude_m: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_LAND,
+            confirmation=0,
+            param1=0.0,
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
+        )
+
+    def arm_disarm(
+        self,
+        *,
+        target_sysid: int,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_COMPONENT_ARM_DISARM,
+            confirmation=confirmation,
+            param1=1.0 if arm else 0.0,
+            param2=float(param2),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    # ============================================================================
+    # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
+    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
+    # ============================================================================
+
+    async def change_mode_async(
+        self,
+        *,
+        target_sysid: int,
+        custom_mode: float,
+        target_compid: int = 0,
+        base_mode: float = 1.0,
+        confirmation: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 change_mode（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.change_mode(
+                target_sysid=target_sysid,
+                custom_mode=custom_mode,
+                target_compid=target_compid,
+                base_mode=base_mode,
+                confirmation=confirmation,
+                timeout_sec=timeout_sec,
+            )
+        )
+
+    async def arm_disarm_async(
+        self,
+        *,
+        target_sysid: int,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 arm_disarm（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.arm_disarm(
+                target_sysid=target_sysid,
+                arm=arm,
+                target_compid=target_compid,
+                confirmation=confirmation,
+                param2=param2,
+                timeout_sec=timeout_sec,
+            )
+        )
+
+    async def takeoff_async(
+        self,
+        *,
+        target_sysid: int,
+        altitude_m: float,
+        target_compid: int = 0,
+        min_pitch_deg: float = 0.0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 takeoff（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.takeoff(
+                target_sysid=target_sysid,
+                altitude_m=altitude_m,
+                target_compid=target_compid,
+                min_pitch_deg=min_pitch_deg,
+                yaw_deg=yaw_deg,
+                latitude=latitude,
+                longitude=longitude,
+                timeout_sec=timeout_sec,
+            )
+        )
+
+    async def land_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        altitude_m: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 land（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.land(
+                target_sysid=target_sysid,
+                target_compid=target_compid,
+                yaw_deg=yaw_deg,
+                latitude=latitude,
+                longitude=longitude,
+                altitude_m=altitude_m,
+                timeout_sec=timeout_sec,
+            )
+        )

src/fc_network_apps/navigation.py

@@ -0,0 +1,262 @@
+"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavPositionTargetGlobalInt
+
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/pos_global_int"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+# MAV_FRAME
+MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # 使用 WGS84 GNSS 座標系 高度為相對於 Home 點高度
+
+# POSITION_TARGET_TYPEMASK
+# 1  1      Ignore position x
+# 2  2      Ignore position y
+# 3  4      Ignore position z
+# 4  8      Ignore velocity x
+# 5  16     Ignore velocity y
+# 6  32     Ignore velocity z
+# 7  64     Ignore acceleration x
+# 8  128	Ignore acceleration y
+# 9  256	Ignore acceleration z
+# 10 512	Use force instead of acceleration
+# 11 1024	Ignore yaw
+# 12 2048   Ignore yaw rate
+# 13+       Not Used 
+#                 6543210987654321
+POSITION_ONLY = 0b0000110111111000 # 忽略速度、加速度、偏航
+
+
+# 回傳 echo 與送出值比對時，浮點欄位允許的絕對誤差（MAVLink float 轉傳常有小差）
+_FLOAT_ECHO_ABS_TOL = 1e-5
+def _float_echo_equal(a: float, b: float) -> bool:
+    return math.isclose(float(a), float(b), rel_tol=0.0, abs_tol=_FLOAT_ECHO_ABS_TOL)
+
+
+def _echo_position_target_matches(
+    *,
+    type_mask: int,
+    lat_int: int,
+    lon_int: int,
+    alt: float,
+    vx: float,
+    vy: float,
+    vz: float,
+    afx: float,
+    afy: float,
+    afz: float,
+    yaw: float,
+    yaw_rate: float,
+    time_boot_ms: int,
+    resp,
+) -> tuple[bool, str]:
+    """
+    比對本次送出的軌跡欄位與 server 填回的 r_* 是否一致。
+    若不符，回傳 (False, 簡短說明)；相符則 (True, "")。
+    """
+
+    _tm_sent = int(type_mask) & 0xFFF
+    _tm_recv = int(resp.r_type_mask) & 0xFFF
+    if _tm_recv != _tm_sent:
+        return False, f"type_mask echo: sent {_tm_sent:012b} != ack {_tm_recv:012b}"
+
+    coordinate_tolerance = 5 * 100
+    if abs(resp.r_lat_int - lat_int) >= coordinate_tolerance:
+        return False, f"lat_int echo: sent {lat_int} != ack {resp.r_lat_int}"
+    if abs(resp.r_lon_int - lon_int) >= coordinate_tolerance:
+        return False, f"lon_int echo: sent {lon_int} != ack {resp.r_lon_int}"
+    # TODO 我給飛控相對高度 它回給我絕對高度 看來這個比較方法 還有很多改良點 但是之後吧
+    # if not _float_echo_equal(resp.r_alt, alt):
+    #     return False, f"alt echo: sent {alt} != ack {resp.r_alt}"
+    if not _float_echo_equal(resp.r_vx, vx):
+        return False, f"vx echo: sent {vx} != ack {resp.r_vx}"
+    if not _float_echo_equal(resp.r_vy, vy):
+        return False, f"vy echo: sent {vy} != ack {resp.r_vy}"
+    if not _float_echo_equal(resp.r_vz, vz):
+        return False, f"vz echo: sent {vz} != ack {resp.r_vz}"
+    if not _float_echo_equal(resp.r_afx, afx):
+        return False, f"afx echo: sent {afx} != ack {resp.r_afx}"
+    if not _float_echo_equal(resp.r_afy, afy):
+        return False, f"afy echo: sent {afy} != ack {resp.r_afy}"
+    if not _float_echo_equal(resp.r_afz, afz):
+        return False, f"afz echo: sent {afz} != ack {resp.r_afz}"
+    if not _float_echo_equal(resp.r_yaw, yaw):
+        return False, f"yaw echo: sent {yaw} != ack {resp.r_yaw}"
+    if not _float_echo_equal(resp.r_yaw_rate, yaw_rate):
+        return False, f"yaw_rate echo: sent {yaw_rate} != ack {resp.r_yaw_rate}"
+    # if int(resp.r_time_boot_ms) != int(time_boot_ms):
+    #     return False, f"time_boot_ms echo: sent {time_boot_ms} != r {resp.r_time_boot_ms}"
+    return True, ""
+
+
+@dataclass
+class PositionTargetGlobalIntResult:
+    """對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
+    success: bool
+    message: str
+    ack_result: int
+    r_time_boot_ms: int = 0
+    r_type_mask: int = 0
+    r_lat_int: int = 0
+    r_lon_int: int = 0
+    r_alt: float = 0.0
+    r_vx: float = 0.0
+    r_vy: float = 0.0
+    r_vz: float = 0.0
+    r_afx: float = 0.0
+    r_afy: float = 0.0
+    r_afz: float = 0.0
+    r_yaw: float = 0.0
+    r_yaw_rate: float = 0.0
+
+class PositionTargetGlobalIntClient(Node):
+    """
+    ROS2 client：對 pos_global_int service 發送 SET_POSITION_TARGET_GLOBAL_INT (msg 86)，
+    並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。
+    """
+
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        super().__init__("fc_position_target_global_int_client")
+        self._client = self.create_client(MavPositionTargetGlobalInt, service_name)
+
+    def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
+        return self._client.wait_for_service(timeout_sec=timeout_sec)
+
+    def _send_position_target_global_int(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        time_boot_ms: int,
+        coordinate_frame: int,
+        type_mask: int,
+        lat_int: int,
+        lon_int: int,
+        alt: float,
+        vx: float,
+        vy: float,
+        vz: float,
+        afx: float,
+        afy: float,
+        afz: float,
+        yaw: float,
+        yaw_rate: float,
+        timeout_sec: float,
+    ) -> PositionTargetGlobalIntResult:
+        req = MavPositionTargetGlobalInt.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.time_boot_ms = time_boot_ms
+        req.coordinate_frame = coordinate_frame
+        req.type_mask = type_mask
+        req.lat_int = lat_int
+        req.lon_int = lon_int
+        req.alt = float(alt)
+        req.vx = float(vx)
+        req.vy = float(vy)
+        req.vz = float(vz)
+        req.afx = float(afx)
+        req.afy = float(afy)
+        req.afz = float(afz)
+        req.yaw = float(yaw)
+        req.yaw_rate = float(yaw_rate)
+        req.timeout_sec = float(timeout_sec)
+
+        future = self._client.call_async(req)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=float(timeout_sec) + 1.0)
+        if not future.done() or future.result() is None:
+            return PositionTargetGlobalIntResult(
+                success=False,
+                message="Service timeout.",
+                ack_result=-1,
+            )
+
+        resp = future.result()
+
+        if resp.ack_result != 0:
+            return PositionTargetGlobalIntResult(
+                success=False,
+                message=resp.message,
+                ack_result=resp.ack_result,
+            )
+
+        echo_ok, echo_detail = _echo_position_target_matches(
+            type_mask=type_mask,
+            lat_int=lat_int,
+            lon_int=lon_int,
+            alt=alt,
+            vx=vx,
+            vy=vy,
+            vz=vz,
+            afx=afx,
+            afy=afy,
+            afz=afz,
+            yaw=yaw,
+            yaw_rate=yaw_rate,
+            time_boot_ms=time_boot_ms,
+            resp=resp,
+        )
+        # out_message = resp.message
+        # if not echo_ok:
+        #     out_message = echo_detail if not out_message else f"{echo_detail} | server: {out_message}"
+
+        return PositionTargetGlobalIntResult(
+            success=echo_ok,
+            message=echo_detail,
+            ack_result=resp.ack_result,
+            r_time_boot_ms=int(resp.r_time_boot_ms),
+            r_type_mask=int(resp.r_type_mask),
+            r_lat_int=int(resp.r_lat_int),
+            r_lon_int=int(resp.r_lon_int),
+            r_alt=float(resp.r_alt),
+            r_vx=float(resp.r_vx),
+            r_vy=float(resp.r_vy),
+            r_vz=float(resp.r_vz),
+            r_afx=float(resp.r_afx),
+            r_afy=float(resp.r_afy),
+            r_afz=float(resp.r_afz),
+            r_yaw=float(resp.r_yaw),
+            r_yaw_rate=float(resp.r_yaw_rate),
+        )
+
+    def goto_position_only(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        latitude_deg: float,
+        longitude_deg: float,
+        alt: float = 0.0,
+        timeout_sec: float,
+    ):
+        lat_int = int(round(latitude_deg * 1e7))
+        lon_int = int(round(longitude_deg * 1e7))
+        
+        return self._send_position_target_global_int(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            time_boot_ms= 0,
+            coordinate_frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            type_mask=POSITION_ONLY,
+            lat_int=lat_int,
+            lon_int=lon_int,
+            alt=alt,
+            vx=0,
+            vy=0,
+            vz=0,
+            afx=0,
+            afy=0,
+            afz=0,
+            yaw=0,
+            yaw_rate=0,
+            timeout_sec=timeout_sec,
+        )

src/someotherpkg/src/example_wholeMoving.py

@@ -0,0 +1,111 @@
+
+
+import fc_network_apps as fap
+import time
+
+def main():
+    commandAPI = fap.CommandLongClient()
+    navAPI = fap.PositionTargetGlobalIntClient()
+
+    target_sysid = 3
+
+
+    print("=== change mode ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.change_mode(
+            target_sysid=target_sysid,
+            base_mode=1.0,
+            custom_mode=4.0,
+            timeout_sec=3.0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+    print("=== arm vehicle ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.arm_disarm(
+            target_sysid=target_sysid,
+            arm = True,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+    print("=== takeoff ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.takeoff(
+            target_sysid=target_sysid,
+            altitude_m = 100.0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+
+    time.sleep(15)
+
+    print("=== set_position_target_global_int_deg ===")
+    result_deg = navAPI.goto_position_only(
+        target_sysid=target_sysid,
+        latitude_deg=-35.376655,
+        longitude_deg=149.157011,
+        alt=150.0,
+        timeout_sec=3.0,
+    )
+    print(f"success   : {result_deg.success}")
+    print(f"ack_result: {result_deg.ack_result}")
+    print(f"message   : {result_deg.message}")
+    print(f"r_lat_int : {result_deg.r_lat_int}  r_lon_int: {result_deg.r_lon_int}")
+
+    time.sleep(180)
+
+    result_deg = navAPI.goto_position_only(
+        target_sysid=target_sysid,
+        latitude_deg=-35.3632621,
+        longitude_deg=149.1652375,
+        alt=100.0,
+        timeout_sec=3.0,
+    )
+    print("=== set_position_target_global_int_deg ===")
+    print(f"success   : {result_deg.success}")
+    print(f"ack_result: {result_deg.ack_result}")
+    print(f"message   : {result_deg.message}")
+    print(f"r_lat_int : {result_deg.r_lat_int}  r_lon_int: {result_deg.r_lon_int}")
+
+    time.sleep(180)
+
+    print("=== land ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.land(
+            target_sysid=3,
+            target_compid=0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+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.sysids = [1, 5, 10]
-        self.connection = mavutil.mavlink_connection(usb, baud=57600)
+        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:
+        for sysid in self.sysids:
-            sysid = int(sysid.replace('UAV', ''))
             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:
+        for sysid in self.sysids:
-            self.request_param(namespace, "SR1_EXTRA1")
+            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):
+    def request_param(self, sysid, param_name):
-        sysid = int(namespace.replace('UAV', ''))
         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, 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 訊息頻率 """
-        freqs = [0, 2, 4]
+        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):
+    def init_drone(self, namespace):
-        """ 直接設置 MAVLink 訊息頻率 """
+        sysid = self.get_sysid(namespace)
-        freqs = [0, 1, 1]
+        self.set_local_position(sysid, 0, 0, 0)
-        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.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()

src/unitdev02/unitdev02/devnote.txt

@@ -22,14 +22,36 @@
 自己的常用指令
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+
+python -m someotherpkg.src.example_takeoff_land
+python -m someotherpkg.src.example_change_mode
 
 ros2 topic list
-ros2 topic echo 
+ros2 topic echo /fc_network/vehicle/sys3/battery
 
 
 /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
 
 mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560
 
+ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 8}"
+ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
+
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
+
+ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
 
 
+sudo tcpdump -i lo 'udp dst port 14561' -X
+sudo tcpdump -i lo 'udp dst port 14550' -X -vv
+sudo tcpdump -i lo -X udp port 14550
+
+幾個要討論的
+1. 專案文件中 .mat 檔案是幹嘛的?
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
+3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.

src/unitdev03/0328/close_loop.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import socket
+import re
+import time
+import math
+
+def main():
+    # 建立MAVLink連接
+    print("建立MAVLink連接...")
+    connection_string = "udp:127.0.0.1:14550"
+    master = mavutil.mavlink_connection(connection_string)
+    master.wait_heartbeat()
+    print("MAVLink連接已建立")
+    
+    # 切換到GUIDED模式
+    master.mav.set_mode_send(
+        master.target_system,
+        mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+        4  # GUIDED模式ID
+    )
+    print("已切換到GUIDED模式")
+    
+    # 解鎖無人機
+    master.mav.command_long_send(
+        master.target_system,
+        master.target_component,
+        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+        0,
+        1, 0, 0, 0, 0, 0, 0  # 1表示解鎖
+    )
+    print("無人機已解鎖")
+    
+    # 設置UDP服務器
+    server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_socket.bind(('0.0.0.0', 5000))
+    server_socket.settimeout(0.1)  # 非阻塞操作
+    print("UDP服務器已啟動，監聽端口5000")
+    
+    # 主循環，持續處理數據
+    while True:
+        try:
+            # 接收UDP數據
+            try:
+                data, _ = server_socket.recvfrom(1024)
+                decoded_data = data.decode('utf-8')
+                
+                # 提取velocity數據
+                vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data)
+                
+                if vel_match:
+                    velocity_str = vel_match.group(1)
+                    velocity = [float(x) for x in velocity_str.split(',')]
+                    vx, vy, vz = velocity
+                    
+                    # 獲取當前位置
+                    pos_msg = master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=0.5)
+                    if pos_msg:
+                        x, y, z = pos_msg.x, pos_msg.y, pos_msg.z
+                        
+                        # 設置type_mask - 僅使用速度控制
+                        type_mask = (
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+                            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
+                        )
+                        
+                        # 發送位置目標命令
+                        print(f"發送速度指令: vx={vx}, vy={vy}, vz={vz}")
+                        master.mav.set_position_target_local_ned_send(
+                            0,
+                            master.target_system,
+                            master.target_component,
+                            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                            type_mask,
+                            0, 0, 0,   
+                            vx, vy, vz,  # 速度
+                            0, 0, 0,  # 加速度
+                            0, 0  # 航向
+                        )
+                        print("success")
+                        # 要再去偵錯沒有data進來的狀況
+            except socket.timeout:
+                pass
+            
+            time.sleep(0.05)  # 控制循環速率
+            
+        except KeyboardInterrupt:
+            print("程序終止")
+            server_socket.close()
+            break
+        except Exception as e:
+            print(f"錯誤: {e}")
+            time.sleep(1)
+
+if __name__ == "__main__":
+    main()

src/unitdev03/0328/pymavlink_02.py

@@ -0,0 +1,497 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import time
+import math
+import threading
+import sys
+
+class DroneController:
+    def __init__(self, connection_string="udp:127.0.0.1:14550", drone_id=0):
+        self.master = mavutil.mavlink_connection(connection_string)
+        self.drone_id = drone_id
+        self.connection_string = connection_string
+        self.master.wait_heartbeat()
+        print(f"無人機 #{self.drone_id} 已建立連接!")
+
+    def set_mode(self, mode):
+        if mode == 'STABILIZE':
+            mode_id = 0
+        elif mode == 'AUTO':
+            mode_id = 3
+        elif mode == 'GUIDED':
+            mode_id = 4
+        elif mode == 'LOITER':
+            mode_id = 5
+        elif mode == 'RTL':
+            mode_id = 6
+        elif mode == 'LAND':
+            mode_id = 9
+        else:
+            print(f"不支援的模式: {mode}")
+            return False
+
+        self.master.mav.set_mode_send(
+            self.master.target_system,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            mode_id
+        )
+        
+        for _ in range(5):
+            msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg and msg.custom_mode == mode_id:
+                return True
+        return False
+
+    def arm(self, arm_state=True):
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,
+            1 if arm_state else 0, 0, 0, 0, 0, 0, 0
+        )
+        
+        for _ in range(5):
+            msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg:
+                armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
+                if (arm_state and armed) or (not arm_state and not armed):
+                    return True
+        return False
+
+    def takeoff(self, altitude):
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0,
+            0, 0, 0, 0, 0, 0,
+            altitude
+        )
+        
+        timeout = time.time() + 30
+        while time.time() < timeout:
+            msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            if msg:
+                current_alt = msg.relative_alt / 1000.0
+                if abs(current_alt - altitude) < 0.5:
+                    return True
+            time.sleep(0.1)
+        return False
+
+    def goto_position_local(self, x, y, z, vx=0, vy=0, vz=0, yaw=0, yaw_rate=0):
+        z = -z
+        
+        type_mask = (
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE
+        )
+        
+        print(f"無人機 #{self.drone_id} 發送位置指令: x={x}, y={y}, z={-z}, vx={vx}, vy={vy}, vz={vz}")
+        
+        self.master.mav.set_position_target_local_ned_send(
+            0,
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+            type_mask,
+            x, y, z,
+            vx, vy, vz,
+            0, 0, 0,
+            yaw, yaw_rate
+        )
+
+        print(f"無人機 #{self.drone_id} 開始監控位置變化...")
+        start_time = time.time()
+        last_progress_time = start_time
+        min_dist = float('inf')
+        
+        while time.time() - start_time < 30:
+            msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            if msg:
+                dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2)
+                min_dist = min(min_dist, dist)
+                
+                current_time = time.time()
+                if current_time - last_progress_time >= 5:
+                    if dist > min_dist + 0.1:
+                        print(f"無人機 #{self.drone_id} 偏離目標，重新調整")
+                        self.master.mav.set_position_target_local_ned_send(
+                            0, self.master.target_system, self.master.target_component,
+                            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                            type_mask,
+                            x, y, z, vx, vy, vz, 0, 0, 0, yaw, yaw_rate
+                        )
+                    last_progress_time = current_time
+                
+                print(f"無人機 #{self.drone_id} 當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m")
+                
+                if dist < 0.5:
+                    print(f"無人機 #{self.drone_id} 到達目標位置")
+                    return True
+                    
+                vel_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=False)
+                if vel_msg:
+                    vel_total = math.sqrt(vel_msg.vx**2 + vel_msg.vy**2 + vel_msg.vz**2)
+                    if dist < 1.0 and vel_total < 0.1:
+                        print(f"無人機 #{self.drone_id} 已穩定懸停在目標位置附近")
+                        return True
+                        
+            time.sleep(0.1)
+        
+        print(f"無人機 #{self.drone_id} 移動超時")
+        return False
+
+    def get_current_state(self):
+        state = {}
+        
+        # Mode and armed status
+        hb_msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=0.5)
+        if hb_msg:
+            mode_id = hb_msg.custom_mode
+            mode_name = {
+                0: "STABILIZE",
+                3: "AUTO",
+                4: "GUIDED",
+                5: "LOITER",
+                6: "RTL",
+                9: "LAND"
+            }.get(mode_id, f"Unknown({mode_id})")
+            
+            state['mode'] = mode_name
+            state['armed'] = bool(hb_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
+        
+        # Position
+        pos_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1.0)
+        if pos_msg:
+            state['position'] = {
+                'x': round(pos_msg.x, 2),
+                'y': round(pos_msg.y, 2),
+                'z': round(-pos_msg.z, 2)
+            }
+        
+        # GPS
+        gps_msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1.0)
+        if gps_msg:
+            state['gps_position'] = {
+                'lat': gps_msg.lat / 1e7,
+                'lon': gps_msg.lon / 1e7,
+                'alt': gps_msg.alt / 1000,
+                'relative_alt': gps_msg.relative_alt / 1000
+            }
+        
+        return state
+
+class MultiDroneController:
+    def __init__(self, num_drones=5):
+        self.drones = []
+        self.num_drones = num_drones
+        
+        for i in range(num_drones):
+            port = 14550 + i
+            connection_string = f"udp:127.0.0.1:{port}"
+            drone = DroneController(connection_string, i)
+            self.drones.append(drone)
+            print(f"已初始化無人機 #{i} 在端口 {port}")
+    
+    def get_drone(self, drone_id):
+        if 0 <= drone_id < len(self.drones):
+            return self.drones[drone_id]
+        return None
+    
+    def all_drones_command(self, command, *args):
+        results = []
+        threads = []
+        
+        for drone in self.drones:
+            thread = threading.Thread(
+                target=lambda d, cmd, args, results: results.append((d.drone_id, getattr(d, cmd)(*args))),
+                args=(drone, command, args, results)
+            )
+            threads.append(thread)
+            thread.start()
+            
+        for thread in threads:
+            thread.join()
+            
+        for drone_id, success in results:
+            print(f"無人機 #{drone_id} 執行 {command} 結果: {'成功' if success else '失敗'}")
+            
+        return all(success for _, success in results)
+
+def print_menu():
+    menu = [
+        "\n=== 多機無人機控制界面 ===",
+        "1. 選擇操作的無人機",
+        "2. 對所有無人機執行命令",
+        "3. 查看所有無人機狀態",
+        "4. 退出",
+        "0. 顯示此清單",
+        "=========================="
+    ]
+    for item in menu:
+        print(item)
+
+def print_drone_menu():
+    menu = [
+        "\n=== 單機無人機控制界面 ===",
+        "1. 切換至 GUIDED 模式",
+        "2. 切換至 LAND 模式",
+        "3. 解鎖",
+        "4. 上鎖",
+        "5. 起飛",
+        "6. 飛到指定位置",
+        "7. 切換至 RTL 模式(返航)",
+        "8. 查看當前狀態",
+        "9. 返回主菜單",
+        "0. 顯示此清單",
+        "=========================="
+    ]
+    for item in menu:
+        print(item)
+
+def print_all_drones_menu():
+    menu = [
+        "\n=== 所有無人機控制界面 ===",
+        "1. 全部切換至 GUIDED 模式",
+        "2. 全部切換至 LAND 模式",
+        "3. 全部解鎖",
+        "4. 全部上鎖",
+        "5. 全部起飛",
+        "6. 全部返航",
+        "7. 返回主菜單",
+        "0. 顯示此清單",
+        "=========================="
+    ]
+    for item in menu:
+        print(item)
+
+def control_single_drone(drone):
+    print_drone_menu()
+    current_drone_id = drone.drone_id
+    
+    while True:
+        try:
+            choice = input(f"\n無人機 #{current_drone_id} - 請輸入指令編號: ")
+
+            if choice == '0':
+                print_drone_menu()
+                
+            elif choice == '1':
+                if drone.set_mode("GUIDED"):
+                    print(f"無人機 #{current_drone_id} 已切換至 GUIDED 模式")
+                else:
+                    print(f"無人機 #{current_drone_id} 切換模式失敗")
+                    
+            elif choice == '2':
+                if drone.set_mode("LAND"):
+                    print(f"無人機 #{current_drone_id} 已切換至 LAND 模式")
+                else:
+                    print(f"無人機 #{current_drone_id} 切換模式失敗")
+                    
+            elif choice == '3':
+                if drone.arm(True):
+                    print(f"無人機 #{current_drone_id} 已解鎖")
+                else:
+                    print(f"無人機 #{current_drone_id} 解鎖失敗")
+                    
+            elif choice == '4':
+                if drone.arm(False):
+                    print(f"無人機 #{current_drone_id} 已上鎖")
+                else:
+                    print(f"無人機 #{current_drone_id} 上鎖失敗")
+                    
+            elif choice == '5':
+                altitude = float(input("請輸入起飛高度(公尺): "))
+                if drone.takeoff(altitude):
+                    print(f"無人機 #{current_drone_id} 已到達目標高度: {altitude}公尺")
+                else:
+                    print(f"無人機 #{current_drone_id} 起飛失敗")
+                    
+            elif choice == '6':
+                x = float(input("請輸入X座標(公尺): "))
+                y = float(input("請輸入Y座標(公尺): "))
+                z = float(input("請輸入Z座標(公尺,高度為正值): "))
+                vx = float(input("請輸入X方向速度(公尺/秒，可選): ") or "0")
+                vy = float(input("請輸入Y方向速度(公尺/秒，可選): ") or "0")
+                if drone.goto_position_local(x, y, z, vx, vy):
+                    print(f"無人機 #{current_drone_id} 已到達目標位置")
+                else:
+                    print(f"無人機 #{current_drone_id} 移動失敗")
+                    
+            elif choice == '7':
+                if drone.set_mode("RTL"):
+                    print(f"無人機 #{current_drone_id} 已切換至 RTL 模式，正在返航")
+                else:
+                    print(f"無人機 #{current_drone_id} 切換至 RTL 模式失敗")
+                    
+            elif choice == '8':
+                state = drone.get_current_state()
+                if state:
+                    print(f"\n=== 無人機 #{current_drone_id} 狀態 ===")
+                    print(f"模式: {state.get('mode', '未知')}")
+                    print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
+                    
+                    if 'gps_position' in state:
+                        gps = state['gps_position']
+                        print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
+                        
+                    if 'position' in state:
+                        pos = state['position']
+                        print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
+                    print("=============================")
+                else:
+                    print(f"無法獲取無人機 #{current_drone_id} 狀態")
+                    
+            elif choice == '9':
+                print("返回主菜單")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+def control_all_drones(controller):
+    print_all_drones_menu()
+    
+    while True:
+        try:
+            choice = input("\n請輸入指令編號: ")
+
+            if choice == '0':
+                print_all_drones_menu()
+                
+            elif choice == '1':
+                if controller.all_drones_command("set_mode", "GUIDED"):
+                    print("所有無人機已切換至 GUIDED 模式")
+                else:
+                    print("部分無人機切換模式失敗")
+                    
+            elif choice == '2':
+                if controller.all_drones_command("set_mode", "LAND"):
+                    print("所有無人機已切換至 LAND 模式")
+                else:
+                    print("部分無人機切換模式失敗")
+                    
+            elif choice == '3':
+                if controller.all_drones_command("arm", True):
+                    print("所有無人機已解鎖")
+                else:
+                    print("部分無人機解鎖失敗")
+                    
+            elif choice == '4':
+                if controller.all_drones_command("arm", False):
+                    print("所有無人機已上鎖")
+                else:
+                    print("部分無人機上鎖失敗")
+                    
+            elif choice == '5':
+                altitude = float(input("請輸入所有無人機起飛高度(公尺): "))
+                if controller.all_drones_command("takeoff", altitude):
+                    print(f"所有無人機已到達目標高度: {altitude}公尺")
+                else:
+                    print("部分無人機起飛失敗")
+                    
+            elif choice == '6':
+                if controller.all_drones_command("set_mode", "RTL"):
+                    print("所有無人機已切換至 RTL 模式，正在返航")
+                else:
+                    print("部分無人機切換至 RTL 模式失敗")
+                    
+            elif choice == '7':
+                print("返回主菜單")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+def display_all_drone_states(controller):
+    """顯示所有無人機狀態"""
+    print("\n=== 所有無人機狀態 ===")
+    
+    for drone in controller.drones:
+        max_attempts = 5
+        state = None
+        
+        for attempt in range(max_attempts):
+            state = drone.get_current_state()
+            if state and 'position' in state:
+                break
+            time.sleep(0.2)
+            
+        print(f"\n--- 無人機 #{drone.drone_id} ---")
+        print(f"連接: {drone.connection_string}")
+        
+        if not state:
+            print("狀態: 無法獲取完整數據")
+            continue
+            
+        print(f"飛行模式: {state.get('mode', '未知')}")
+        print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
+        
+        if 'gps_position' in state:
+            gps = state['gps_position']
+            print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
+            print(f"相對高度: {gps['relative_alt']:.2f}m")
+            
+        if 'position' in state:
+            pos = state['position']
+            print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
+            
+        print("-----------------------")
+    
+    print("=========================")
+
+def main():
+    controller = MultiDroneController(5)
+    print_menu()
+
+    while True:
+        try:
+            choice = input("\n請輸入指令編號: ")
+
+            if choice == '0':
+                print_menu()
+                
+            elif choice == '1':
+                drone_id = int(input("請輸入要操作的無人機ID (0-4): "))
+                drone = controller.get_drone(drone_id)
+                if drone:
+                    control_single_drone(drone)
+                else:
+                    print(f"無效的無人機ID: {drone_id}")
+                    
+            elif choice == '2':
+                control_all_drones(controller)
+                    
+            elif choice == '3':
+                display_all_drone_states(controller)
+                    
+            elif choice == '4':
+                print("程式結束")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+if __name__ == "__main__":
+    try:
+        main()
+    except KeyboardInterrupt:
+        print("\n程式被使用者中斷")
+        sys.exit(0)                

src/unitdev03/0328/test_transform.py

@@ -0,0 +1,331 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+import json
+
+def mavlink_analyzer_simple(count = 500):
+    # rclpy.init()
+    # node = rclpy.create_node('mavlink_analyzer_simple')
+ 
+    print('Inintializing connection')
+
+    connection_string="udp:127.0.0.1:14550"
+    connection = mavutil.mavlink_connection(connection_string)
+
+    print('Catch messages')
+    msg_count = {}
+
+    start_time = time.time()
+
+    for _ in range(count):
+        msg = connection.recv_msg()
+        # msg = connection.recv_match(blocking=True)
+        if msg:
+            msg_type = msg.get_type()
+            if msg_type in msg_count:
+                msg_count[msg_type] += 1
+            else:
+                msg_count[msg_type] = 1
+
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                print(msg)
+                pass
+
+        else:
+            print('No message yet, 1 second for data to fill')
+            sleep(1)
+
+    print('Messages Type received:')
+    print(msg_count)
+
+    end_time = time.time()
+    print('Time elapsed: ', end_time - start_time)
+
+    print('Closing connection')
+    connection.close()
+
+
+def fdm_parser_example(data=None):
+    if data is None:
+        data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+    # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+    parse_format = 'HHI16H'
+
+    if len(data) != struct.calcsize(parse_format):
+        print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+        exit(1)
+
+    decoded = struct.unpack(parse_format,data)
+
+    magic = decoded[0]
+    frame_rate_hz = decoded[1]
+    frame_count = decoded[2]
+    pwm = decoded[3:]
+
+
+    print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    沒有轉換數據格式
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9002))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #print(f'address:{addr_s2g}')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            #print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack:{data_g2s}')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
+    '''
+    # read info from SITL via MAVLink
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection = mavutil.mavlink_connection(connection_string)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    data_queue = queue.Queue()
+    servo_out = [0] * 16
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1)
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
+            sock.sendto(data, server_address)
+
+            # 新增：寫入 JSON 檔案
+            data_json = {
+                "magic": 0x481a,
+                "frame_rate_hz": frame_rate_fdm,
+                "frame_count": frame_count_fdm,
+                "pwm": list(servo_out)
+            }
+            with open('fdm_data.json', 'w') as f:
+                json.dump(data_json, f)
+
+            frame_count_fdm += 1
+            fdm_parser_example(data) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+
+def fdm_switch_example_one_with_remote_forwarding():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    並且將JSON數據轉發到指定的遠端IP
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9002))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    # 建立與遠端IP的socket連接
+    remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    remote_address = ('140.120.31.130', 9003)  # 遠端IP與埠號，您可以根據需要更改埠號
+    print(f'Setting up forwarding to remote address: {remote_address}')
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                
+                # 將JSON數據轉發到遠端IP
+                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            print(f'JSON Pack:{data_g2s}')
+            print(f'Forwarded data to remote IP: 140.120.31.130')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+print('Start')
+#fdm_switch_example_two()
+#mavlink_analyzer_simple()
+#fdm_switch_example_one()
+fdm_switch_example_one_with_remote_forwarding()
+#fdm_parser_example()

src/unitdev03/0328/testserver.py

@@ -0,0 +1,55 @@
+import socket
+import re
+
+def start_udp_server(host='0.0.0.0', port=5000, buffer_size=1024):
+    server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_socket.bind((host, port))
+    
+    print(f"UDP Server running on {host}:{port}")
+    print("Waiting for data...")
+    
+    position = None
+    velocity = None
+    
+    try:
+        while True:
+            data, client_address = server_socket.recvfrom(buffer_size)
+            
+            print(f"\n----- 接收到的資料 -----")
+            print(f"從 {client_address} 接收:")
+            
+            try:
+                decoded_data = data.decode('utf-8')
+                print(f"文本數據: {decoded_data}")
+                
+                # 直接使用正則表達式提取數據
+                pos_match = re.search(r'"position":\[([-\d\.\,]+)\]', decoded_data)
+                vel_match = re.search(r'"velocity":\[([-\d\.\,]+)\]', decoded_data)
+                
+                if pos_match:
+                    position_str = pos_match.group(1)
+                    position = [float(x) for x in position_str.split(',')]
+                    print(f"Position: {position}")
+                
+                if vel_match:
+                    velocity_str = vel_match.group(1)
+                    velocity = [float(x) for x in velocity_str.split(',')]
+                    print(f"Velocity: {velocity}")
+                
+                # 這裡可以對 position 和 velocity 進行進一步操作
+                
+            except UnicodeDecodeError:
+                print(f"二進制數據: {data.hex()}")
+            except Exception as e:
+                print(f"處理數據時出錯: {e}")
+            
+            print(f"數據大小: {len(data)} 字節")
+            print("-----------------------")
+            
+    except KeyboardInterrupt:
+        print("Server shutting down...")
+    finally:
+        server_socket.close()
+
+if __name__ == "__main__":
+    start_udp_server(port=5000)

src/unitdev03/0519/close_loop_multi_1.py

@@ -0,0 +1,297 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import socket
+import re
+import time
+import math
+import threading
+import json
+import os
+import datetime
+import queue
+import argparse
+from select import select
+
+class Drone:
+    def __init__(self, instance_id, mavlink_port, udp_port):
+        self.instance_id = instance_id
+        self.mavlink_port = mavlink_port
+        self.udp_port = udp_port
+        self.running = True
+        self.connection = None
+        
+        # 創建數據緩存
+        self.latest_position = None
+        
+        # 創建日誌佇列
+        self.log_queue = queue.Queue()
+        
+        # 創建日誌文件目錄
+        self.log_dir = "drone_logs"
+        if not os.path.exists(self.log_dir):
+            os.makedirs(self.log_dir)
+        # 創建一個含時間戳的日誌文件名
+        timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
+        self.log_file = os.path.join(self.log_dir, f"drone_{self.instance_id}_{timestamp}.log")
+        
+        # 通訊統計
+        self.packet_counter = 0
+        self.start_time = time.time()
+        self.packets_per_second = 0
+
+    def connect(self):
+        """僅建立MAVLink連接"""
+        print(f"Drone {self.instance_id}: 建立MAVLink連接 (Port {self.mavlink_port})...")
+        connection_string = f"udp:127.0.0.1:{self.mavlink_port}"
+        self.connection = mavutil.mavlink_connection(connection_string)
+        self.connection.wait_heartbeat()
+        print(f"Drone {self.instance_id}: MAVLink連接已建立")
+    
+    def set_guided_mode(self):
+        """切換到GUIDED模式"""
+        self.connection.mav.set_mode_send(
+            self.connection.target_system,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            4  # GUIDED模式ID
+        )
+        print(f"Drone {self.instance_id}: 已切換到GUIDED模式")
+    
+    def arm(self):
+        """解鎖無人機"""
+        self.connection.mav.command_long_send(
+            self.connection.target_system,
+            self.connection.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0, 1, 0, 0, 0, 0, 0, 0  # 1表示解鎖
+        )
+        print(f"Drone {self.instance_id}: 無人機已解鎖")
+    
+    def takeoff(self, altitude=3.0):
+        """起飛到指定高度"""
+        self.connection.mav.command_long_send(
+            self.connection.target_system,
+            self.connection.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0, 0, 0, 0, 0, 0, 0, altitude
+        )
+        print(f"Drone {self.instance_id}: 起飛命令已發送，目標高度{altitude}公尺")
+    
+    def mavlink_listener(self):
+        """事件驅動方式監聽MAVLink訊息"""
+        # 獲取底層socket
+        mavlink_socket = self.connection.port.fileno()
+        
+        while self.running:
+            # 使用select等待數據，無須sleep
+            readable, _, _ = select([mavlink_socket], [], [], 0.001)
+            if readable:
+                msg = self.connection.recv_msg()
+                if msg and msg.get_type() == 'LOCAL_POSITION_NED':
+                    self.latest_position = msg
+    
+    def log_worker(self):
+        """事件驅動方式處理日誌寫入"""
+        while self.running:
+            if not self.log_queue.empty():
+                try:
+                    # 批量處理日誌
+                    batch = []
+                    while not self.log_queue.empty() and len(batch) < 100:
+                        try:
+                            entry = self.log_queue.get_nowait()
+                            batch.append(entry)
+                        except queue.Empty:
+                            break
+                    
+                    # 寫入日誌
+                    if batch:
+                        with open(self.log_file, 'a', encoding='utf-8') as f:
+                            for entry in batch:
+                                f.write(json.dumps(entry, ensure_ascii=False) + "\n")
+                except Exception as e:
+                    print(f"Drone {self.instance_id}: 日誌寫入錯誤: {e}")
+            
+            # 每秒計算一次統計數據
+            now = time.time()
+            if now - self.start_time >= 1.0:
+                self.packets_per_second = self.packet_counter
+                print(f"Drone {self.instance_id} - 每秒封包數: {self.packets_per_second}")
+                self.packet_counter = 0
+                self.start_time = now
+            
+            # 使用select等待短暫時間而非sleep
+            select([], [], [], 0.01)  # 10ms的微小等待
+    
+    def log_packet(self, packet_data, packet_num):
+        """將數據加入日誌佇列"""
+        try:
+            timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+            log_entry = {
+                "timestamp": timestamp,
+                "packet_number": packet_num,
+                "data": packet_data
+            }
+            self.log_queue.put(log_entry)
+        except Exception as e:
+            print(f"Drone {self.instance_id}: 佇列添加錯誤: {e}")
+    
+    def run(self):
+        """使用事件驅動方式處理UDP通訊"""
+        # 啟動背景線程
+        mavlink_thread = threading.Thread(target=self.mavlink_listener)
+        mavlink_thread.daemon = True
+        mavlink_thread.start()
+        
+        log_thread = threading.Thread(target=self.log_worker)
+        log_thread.daemon = True
+        log_thread.start()
+        
+        # 設置UDP服務器
+        server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 262144)  # 增大接收緩衝區
+        server_socket.bind(('0.0.0.0', self.udp_port))
+        server_socket.setblocking(0)  # 非阻塞模式
+        
+        print(f"Drone {self.instance_id}: UDP服務器已啟動，監聽端口{self.udp_port}")
+        print(f"Drone {self.instance_id}: 日誌將保存至 {self.log_file}")
+        
+        # 主事件循環
+        while self.running:
+            # 使用select監聽UDP
+            readable, _, _ = select([server_socket], [], [], 0.001)
+            
+            if readable:
+                try:
+                    data, _ = server_socket.recvfrom(4096)
+                    decoded_data = data.decode('utf-8')
+                    
+                    # 增加計數器
+                    self.packet_counter += 1
+                    total_packets = self.packet_counter
+                    
+                    try:
+                        # 解析JSON
+                        json_data = json.loads(decoded_data)
+                        
+                        # 寫入日誌
+                        self.log_packet(json_data, total_packets)
+                        
+                        # 提取速度數據
+                        vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data)
+                        if vel_match:
+                            velocity_str = vel_match.group(1)
+                            velocity = [float(x) for x in velocity_str.split(',')]
+                            vx, vy, vz = velocity
+                            
+                            # 設置type_mask - 僅使用速度控制
+                            type_mask = (
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+                                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
+                            )
+                            
+                            # 直接發送速度指令，不需等待位置數據
+                            self.connection.mav.set_position_target_local_ned_send(
+                                0,
+                                self.connection.target_system,
+                                self.connection.target_component,
+                                mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                                type_mask,
+                                0, 0, 0,  # 位置
+                                vx, vy, -vz,  # 速度
+                                0, 0, 0,  # 加速度
+                                0, 0  # 航向
+                            )
+                            print(f"Drone {self.instance_id}: 發送速度指令: vx={vx}, vy={vy}, vz={-vz}")
+                    
+                    except json.JSONDecodeError as je:
+                        print(f"Drone {self.instance_id}: JSON解析錯誤: {je}")
+                        self.log_packet({"raw_data": decoded_data}, total_packets)
+                
+                except Exception as e:
+                    print(f"Drone {self.instance_id}: 處理錯誤: {e}")
+        
+        # 清理資源
+        server_socket.close()
+        print(f"Drone {self.instance_id}: 線程結束")
+
+def main():
+    # 解析命令行參數
+    parser = argparse.ArgumentParser(description='多無人機控制程序')
+    parser.add_argument('--drones', type=str, default='1,2,3',
+                        help='要控制的無人機ID，用逗號分隔 (例如: 1,2,3)')
+    args = parser.parse_args()
+    
+    # 所有可用的無人機配置
+    all_drone_configs = [
+        (1, 14550, 5003),
+        (2, 14560, 5013),
+        (3, 14570, 5023)
+    ]
+    
+    # 解析要使用的無人機ID
+    drone_ids = [int(id_str) for id_str in args.drones.split(',')]
+    drone_configs = [config for config in all_drone_configs if config[0] in drone_ids]
+    
+    drones = []
+    threads = []
+    
+    try:
+        # 創建所有無人機實例
+        for instance_id, mavlink_port, udp_port in drone_configs:
+            drone = Drone(instance_id, mavlink_port, udp_port)
+            drones.append(drone)
+        
+        # 步驟1: 所有無人機連接
+        for drone in drones:
+            drone.connect()
+        
+        # 適當延遲確保連接穩定
+        time.sleep(1)
+        
+        # 步驟2: 所有無人機同時切換到GUIDED模式
+        for drone in drones:
+            drone.set_guided_mode()
+        
+        time.sleep(1)
+        
+        # 步驟3: 所有無人機同時解鎖
+        for drone in drones:
+            drone.arm()
+        
+        time.sleep(1)
+        
+        # 步驟4: 所有無人機同時起飛
+        for drone in drones:
+            drone.takeoff()
+        
+        # 啟動監聽線程
+        for drone in drones:
+            thread = threading.Thread(target=drone.run)
+            thread.daemon = True
+            threads.append(thread)
+            thread.start()
+        
+        print(f"已啟動 {len(drones)} 台無人機 (ID: {args.drones})，按Ctrl+C結束程序")
+        
+        # 主線程事件循環
+        while True:
+            select([], [], [], 0.1)  # 使用select代替time.sleep
+            
+    except KeyboardInterrupt:
+        print("正在停止所有無人機...")
+        for drone in drones:
+            drone.running = False
+        
+        for thread in threads:
+            thread.join(timeout=2)
+        
+        print("程序已結束")
+
+if __name__ == "__main__":
+    main()

src/unitdev03/0519/enemy_status.py

@@ -0,0 +1,340 @@
+#!/usr/bin/env python3
+
+from pymavlink import mavutil
+import time
+import sys
+import json
+import socket
+import threading
+import os
+from datetime import datetime
+
+# 固定初始位置設定 (5,5,0)
+INITIAL_POSITION = [5, 5, 0]
+
+# MAVLink 連接設定
+connection_string = 'udp:127.0.0.1:14553'
+
+# 網路傳輸設定
+target_ip = '140.120.31.130'
+target_port = 9053
+
+# 建立 UDP socket 用於傳送數據
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+# 封包編號計數器
+packet_counter = 0
+
+# 共享變數 - 存儲最新位置
+current_position = None
+initial_position = INITIAL_POSITION.copy()
+position_lock = threading.Lock()
+
+# 連接到無人機
+print(f"嘗試連接到 MAVLink: {connection_string}")
+try:
+    connection = mavutil.mavlink_connection(connection_string, autoreconnect=True)
+    connection.wait_heartbeat()
+    print(f"成功連接到系統 {connection.target_system}!")
+except Exception as e:
+    print(f"MAVLink 連接失敗: {e}")
+    sys.exit(1)
+
+print(f"使用指定初始位置: 北={initial_position[0]}, 東={initial_position[1]}, 下={initial_position[2]}")
+
+# 請求位置數據流
+connection.mav.request_data_stream_send(
+    connection.target_system,
+    connection.target_component,
+    mavutil.mavlink.MAV_DATA_STREAM_POSITION,
+    10,
+    1
+)
+
+# 日誌記錄函數
+def start_logging():
+    """創建並開啟日誌文件"""
+    log_dir = "logs"
+    if not os.path.exists(log_dir):
+        os.makedirs(log_dir)
+    
+    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+    log_file = os.path.join(log_dir, f"drone_log_{timestamp}.csv")
+    
+    file = open(log_file, "w")
+    file.write("packet_id,timestamp,x,y,z,vx,vy,vz\n")  # CSV標頭
+    print(f"日誌記錄已開始: {log_file}")
+    return file
+
+# 控制函數
+def set_guided_mode():
+    """設置為 GUIDED 模式"""
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+        0,
+        mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+        4,
+        0, 0, 0, 0, 0
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def arm_throttle():
+    """解鎖油門"""
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+        0, 1, 0, 0, 0, 0, 0, 0
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def takeoff(altitude):
+    """起飛到指定高度"""
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+        0, 0, 0, 0, 0, 0, 0, altitude
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def set_speed_limit(speed_type, speed):
+    """
+    設置速度限制
+    
+    Parameters:
+    -----------
+    speed_type : int
+        速度類型 (0=空速, 1=地速, 2=爬升速度, 3=下降速度)
+    speed : float
+        速度值 (米/秒)
+    """
+    connection.mav.command_long_send(
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
+        0,  # 確認
+        speed_type,  # 參數1: 速度類型
+        speed,       # 參數2: 速度 (m/s)
+        -1,          # 參數3: 油門 (-1 表示不變)
+        0, 0, 0, 0   # 參數4-7: 未使用
+    )
+    ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+    success = ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+    if success:
+        print(f"已設置速度限制為 {speed:.2f} m/s")
+    else:
+        print("設置速度限制失敗")
+    return success
+
+def goto_position(north, east, down):
+    """前往指定位置 (世界座標系)"""
+    # 將世界座標轉換為相對座標
+    rel_north = north - initial_position[0]
+    rel_east = east - initial_position[1]
+    rel_down = down - initial_position[2]
+    
+    connection.mav.set_position_target_local_ned_send(
+        0,
+        connection.target_system,
+        connection.target_component,
+        mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+        0b0000111111111000,
+        rel_north, rel_east, rel_down,
+        0, 0, 0,
+        0, 0, 0,
+        0, 0
+    )
+    # print(f"相對位置: 北={rel_north:.2f}, 東={rel_east:.2f}, 下={rel_down:.2f}")
+
+def get_current_position():
+    """獲取存儲的當前位置"""
+    with position_lock:
+        if current_position is None:
+            return None
+        return current_position.copy()
+
+def control_interface():
+    """命令行控制界面"""
+    print("無人機控制介面已啟動，輸入命令：")
+    print("  arm - 解鎖油門")
+    print("  takeoff [高度] - 起飛")
+    print("  goto [北] [東] [高度] - 前往位置 (世界座標系)")
+    print("  pos - 顯示當前位置 (相對座標和世界座標)")
+    print("  init - 顯示初始位置")
+    print("  exit - 退出控制")
+    
+    while True:
+        try:
+            cmd = input("命令> ").strip().split()
+            if not cmd:
+                continue
+                
+            if cmd[0] == "arm":
+                print("嘗試解鎖油門...")
+                if set_guided_mode() and arm_throttle():
+                    print("成功解鎖油門！")
+                else:
+                    print("解鎖失敗")
+                    
+            elif cmd[0] == "takeoff":
+                alt = float(cmd[1]) if len(cmd) > 1 else 3.0
+                print(f"嘗試起飛到 {alt} 米高...")
+                if takeoff(alt):
+                    print("起飛命令已發送")
+                else:
+                    print("起飛失敗")
+                    
+            elif cmd[0] == "goto":
+                if len(cmd) >= 4:
+                    north = float(cmd[1])
+                    east = float(cmd[2])
+                    down = -float(cmd[3])  # 高度是負的下
+                    print(f"前往世界位置: 北:{north} 東:{east} 下:{-down} 米")
+                    goto_position(north, east, down)
+                else:
+                    print("用法: goto [北] [東] [高度]")
+            
+            elif cmd[0] == "pos":
+                pos = get_current_position()
+                if pos:
+                    rel_x, rel_y, rel_z = pos
+                    # 計算世界座標
+                    world_x = initial_position[0] + rel_x
+                    world_y = initial_position[1] + rel_y
+                    world_z = initial_position[2] + rel_z
+                    # NED 轉 ENU
+                    enu_x = rel_y
+                    enu_y = rel_x
+                    enu_z = -rel_z
+                    
+                    print("\n當前位置:")
+                    #print(f"  相對座標 (NED): 北={rel_x:.2f}m, 東={rel_y:.2f}m, 下={rel_z:.2f}m")
+                    print(f"  世界座標 : 北={world_x:.2f}m, 東={world_y:.2f}m, 下={-world_z:.2f}m")
+                    #print(f"  相對座標 (ENU): 東={enu_x:.2f}m, 北={enu_y:.2f}m, 上={enu_z:.2f}m")
+                    #print(f"  高度: {-rel_z:.2f}m")
+                else:
+                    print("尚未獲取到位置信息")
+
+            elif cmd[0] == "speed":
+                if len(cmd) > 1:
+                    speed = float(cmd[1])
+                    set_speed_limit(1, speed)  # 1 = 地速
+                else:
+                    print("用法: speed [速度m/s]")
+                    
+            elif cmd[0] == "climbspeed":
+                if len(cmd) > 1:
+                    speed = float(cmd[1])
+                    set_speed_limit(2, speed)  # 2 = 爬升速度
+                    set_speed_limit(3, speed)  # 3 = 下降速度
+                else:
+                    print("用法: climbspeed [速度m/s]")                    
+            
+            elif cmd[0] == "init":
+                print(f"初始位置 (NED): 北={initial_position[0]:.2f}m, 東={initial_position[1]:.2f}m, 下={initial_position[2]:.2f}m")
+                    
+            elif cmd[0] == "exit":
+                print("退出控制介面")
+                break
+                
+            else:
+                print(f"未知命令: {cmd[0]}")
+        except Exception as e:
+            print(f"命令執行錯誤: {e}")
+
+# 數據傳輸線程函數
+def data_sending_thread():
+    global packet_counter, current_position
+    
+    # 開啟日誌文件
+    log_file = start_logging()
+    
+    try:
+        print(f"開始接收數據並傳送到 {target_ip}:{target_port}...")
+        while True:
+            msg = connection.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            if msg is not None:
+                # 更新當前位置（線程安全）
+                with position_lock:
+                    current_position = [msg.x, msg.y, msg.z]
+                
+                # 增加封包編號
+                packet_counter += 1
+
+                # 計算世界座標
+                world_x = initial_position[0] + msg.x
+                world_y = initial_position[1] + msg.y
+                world_z = initial_position[2] + msg.z
+
+                # NED 轉換為 ENU 並使用世界座標
+                enu_position = {
+                    "x": world_x,  # 東 (從 NED 的 x)
+                    "y": world_y,  # 北 (從 NED 的 y)
+                    "z": -world_z  # 上 (從 NED 的 -z)
+                }
+                
+                enu_velocity = {
+                    "vx": msg.vy,  # 東向速度
+                    "vy": msg.vx,  # 北向速度
+                    "vz": -msg.vz  # 上向速度
+                }
+                
+                # 創建JSON對象 (使用世界座標)
+                json_data = {
+                    "packet_id": packet_counter,
+                    "timestamp": msg.time_boot_ms,
+                    "enemy_position": enu_position,
+                    "enemy_velocity": enu_velocity
+                }
+                
+                # 寫入日誌文件 (CSV格式)
+                try:
+                    log_file.write(f"{packet_counter},{msg.time_boot_ms},{enu_position['x']},{enu_position['y']},{enu_position['z']},{enu_velocity['vx']},{enu_velocity['vy']},{enu_velocity['vz']}\n")
+                    log_file.flush()  # 確保數據即時寫入文件
+                except Exception as e:
+                    print(f"寫入日誌失敗: {e}")
+                
+                json_str = json.dumps(json_data)
+                
+                try:
+                    sock.sendto(json_str.encode('utf-8'), (target_ip, target_port))
+                except Exception as e:
+                    print(f"傳送數據失敗: {e}")
+                
+                time.sleep(0.1)
+    except Exception as e:
+        print(f"數據傳輸線程錯誤: {e}")
+    finally:
+        # 確保關閉日誌文件
+        log_file.close()
+        print("日誌文件已關閉")
+
+# 創建並啟動數據發送線程
+sender_thread = threading.Thread(target=data_sending_thread)
+sender_thread.daemon = True
+sender_thread.start()
+
+# 啟動命令行控制界面
+try:
+    control_interface()
+except KeyboardInterrupt:
+    print("\n程序已被用戶中斷")
+finally:
+    if connection:
+        connection.mav.request_data_stream_send(
+            connection.target_system,
+            connection.target_component,
+            mavutil.mavlink.MAV_DATA_STREAM_ALL,
+            0, 0
+        )
+        print("數據流已停止")
+    sock.close()
+    print("網絡連接已關閉")
+    print(f"總共傳送了 {packet_counter} 個封包")
+    print("日誌文件已儲存")

src/unitdev03/0519/test_transform.py

@@ -0,0 +1,663 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+import json
+
+def mavlink_analyzer_simple(count = 500):
+    # rclpy.init()
+    # node = rclpy.create_node('mavlink_analyzer_simple')
+ 
+    print('Inintializing connection')
+
+    connection_string="udp:127.0.0.1:14550"
+    connection = mavutil.mavlink_connection(connection_string)
+
+    print('Catch messages')
+    msg_count = {}
+
+    start_time = time.time()
+
+    for _ in range(count):
+        msg = connection.recv_msg()
+        # msg = connection.recv_match(blocking=True)
+        if msg:
+            msg_type = msg.get_type()
+            if msg_type in msg_count:
+                msg_count[msg_type] += 1
+            else:
+                msg_count[msg_type] = 1
+
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                print(msg)
+                pass
+
+        else:
+            print('No message yet, 1 second for data to fill')
+            sleep(1)
+
+    print('Messages Type received:')
+    print(msg_count)
+
+    end_time = time.time()
+    print('Time elapsed: ', end_time - start_time)
+
+    print('Closing connection')
+    connection.close()
+
+
+def fdm_parser_example(data=None):
+    if data is None:
+        data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+    # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+    parse_format = 'HHI16H'
+
+    if len(data) != struct.calcsize(parse_format):
+        print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+        exit(1)
+
+    decoded = struct.unpack(parse_format,data)
+
+    magic = decoded[0]
+    frame_rate_hz = decoded[1]
+    frame_count = decoded[2]
+    pwm = decoded[3:]
+
+
+    print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    沒有轉換數據格式
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9012))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19012)
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #print(f'address:{addr_s2g}')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            #print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack:{data_g2s}')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
+    '''
+    # read info from SITL via MAVLink
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection = mavutil.mavlink_connection(connection_string)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19012)
+
+    data_queue = queue.Queue()
+    servo_out = [0] * 16
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1)
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
+            sock.sendto(data, server_address)
+
+            # 新增：寫入 JSON 檔案
+            data_json = {
+                "magic": 0x481a,
+                "frame_rate_hz": frame_rate_fdm,
+                "frame_count": frame_count_fdm,
+                "pwm": list(servo_out)
+            }
+            with open('fdm_data.json', 'w') as f:
+                json.dump(data_json, f)
+
+            frame_count_fdm += 1
+            fdm_parser_example(data) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+
+def fdm_switch_example_one_with_remote_forwarding():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    並且將JSON數據轉發到指定的遠端IP
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9012))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19012)
+
+    # 建立與遠端IP的socket連接
+    remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    remote_address = ('140.120.31.130', 9003)  # 遠端IP與埠號，您可以根據需要更改埠號
+    print(f'Setting up forwarding to remote address: {remote_address}')
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+
+            # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+            try:
+                # 解析從 SITL 到 Gazebo 的數據
+                decoded = struct.unpack('HHI16H', data_s2g)
+                s2g_json = {
+                    "magic": decoded[0],
+                    "frame_rate_hz": decoded[1],
+                    "frame_count": decoded[2],
+                    "pwm": list(decoded[3:])
+                }
+                
+                # 解析從 Gazebo 到 SITL 的數據
+                try:
+                    g2s_data_str = data_g2s.decode('utf-8')
+                    # 保存原始數據，適合後續分析
+                    with open('gazebo_response_raw.txt', 'a') as f:
+                        f.write(g2s_data_str + '\n')
+                except:
+                    # 如果不是 UTF-8 格式，可能是二進制數據
+                    g2s_data_str = str(data_g2s)
+                
+                # 合併數據到一個 JSON 對象
+                combined_data = {
+                    "sitl_to_gazebo": s2g_json,
+                    "gazebo_to_sitl": g2s_data_str,
+                    "timestamp": time.time()
+                }
+                
+                # 寫入 JSON 檔案
+                with open('fdm_one_data.json', 'w') as f:
+                    json.dump(combined_data, f, indent=2)
+                
+                # 將JSON數據轉發到遠端IP
+                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+                    
+            except Exception as e:
+                print(f"JSON 處理錯誤: {e}")
+
+            packet_count += 1
+        except socket.timeout:
+            print(f'no value')
+            #pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            print(f'JSON Pack:{data_g2s}')
+            print(f'Forwarded data to remote IP: 140.120.31.130')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_multiple(num_systems=3):
+    '''
+    Create multiple UDP forwarders between SITL and Gazebo using threading
+    Each forwarder uses its own set of ports following the pattern:
+    - SITL ports: 9002, 9012, 9022, ...
+    - Gazebo ports: 19002, 19012, 19022, ...
+    
+    Parameters:
+    num_systems (int): Number of forwarding pairs to create
+    '''
+    # Flag to control thread execution
+    running = True
+    
+    def forwarder_thread(system_id):
+        sitl_port = 9002 + (system_id * 10)
+        gazebo_port = 19002 + (system_id * 10)
+        
+        # Create SITL socket (server)
+        sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        sitl_socket.bind(('', sitl_port))
+        sitl_socket.settimeout(0.1)
+        
+        # Create Gazebo socket (client)
+        gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        gazebo_socket.settimeout(0.1)
+        gazebo_address = ('127.0.0.1', gazebo_port)
+        
+        print(f"System {system_id}: Forwarding {sitl_port} → {gazebo_port}")
+        
+        packet_count = 0
+        start_time = time.time()
+        last_data_s2g = None
+        
+        try:
+            while running:
+                try:
+                    # Receive from SITL and forward to Gazebo
+                    data_s2g, addr_s2g = sitl_socket.recvfrom(1024)
+                    last_data_s2g = data_s2g
+                    gazebo_socket.sendto(data_s2g, gazebo_address)
+                    
+                    try:
+                        # Get response from Gazebo and send back to SITL
+                        data_g2s, addr_g2s = gazebo_socket.recvfrom(1024)
+                        sitl_socket.sendto(data_g2s, addr_s2g)
+                        
+                        # Process and store data
+                        try:
+                            # Parse SITL to Gazebo data
+                            decoded = struct.unpack('HHI16H', data_s2g)
+                            s2g_json = {
+                                "magic": decoded[0],
+                                "frame_rate_hz": decoded[1],
+                                "frame_count": decoded[2],
+                                "pwm": list(decoded[3:])
+                            }
+                            
+                            # Try to parse Gazebo to SITL data
+                            try:
+                                g2s_data_str = data_g2s.decode('utf-8')
+                                if packet_count % 50 == 0:
+                                    with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f:
+                                        f.write(g2s_data_str + '\n')
+                            except:
+                                g2s_data_str = str(data_g2s)
+                            
+                            # Combine data
+                            combined_data = {
+                                "sitl_to_gazebo": s2g_json,
+                                "gazebo_to_sitl": g2s_data_str,
+                                "timestamp": time.time(),
+                                "system_id": system_id,
+                                "sitl_port": sitl_port,
+                                "gazebo_port": gazebo_port
+                            }
+                            
+                            # Save to JSON file (once per second)
+                            current_time = time.time()
+                            elapsed_time = current_time - start_time
+                            if elapsed_time >= 1.0:
+                                with open(f'fdm_data_system_{system_id}.json', 'w') as f:
+                                    json.dump(combined_data, f, indent=2)
+                                
+                        except Exception as e:
+                            print(f"System {system_id} - JSON processing error: {e}")
+                        
+                        packet_count += 1
+                        
+                    except socket.timeout:
+                        # No response from Gazebo, continue
+                        pass
+                    
+                except socket.timeout:
+                    # No data from SITL, continue
+                    pass
+                
+                # Print stats periodically
+                current_time = time.time()
+                elapsed_time = current_time - start_time
+                
+                if elapsed_time >= 1.0:
+                    # print(f"System {system_id} - Packets: {packet_count} in the last second")
+                    if packet_count > 0 and last_data_s2g:
+                        print(f'System {system_id} - JSON Pack: {data_g2s}')
+                        fdm_parser_example(last_data_s2g)
+                    packet_count = 0
+                    start_time = current_time
+        
+        except Exception as e:
+            print(f"System {system_id} - Error in forwarder thread: {e}")
+        finally:
+            # Clean up resources
+            sitl_socket.close()
+            gazebo_socket.close()
+            print(f"System {system_id} - Forwarder thread terminated")
+    
+    # Create and start threads for each system
+    threads = []
+    for i in range(num_systems):
+        thread = threading.Thread(target=forwarder_thread, args=(i,))
+        thread.daemon = True
+        threads.append(thread)
+        thread.start()
+    
+    # Keep the main thread running
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print("\nShutting down all forwarders...")
+        running = False
+        time.sleep(1)
+        print("Done.")
+        return
+
+
+def fdm_switch_multiple_with_remote(num_systems=3, remote_ip='140.120.31.130'):
+    '''
+    Create multiple UDP forwarders between SITL and Gazebo using threading
+    Also forwards the combined data to a remote IP address
+    
+    Each forwarder uses its own set of ports following the pattern:
+    - SITL ports: 9002, 9012, 9022, ...
+    - Gazebo ports: 19002, 19012, 19022, ...
+    - Remote ports: 9003, 9013, 9023, ... (for the remote forwarding)
+    
+    Parameters:
+    num_systems (int): Number of forwarding pairs to create
+    remote_ip (str): IP address for remote forwarding
+    '''
+    # Flag to control thread execution
+    running = True
+
+    # Add shared counters for total packets
+    total_packets_sent = 0
+    total_packets_lock = threading.Lock()  # For thread safety    
+    
+    def forwarder_thread(system_id):
+        nonlocal total_packets_sent
+        sitl_port = 9002 + (system_id * 10)
+        gazebo_port = 19002 + (system_id * 10)
+        remote_port = 9003 + (system_id * 10)
+        
+        # Create SITL socket (server)
+        sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        sitl_socket.bind(('', sitl_port))
+        sitl_socket.settimeout(0.1)
+        
+        # Create Gazebo socket (client)
+        gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        gazebo_socket.settimeout(0.1)
+        gazebo_address = ('127.0.0.1', gazebo_port)
+        
+        # Create remote forwarding socket
+        remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        remote_address = (remote_ip, remote_port)
+        
+        print(f"System {system_id}: SITL:{sitl_port} → Gazebo:{gazebo_port}, Remote:{remote_ip}:{remote_port}")
+        
+        packet_count = 0
+        start_time = time.time()
+        last_data_s2g = None
+        forward_counter = 0  # 添加轉發計數器
+        system_packets_sent = 0  # Local counter for this system
+
+        try:
+            while running:
+                try:
+                    # Receive from SITL and forward to Gazebo
+                    data_s2g, addr_s2g = sitl_socket.recvfrom(1024)
+                    last_data_s2g = data_s2g
+                    gazebo_socket.sendto(data_s2g, gazebo_address)
+                    
+                    try:
+                        # Get response from Gazebo and send back to SITL
+                        data_g2s, addr_g2s = gazebo_socket.recvfrom(1024)
+                        sitl_socket.sendto(data_g2s, addr_s2g)
+                        
+                        # Process and store data
+                        try:
+                            # Parse SITL to Gazebo data
+                            decoded = struct.unpack('HHI16H', data_s2g)
+                            s2g_json = {
+                                "magic": decoded[0],
+                                "frame_rate_hz": decoded[1],
+                                "frame_count": decoded[2],
+                                "pwm": list(decoded[3:])
+                            }
+                            
+                            # Try to parse Gazebo to SITL data
+                            try:
+                                g2s_data_str = data_g2s.decode('utf-8')
+                                if packet_count % 50 == 0:
+                                    with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f:
+                                        f.write(g2s_data_str + '\n')
+                            except:
+                                g2s_data_str = str(data_g2s)
+                            
+                            # Combine data
+                            combined_data = {
+                                "sitl_to_gazebo": s2g_json,
+                                "gazebo_to_sitl": g2s_data_str,
+                                "timestamp": time.time(),
+                                "system_id": system_id,
+                                "sitl_port": sitl_port,
+                                "gazebo_port": gazebo_port
+                            }
+                            
+                            '''
+                            # 每n筆才轉發一次
+                            forward_counter += 1
+                            if forward_counter >= 18:
+                                # Forward data to remote server
+                                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+                                #print(f"System {system_id} - Send data to remote (第{packet_count}個包)")
+                                forward_counter = 0  # 重置計數器
+                            '''
+
+                            # 使用模運算代替計數器重置
+                            forward_counter += 1
+                            if packet_count % 1 == 0:
+                                # Forward data to remote server
+                                remote_socket.sendto(json.dumps(combined_data).encode(), remote_address) 
+                                system_packets_sent += 1
+                                with total_packets_lock:
+                                    total_packets_sent += 1                                
+                                # Debug用，看每秒傳了多少筆出去
+                                #print(f"System {system_id} - Send data to remote (第{packet_count}個包)")               
+                            
+                            # Save to JSON file (once per second)
+                            current_time = time.time()
+                            elapsed_time = current_time - start_time
+                            if elapsed_time >= 1.0:
+                                with open(f'fdm_data_system_{system_id}.json', 'w') as f:
+                                    json.dump(combined_data, f, indent=2)
+                                
+                        except Exception as e:
+                            print(f"System {system_id} - JSON processing error: {e}")
+                        
+                        packet_count += 1
+                        
+                    except socket.timeout:
+                        # No response from Gazebo, continue
+                        pass
+                    
+                except socket.timeout:
+                    # No data from SITL, continue
+                    pass
+                
+                # Print stats periodically
+                current_time = time.time()
+                elapsed_time = current_time - start_time
+                
+                if elapsed_time >= 1.0:
+                    print(f"System {system_id} - Packets: {packet_count} in the last second")
+                    if packet_count > 0 and last_data_s2g:
+                        fdm_parser_example(last_data_s2g)
+                    packet_count = 0
+                    start_time = current_time
+        
+        except Exception as e:
+            print(f"System {system_id} - Error in forwarder thread: {e}")
+        finally:
+            # Clean up resources
+            sitl_socket.close()
+            gazebo_socket.close()
+            remote_socket.close()
+            print(f"System {system_id} - Forwarder thread terminated")
+
+    # Create statistics display thread
+    def stats_display_thread():
+        start_time = time.time()
+        while running:
+            time.sleep(5)  # Display stats every 5 seconds
+            elapsed = time.time() - start_time
+            with total_packets_lock:
+                rate = total_packets_sent / elapsed if elapsed > 0 else 0
+                print(f"\n===== STATISTICS =====")
+                print(f"Total packets sent: {total_packets_sent}")
+                print(f"Average rate: {rate:.2f} packets/sec")
+                print(f"Running time: {elapsed:.1f} seconds")
+                print(f"======================\n")
+
+    # Create and start threads for each system
+    threads = []
+    for i in range(num_systems):
+        thread = threading.Thread(target=forwarder_thread, args=(i,))
+        thread.daemon = True
+        threads.append(thread)
+        thread.start()
+
+    # Start stats display thread
+    stats_thread = threading.Thread(target=stats_display_thread)
+    stats_thread.daemon = True
+    stats_thread.start()        
+    
+    # Keep the main thread running
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print("\nShutting down all forwarders...")
+        running = False
+        time.sleep(1)
+        print(f"Final total packets sent: {total_packets_sent}")
+        print("Done.")
+        return
+
+
+print('Start')
+#fdm_switch_example_two()
+#mavlink_analyzer_simple()
+#fdm_switch_example_one()
+#fdm_switch_example_one_with_remote_forwarding()
+#fdm_switch_multiple(1)  # Create 3 forwarding pairs
+fdm_switch_multiple_with_remote(3)  # Create 1 forwarding pair with remote forwarding
+#fdm_parser_example()