Force sync local master to remote ken

主專案整合

README.md

@@ -1,43 +1,46 @@
 這是天雷系統的專案
 
+
+
 ===
-專案核心框架
+## 功能簡介
+1. mavlink 多對多支援平台
+2. 不允許進到 ros 系統有相同 sysid
+3. 假設一台載具上所有 component 共用同一 socket
+
+===
+## 運行環境
+
+### 專案核心框架
 1. ROS2 Humble
 2. Python 3.8.10
 
+### 必要相依套件及版本
+- Python
+[Package] fc_network_adapter
+    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
-必要相依套件 順便記錄我開發時的環境版本
-
-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
-
-
-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. mavlink 多對多支援平台
-2. 不允許進到 ros 系統有相同 sysid
-3. 假設所有 component 共用同一 socket
-
-===
-開發用輔助專案
 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
@@ -49,34 +52,53 @@ 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

@@ -12,6 +12,7 @@ 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/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 連線機制 以利後期擴容其他模組
+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):
@@ -39,9 +39,9 @@ class PanelState:
         self.object_manager_state = "Stopped"
         self.serial_manager_state = "Stopped"
         self.ros2_manager_state = "Stopped"
-        self.socket_object_list = [] # 已有的 mavlink object 
+        self.socket_object_list = [] # 已有的 mavlink object
         self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string
-        self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 
+        self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息
 
         # 關於創建通道時的暫存資訊
         self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊
@@ -102,36 +102,36 @@ class ControlPanel:
     def input_dialog(stdscr, prompt="請輸入文字: "):
         """顯示輸入對話框"""
         height, width = stdscr.getmaxyx()
-        
+
         # 建立輸入視窗
         dialog_height = 5
         dialog_width = min(60, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         # 建立視窗邊框
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.border()
         dialog_win.addstr(1, 2, prompt)
         dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消")
         dialog_win.refresh()
-        
+
         # 輸入區域
         input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2)
         input_win.keypad(True)
-        
+
         curses.echo()
         curses.curs_set(1)
-        
+
         user_input = ""
-        
+
         while True:
             input_win.clear()
             input_win.addstr(0, 0, user_input[-dialog_width+8:])  # 顯示輸入內容（滾動）
             input_win.refresh()
-            
+
             ch = input_win.getch()
-            
+
             if ch == 27:  # ESC
                 user_input = None
                 break
@@ -141,16 +141,16 @@ class ControlPanel:
                 user_input = user_input[:-1]
             elif 32 <= ch <= 126:  # 可打印字符
                 user_input += chr(ch)
-        
+
         curses.noecho()
         curses.curs_set(0)
-        
+
         # 清理視窗
         del input_win
         del dialog_win
         stdscr.clear()
         stdscr.refresh()
-        
+
         return user_input
 
     # ================ 關於 主要選單 的部份 ===================
@@ -179,9 +179,9 @@ class ControlPanel:
                 ]),
                 MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
                 MenuNode("Engineer Mode", "工程模式", children=[
-                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), 
+                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
-                    MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), 
+                    MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
-                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), 
+                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
                 ]),
                 MenuNode("Shutdown", "關閉整個系統", children=[
                     MenuNode("Return", "繼續運行", "BACK"),
@@ -191,7 +191,7 @@ class ControlPanel:
 
     def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event):
         stdscr = None
-    
+
         def cleanup():
             """清理 curses 狀態"""
             if stdscr:
@@ -209,7 +209,7 @@ class ControlPanel:
         def draw_menu(screen):
             nonlocal stdscr
             stdscr = screen
-            
+
             curses.curs_set(0)
             stdscr.nodelay(False)  # 阻塞讀鍵
             stdscr.keypad(True)
@@ -219,12 +219,12 @@ class ControlPanel:
             idx_stack = [0]  # 索引堆疊
 
             state.intoSTART() # 設定狀態為運行中
-            
+
             while not stop_evt.is_set():
 
                 current_menu = menu_stack[-1]
                 current_idx = idx_stack[-1]
-                
+
                 # 獲取終端機尺寸
                 height, width = stdscr.getmaxyx()
                 # 簡單暴力的限制視窗的大小
@@ -237,7 +237,7 @@ class ControlPanel:
                 if height < MIN_HEIGHT or width < 60:
                     logger.error("Terminal size too small for Control Panel.")
                     break
-                
+
                 stdscr.clear()
                 stdscr.border()
 
@@ -267,26 +267,26 @@ class ControlPanel:
                     elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP":
                         link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No"
                         desc = f"{child.desc} [{link_status}]"
-                    
+
                     line = f"{marker}{child.name:15s} – {desc}"
                     attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL
                     stdscr.addstr(start_line + i, 4, line, attr)
 
-                # 顯示訊息區域 
+                # 顯示訊息區域
                 # info_start_line = start_line + len(current_menu.children) + 1
                 info_start_line = height - 8
                 max_msg_lines = 5  # 最多顯示 5 行訊息
                 current_time = time.time()
-                
+
                     # 清理過時的訊息
                 state.panel_info_msg_list = [
                     (msg, timestamp)  for msg, timestamp in state.panel_info_msg_list
                     if current_time - timestamp < 2.0 #秒數
                 ]
-                
+
                     # 只顯示最新的 max_msg_lines 條訊息
                 display_msgs = state.panel_info_msg_list[-max_msg_lines:]
-                
+
                 for i, msg_data in enumerate(display_msgs):
                     if info_start_line + i >= help_line - 1:  # 避免超出邊界
                         break
@@ -298,13 +298,13 @@ class ControlPanel:
 
                     stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD)
 
-                
+
-                
+
                 # 操作說明
                 # 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()
 
@@ -319,8 +319,8 @@ class ControlPanel:
                         # continue
                         break
                     time.sleep(0.1)
-                    if (state.mavlink_bridge_state == "Stopped" and 
+                    if (state.mavlink_bridge_state == "Stopped" and
-                        state.object_manager_state == "Stopped" and 
+                        state.object_manager_state == "Stopped" and
                         state.serial_manager_state == "Stopped"):
                         state.intoSTOPPED()
                         # stop_evt.set()
@@ -330,14 +330,14 @@ class ControlPanel:
                 # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt
                 stdscr.timeout(100)
                 ch = stdscr.getch()
-                
+
                 if ch == -1: # 沒有操作
                     continue
-                    
+
                 # 處理按鍵
                 if ch in (curses.KEY_UP, ord('k')):
                     idx_stack[-1] = (current_idx - 1) % len(current_menu.children)
-                    
+
                 elif ch in (curses.KEY_DOWN, ord('j')):
                     idx_stack[-1] = (current_idx + 1) % len(current_menu.children)
 
@@ -347,14 +347,14 @@ class ControlPanel:
 
                 elif ch == (ord('o')):
                     # 離開工程模式
-                    state.intoSTART()                
+                    state.intoSTART()
-                    
+
                 elif ch == curses.KEY_LEFT:
                     # 返回上層
                     if len(menu_stack) > 1:
                         menu_stack.pop()
                         idx_stack.pop()
-                
+
                 elif ch == curses.KEY_RIGHT:
                     # 進入下層 (但不執行動作)
                     selected = current_menu.children[current_idx]
@@ -369,26 +369,26 @@ class ControlPanel:
 
                 elif ch in (curses.KEY_ENTER, 10, 13):
                     selected = current_menu.children[current_idx]
-                    
+
                     # 處理不同類型的動作
                     if selected.children:  # 有子選單
                         menu_stack.append(selected)
                         idx_stack.append(0)
-                        
+
                     elif selected.action == "BACK":
                         if len(menu_stack) > 1:
                             menu_stack.pop()
                             idx_stack.pop()
-                            
+
                     elif selected.action == "QUIT":
                         state.intoTERMINATION()
                         pre_panel_shutdown()
-                    
+
                     elif selected.action == "TEXT_UDP_IP":
                         result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
                         if result is not None:
                             state.udp_info_temp["IP"] = result
-                    
+
                     elif selected.action == "TEXT_UDP_PORT":
                         result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ")
                         if result is not None:
@@ -402,7 +402,7 @@ class ControlPanel:
                             idx_stack.pop()
                             # menu_stack.pop()
                             # idx_stack.pop()
-                    
+
                     elif selected.action == "CREATE_UDP_OUTBOUND":
                         cmd_q.put("CREATE_UDP_OUTBOUND")
                         # 確認後回到上兩層
@@ -457,8 +457,8 @@ class ControlPanel:
                     elif selected.action == "LIST_SERIAL_LINKS":
                         created_list_menu = self.create_linked_serial_menu(state, page=0)
                         menu_stack.append(created_list_menu)
-                        idx_stack.append(0)                        
+                        idx_stack.append(0)
-                    
+
                     elif selected.action == "INSPECT_LINKED_SERIAL":
                         # 顯示 Serial 連結詳細資訊
                         if hasattr(selected, 'serial_id'):
@@ -488,7 +488,7 @@ class ControlPanel:
                             current_list_menu = menu_stack[-1]
                             menu_stack.pop()
                             idx_stack.pop()
-                            
+
                             # 依據選單種類 重新建立分頁
                             if current_list_menu.name == "Serial Port List":
                                 created_list_menu = self.create_serial_port_menu(state, page=selected.page)
@@ -503,16 +503,16 @@ class ControlPanel:
                                 menu_stack.append(current_list_menu)
                                 idx_stack.append(0)
                                 continue
-                            
+
                             menu_stack.append(created_list_menu)
                             idx_stack.append(0)
-                    
+
                     elif selected.action == "INSPECT_MAV_OBJECT":
                         # 顯示物件詳細資訊
                         if hasattr(selected, 'socket_id'):
                             cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id))
                             self.show_object_info(stdscr, selected.socket_id, state)
-                    
+
                     elif selected.action == "REMOVE_MAV_OBJECT":
                         # 移除物件
                         if hasattr(selected, 'socket_id'):
@@ -575,7 +575,7 @@ class ControlPanel:
                         created_list_menu = self.create_vehicles_list_menu(state, page=0)
                         menu_stack.append(created_list_menu)
                         idx_stack.append(0)
-                    
+
                     elif selected.action == "INSPECT_VEHICLE":
                         # 顯示載具詳細資訊
                         if hasattr(selected, 'sysid') and hasattr(selected, 'compid'):
@@ -585,7 +585,7 @@ class ControlPanel:
                     elif callable(selected.action):
                         # 執行函式
                         cmd_q.put(selected.action)
-                        
+
         try:
             curses.wrapper(draw_menu)
         except KeyboardInterrupt:
@@ -593,12 +593,12 @@ class ControlPanel:
         finally:
             cleanup()
 
-    # ================ 關於 mavlink object 的部份 =================== 
+    # ================ 關於 mavlink object 的部份 ===================
 
     def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8):
         """動態創建 mavlink_object 列表選單（支持分頁）"""
         children = []
-        
+
         if not state.socket_object_list:
             children.append(MenuNode("(Empty)", "目前沒有連結口", None))
         else:
@@ -606,7 +606,7 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # 顯示當前頁的物件
             for socket_id in state.socket_object_list[start_idx:end_idx]:
                 # 為每個 socket 創建子選單
@@ -656,11 +656,11 @@ class ControlPanel:
         dialog_width = min(70, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.border()
         dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD)
-        
+
         # 這裡顯示基本資訊
         dialog_win.addstr(2, 2, f"Socket ID     : {socket_id}")
         dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}")
@@ -676,15 +676,15 @@ class ControlPanel:
         dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}")
 
         state.socket_info_single['InfoReady'] = False  # 重置狀態以便下次使用
-        
+
         dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
         dialog_win.refresh()
-        
+
         dialog_win.getch()
         del dialog_win
         stdscr.clear()
         stdscr.refresh()
-    
+
     def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False):
         """選擇目標 socket 的對話框"""
         height, width = stdscr.getmaxyx()
@@ -692,13 +692,13 @@ class ControlPanel:
         dialog_width = min(50, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.keypad(True)
-        
+
         title = "選擇要移除的目標" if remove_mode else "選擇轉發目標"
         available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id]
-        
+
         if not available_sockets:
             dialog_win.border()
             dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
@@ -710,24 +710,24 @@ class ControlPanel:
             stdscr.clear()
             stdscr.refresh()
             return None
-        
+
         selected_idx = 0
-        
+
         while True:
             dialog_win.clear()
             dialog_win.border()
             dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
-            
+
             for i, socket_id in enumerate(available_sockets):
                 marker = "➤" if i == selected_idx else " "
                 attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL
                 dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr)
-            
+
             dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消")
             dialog_win.refresh()
-            
+
             ch = dialog_win.getch()
-            
+
             if ch in (curses.KEY_UP, ord('k')):
                 selected_idx = (selected_idx - 1) % len(available_sockets)
             elif ch in (curses.KEY_DOWN, ord('j')):
@@ -749,11 +749,11 @@ class ControlPanel:
     def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8):
         """動態創建 serial port 列表選單（支持分頁）"""
         children = []
-        
+
         # 獲取可用的 Serial 連接埠列表
         # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓
         serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
-        
+
         if not serial_ports:
             children.append(MenuNode("(Empty)", "目前沒有串口設備", None))
         else:
@@ -761,7 +761,7 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # 顯示當前頁的串口
             for port in serial_ports[start_idx:end_idx]:
                 port_menu = MenuNode(f"{port}", children=[
@@ -781,7 +781,7 @@ class ControlPanel:
                 for child in port_menu.children:
                     child.port = port
                 children.append(port_menu)
-            
+
             # 添加分頁控制
             if total_pages > 1:
                 children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
@@ -793,7 +793,7 @@ class ControlPanel:
                     next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                     next_node.page = page + 1
                     children.append(next_node)
-        
+
         children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
         menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children)
         menu.current_page = page
@@ -802,7 +802,7 @@ class ControlPanel:
     def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8):
         """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能"""
         children = []
-        
+
         if not state.linked_serial_dict:
             children.append(MenuNode("(Empty)", "目前沒有連結口", None))
         else:
@@ -810,7 +810,7 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # 顯示當前頁的物件
             linked_serial_id_list = list(state.linked_serial_dict.keys())
             for serial_id in linked_serial_id_list[start_idx:end_idx]:
@@ -825,7 +825,7 @@ class ControlPanel:
                 for child in obj_menu.children:
                     child.serial_id = serial_id
                 children.append(obj_menu)
-            
+
             # 添加分頁控制
             if total_pages > 1:
                 children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
@@ -837,7 +837,7 @@ class ControlPanel:
                     next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                     next_node.page = page + 1
                     children.append(next_node)
-        
+
         children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
         menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children)
         menu.current_page = page
@@ -859,14 +859,14 @@ class ControlPanel:
         dialog_width = min(70, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.border()
         dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD)
 
         # 從 linked_serial_dict 獲取資訊
         serial_info = state.linked_serial_dict.get(serial_id, {})
-        
+
         if not serial_info:
             dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊")
         else:
@@ -877,17 +877,17 @@ class ControlPanel:
             dialog_win.addstr(5, 2, f"Communication   : {state.serial_info_single.get('receiver_type', 'N/A')}")
             dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}")
             dialog_win.addstr(7, 2, f"Status          : {state.serial_info_single.get('status', 'Running')}")
-            
+
             # 如果有額外資訊可以繼續添加
             if 'thread_alive' in serial_info:
                 thread_status = "Alive" if serial_info['thread_alive'] else "Stopped"
                 dialog_win.addstr(8, 2, f"Thread Status   : {thread_status}")
-        
+
         state.serial_info_single['InfoReady'] = False  # 重置狀態以便下次使用
 
         dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
         dialog_win.refresh()
-        
+
         dialog_win.getch()
         del dialog_win
         stdscr.clear()
@@ -900,7 +900,7 @@ class ControlPanel:
         每個 vehicle-component 組合都是獨立的選單項目
         """
         children = []
-        
+
         if not state.connected_vehicles_dict:
             children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None))
         else:
@@ -908,22 +908,22 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # vehicle_id_list 現在是 (sysid, compid) 的元組列表
             vehicle_comp_list = list(state.connected_vehicles_dict.keys())
-            
+
             # 顯示當前頁的物件
             for sysid, compid in vehicle_comp_list[start_idx:end_idx]:
                 # 建立顯示名稱
                 display_name = f"Vehicle #{sysid} - Comp #{compid}"
                 desc = f"載具 {sysid} 組件 {compid}"
-                
+
                 vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE")
                 # 將 sysid 和 compid 附加到選單項目上
                 vehicle_menu.sysid = sysid
                 vehicle_menu.compid = compid
                 children.append(vehicle_menu)
-            
+
             # 添加分頁控制
             if total_pages > 1:
                 children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
@@ -935,7 +935,7 @@ class ControlPanel:
                     next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                     next_node.page = page + 1
                     children.append(next_node)
-        
+
         children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
         menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children)
         menu.current_page = page
@@ -943,7 +943,7 @@ class ControlPanel:
 
     def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState):
         """顯示載具組件詳細資訊（動態更新，顯示變化率）"""
-        
+
         # 等待資訊準備
         start = time.time()
         while not state.vehicle_info_single.get('InfoReady', False):
@@ -951,30 +951,30 @@ class ControlPanel:
                 state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time()))
                 return
             time.sleep(0.05)
-        
+
         info = state.vehicle_info_single
         height, width = stdscr.getmaxyx()
         dialog_height = min(22, height - 4)
         dialog_width = min(70, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.nodelay(True)  # 非阻塞模式，允許動態更新
         dialog_win.keypad(True)
-        
+
         # MAV_TYPE 名稱對應
         MAV_TYPE_NAMES = {
             0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter",
             4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover",
             12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera"
         }
-        
+
         # 動態更新迴圈
         last_update = time.time()
         while True:
             current_time = time.time()
-            
+
             # 每 1 秒重新請求資料
             if current_time - last_update >= 1.0:
                 # 觸發資料更新（透過 Orchestrator）
@@ -989,58 +989,58 @@ class ControlPanel:
                 # 更新 info 參照
                 info = state.vehicle_info_single
                 last_update = current_time
-            
+
             dialog_win.clear()
             dialog_win.border()
             dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD)
-            
+
             # === 基礎資訊 ===
             dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE)
             dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE)
-            
+
             # # MAV Type # 這個用不到
             # mav_type = info.get('vehicle_type', 'N/A')
             # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type)
             # dialog_win.addstr(3, 2, f"MAV Type       : {mav_type_str}")
-            
+
             # Component Type
             dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}")
-            
+
             # Autopilot Type
             if info.get('mav_autopilot') is not None:
                 dialog_win.addstr(4, 2, f"Autopilot      : {info.get('mav_autopilot', 'N/A')}")
-            
+
             # Connection Info
             dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}")
             dialog_win.addstr(4, 32, f"Socket ID  : #{info.get('socket_id', 'N/A')}")
-            
+
             # === 封包統計 ===
             stats = info.get('packet_stats', {})
             dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE)
-            
+
             received = stats.get('received', 0)
             lost = stats.get('lost', 0)
             loss_rate = stats.get('loss_rate', 0.0)
             last_seq = stats.get('last_seq', 'N/A')
-            
+
             # 計算變化
             received_delta = stats.get('received_delta', 0)
             lost_delta = stats.get('lost_delta', 0)
-            
+
             # 顯示變化率
             recv_str = f"{received:6d}"
             if received_delta > 0:
                 recv_str += f" (+{received_delta}↑)"
-            
+
             lost_str = f"{lost:4d}"
             if lost_delta > 0:
                 lost_str += f" (+{lost_delta}↑)"
-            
+
             dialog_win.addstr(8, 2, f"Received  : {recv_str}")
             dialog_win.addstr(8, 32, f"Lost       : {lost_str}")
             dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%")
             dialog_win.addstr(9, 32, f"Last Seq   : {last_seq}")
-            
+
             # 最後接收時間
             last_msg_time = stats.get('last_msg_time')
             if last_msg_time:
@@ -1050,50 +1050,50 @@ class ControlPanel:
                 dialog_win.addstr(10, 32, f"Elapsed    : {elapsed:.1f}s")
             else:
                 dialog_win.addstr(10, 2, "Last Time : N/A")
-            
+
             # === 訊息類型分佈 ===
             dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE)
-            
+
             msg_counts = info.get('msg_type_counts', {})
-            
+
             # MAVLink 訊息名稱對應（縮寫版本）
             MSG_NAMES = {
                 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU",
                 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
                 74: "VFR_HUD", 147: "BATT_ST"
             }
-            
+
             # 顯示前 12 個最常見的訊息類型（兩列各 6 個）
             msg_items = list(msg_counts.items())[:12]
             line = 13
             for i, (msg_id, count) in enumerate(msg_items):
                 msg_name = MSG_NAMES.get(msg_id, "???")
                 delta = stats.get(f'msg_delta_{msg_id}', 0)
-                
+
                 # 格式化數據
                 if delta > 0:
                     data_str = f"{count}(+{delta}↑)"
                 else:
                     data_str = f"{count}"
-                
+
                 # 格式化顯示：[ID]NAME  DATA (ID固定3字符寬度，右對齊)
                 display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}"
-                
+
                 # 左列(偶數索引)或右列(奇數索引)
                 col = 2 if i % 2 == 0 else 36
                 row = line + (i // 2)
-                
+
                 if row >= dialog_height - 3:  # 避免超出邊界
                     break
-                    
+
                 dialog_win.addstr(row, col, display_str)
-            
+
             # 確保跳過顯示區域
             line = line + 6
-            
+
             dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...")
             dialog_win.refresh()
-            
+
             # 檢查是否有按鍵（非阻塞）
             ch = dialog_win.getch()
             if ch in (ord('R'), ord('r')):
@@ -1103,10 +1103,10 @@ class ControlPanel:
                 break
             elif ch != -1:  # 有按鍵則退出
                 break
-            
+
             # 短暫延遲
             time.sleep(0.1)
-        
+
         state.vehicle_info_single['InfoReady'] = False
         del dialog_win
         stdscr.clear()
@@ -1140,19 +1140,20 @@ class Orchestrator:
         self.fc_ros_manager = mros.ros2_manager
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
-            'position': 1.0,   
+            'position': 1.0,
-            'attitude': 0.0,    
+            'position_ned': 1.0,
-            'velocity': 0.0,     
+            'attitude': 1.0,
-            'battery': 1.0,   
+            'velocity': 0.0,
-            'vfr_hud': 1.0,    
+            'battery': 1.0,
-            'mode': 0.0,       
+            'vfr_hud': 1.0,
-            'summary': 1.0,    
+            'mode': 0.0,
+            'summary': 1.0,
         }
 
     def engageWholeSystem(self):
         """啟動整個系統"""
         # === 1) 面板部分的啟動 ===
-        self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt)) 
+        self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt))
         self.panel_thread.start()
 
         # === 2) async_io_manager & mavlink_bridge 部分的啟動 ===
@@ -1215,7 +1216,7 @@ class Orchestrator:
                             for s_id in mo.mavlink_object.mavlinkObjects:
                                 s_obj = mo.mavlink_object.mavlinkObjects[s_id]
                                 if socket_id in s_obj.target_sockets:
-                                    self.remove_target_from_object(s_id, socket_id)   
+                                    self.remove_target_from_object(s_id, socket_id)
                             # 再移除該物件
                             self.delete_udp_object(socket_id)
                         elif action == "MAVOBJ_ADD_TARGET":
@@ -1262,7 +1263,7 @@ class Orchestrator:
                         elif action == "UNREGISTER_VEHICLE":
                             sysid = cmd[1]
                             self.vehicle_registry.unregister(sysid)
-                            
+
                     elif cmd == "CREATE_UDP_INBOUND":
                         self.panelState.udp_info_temp["direction"] = "inbound"
                         self.create_udp_object()
@@ -1291,8 +1292,8 @@ class Orchestrator:
             logger.error(f"Unexpected error in main loop: {e}")
         finally:
 
-            # 驗證並確保所有模組都被下達關閉訊號 
+            # 驗證並確保所有模組都被下達關閉訊號
-            # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發 
+            # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發
             if self.bridge.thread.is_alive():
                 if self.bridge.running:
                     self.bridge.stop()
@@ -1318,7 +1319,7 @@ class Orchestrator:
                 self.panel_thread.join(timeout=2)
 
             time.sleep(0.5) # 等待各模組穩定關閉
-                
+
             logger.info("Main orchestrator END!")
 
     # =============== 面板動作 - Mavlink Object ===============
@@ -1339,10 +1340,11 @@ 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']}"
-        
+
         try:
             # 檢測這個 connection_string 是否能成功建立 mavlink 連結
             mavlink_socket = mavutil.mavlink_connection(connection_string)
@@ -1362,7 +1364,7 @@ class Orchestrator:
             mavlink_object.socket_type = socket_type
 
         self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time()))
-    
+
     def delete_udp_object(self, socket_id):
         """移除指定的 mavlink_object"""
 
@@ -1406,10 +1408,10 @@ class Orchestrator:
     def _update_vehicles_list(self):
         """更新已連線載具列表（從 vehicle_registry 獲取）"""
         vehicles_dict = {}
-        
+
         # 從 vehicle_registry 獲取所有載具
         all_vehicles = self.vehicle_registry.get_all()
-        
+
         for sysid, vehicle in all_vehicles.items():
             # 遍歷每個載具的所有組件
             for compid, component in vehicle.components.items():
@@ -1422,9 +1424,9 @@ class Orchestrator:
                     'connection_via': vehicle.connected_via.value,
                     'socket_id': vehicle.custom_meta.get('socket_id', 'N/A')
                 }
-        
+
         self.panelState.connected_vehicles_dict = vehicles_dict
-    
+
     def _prepare_vehicle_info(self, sysid, compid):
         """準備載具組件的詳細資訊（包含變化率計算）"""
         vehicle = self.vehicle_registry.get(sysid)
@@ -1433,37 +1435,37 @@ class Orchestrator:
             return
 
         socket_id = vehicle.custom_meta.get('socket_id', 'N/A')
-        
+
         component = vehicle.get_component(compid)
         if not component:
             logger.warning(f"Component {compid} not found in vehicle {sysid}")
             return
-        
+
         stats = component.packet_stats
-        
+
         # 取得之前的統計資料（用於計算變化）
         prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {})
         prev_received = prev_stats.get('received', 0)
         prev_lost = prev_stats.get('lost', 0)
         prev_msg_counts = prev_stats.get('msg_counts', {})
-        
+
         # 計算變化率
         received_delta = stats.received_count - prev_received
         lost_delta = stats.lost_count - prev_lost
-        
+
         # 準備訊息類型計數（排序後取前幾個）
         sorted_msg_counts = dict(sorted(
             stats.msg_type_count.items(),
             key=lambda x: x[1],
             reverse=True
         )[:12])  # 取前 12 個最常見的
-        
+
         # 計算每種訊息類型的變化
         msg_deltas = {}
         for msg_id, count in sorted_msg_counts.items():
             prev_count = prev_msg_counts.get(msg_id, 0)
             msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count
-        
+
         # 更新 vehicle_info_single
         socket_type = "N/A"
         socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None)
@@ -1481,7 +1483,7 @@ class Orchestrator:
             "packet_stats": {
                 "received": stats.received_count,
                 "lost": stats.lost_count,
-                "loss_rate": (stats.lost_count / stats.received_count * 100 
+                "loss_rate": (stats.lost_count / stats.received_count * 100
                              if stats.received_count > 0 else 0),
                 "last_seq": stats.last_seq,
                 "last_msg_time": stats.last_msg_time,
@@ -1508,8 +1510,8 @@ class Orchestrator:
                 (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time())
             )
             return
-        
+
-        # 獲取可用的 udp port 
+        # 獲取可用的 udp port
         udp_port_tmp = find_available_port(19000, 20000)
 
         # 定義通訊類型映射表
@@ -1526,7 +1528,7 @@ class Orchestrator:
                 ("Please select Communication Type first.", time.time())
             )
             return
-        
+
         # 查找對應的通訊類型
         comm_type_tmp = COMM_TYPE_MAP.get(comm_type)
         if comm_type_tmp is None:
@@ -1554,12 +1556,31 @@ class Orchestrator:
             self.panelState.udp_info_temp['Port'] = str(udp_port_tmp)
             self.panelState.udp_info_temp['direction'] = "inbound"
             self.create_udp_object("SERIAL_XBEE")
-        
+
 
 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()
@@ -1593,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
@@ -61,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)
@@ -386,7 +385,7 @@ class mavlink_object:
             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([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

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
+    從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
+    以最基礎的 mavlink microservice 會實現目標 
+    並不會包含額外的功能
+
+與一個節點管理器
+- fc_ros_manager
 
-從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 """
 
 import os
@@ -22,10 +27,12 @@ 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
@@ -33,12 +40,13 @@ 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:
     """發布頻率控制器 - 按時間間隔控制發布頻率"""
     
@@ -49,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}
@@ -97,11 +108,6 @@ class PublishRateController:
         """重置所有計時器"""
         self.last_publish_time.clear()
 
-
-# ============================================================================
-#                       VehicleStatusPublisher Node
-# ============================================================================
-
 class VehicleStatusPublisher(Node):
     """
     載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
@@ -138,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):
@@ -167,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)
@@ -222,6 +227,42 @@ class VehicleStatusPublisher(Node):
             msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
         
         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）"""
@@ -231,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:
@@ -471,12 +511,17 @@ class MavlinkCommandService(Node):
     - 作為 service server 等待 client 請求
     - 接收請求，組裝 MAVLink 封包
     - 調用 mavlinkObject 發送封包
-    - 處理 ACK 等待和超時（未來實現）
+    - 處理 ACK 等待和超時
     
     設計理念：回歸 MAVLink 純粹結構
     - 只負責將 ROS2 請求轉換為 MAVLink 封包
     - 不預設功能（如 ARM/DISARM) 保持模組化
     - 高層應用可透過此 service 實現各種功能
+
+    講白話一點就是
+    每次接到一個 service 請求 要整個系統丟某種指令給載具時
+    會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+
     """
 
     serviceString_prefix = '/fc_network/vehicle'
@@ -487,8 +532,8 @@ class MavlinkCommandService(Node):
         # 狀態標記
         self.running = True
         
-        # mavlinkObject 的引用（將由外部設置）
+        # # mavlinkObject 的引用（將由外部設置） 用不到
-        self.mavlink_analyzer = None
+        # self.mavlink_analyzer = None
 
         # pending 旗標物件的儲存庫
         self._pending_by_sysid = {} # sysid(int) : PendingEntry
@@ -521,6 +566,15 @@ class MavlinkCommandService(Node):
             self.handle_command_long
         )
 
+        # ═══════════════════════════════════════════════════════════════════
+        # Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_pos_global_int = self.create_service(
+            fcsrv.MavPositionTargetGlobalInt,
+            self.serviceString_prefix + '/pos_global_int',
+            self.handle_set_position_target_global_int
+        )
+
         logger.info("MavlinkCommandService initialized")
 
     def _index(self, target_sysid, target_compid):
@@ -542,7 +596,8 @@ class MavlinkCommandService(Node):
 
     def return_router(self):
         '''
-        這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包
+        在節點管理器哪邊被呼叫
+        消耗 return_packet_ring 裡接收到的 mavlink 封包
         分送到各自的 pending 中 
         藉由 event.set() 解開 service 中的 block 
         '''
@@ -568,6 +623,7 @@ class MavlinkCommandService(Node):
     def handle_mav_timesync_ping(self, request, response):
         '''
         用 timesync 封包驗證來回的時間
+        隸屬於 MavLink PING Protocol
         '''
         # 設定失效回應
         response.success = False
@@ -612,7 +668,7 @@ class MavlinkCommandService(Node):
         fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - TIMESYNC"
+            response.message = "mavlink timeout - TIMESYNC"
             fail_skip = True
 
         # 6) 處理回應封包
@@ -633,35 +689,31 @@ class MavlinkCommandService(Node):
 
         return response
 
-    def handle_add_two_ints(self, request, response):
-        """測試用 Service Handler 未來刪除"""
-        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
-
     # ═══════════════════════════════════════════════════════════════════════
-    # Service Handler: 發送 COMMAND_LONG
+    # Service Handler: 發送 COMMAND_LONG (76)
     # ═══════════════════════════════════════════════════════════════════════
     def handle_command_long(self, request, response):
+        '''
+        用 command long 丟出  MAV_CMD 
+        隸屬於 MavLink Command Protocol
+        '''
         # 設定失效回應
         response.success = False
         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) 接收封包系統 的設定
@@ -691,7 +743,8 @@ class MavlinkCommandService(Node):
         fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - CommLONG"
+            response.message = "mavlink timeout - CommLONG"
+            response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
             fail_skip = True
 
         # 6) 處理回應封包
@@ -711,7 +764,96 @@ class MavlinkCommandService(Node):
 
         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)
+
+        return response
+
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 參數請求
     # ═══════════════════════════════════════════════════════════════════════
@@ -790,7 +932,21 @@ class MavlinkCommandService(Node):
         response.success = True
         response.message = "Param request sent (placeholder implementation)"
         return response
-    
+
+
+    def handle_add_two_ints(self, request, response):
+        """測試用 Service Handler 未來刪除"""
+        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):
         """停止服務"""
         self.running = False
@@ -837,7 +993,8 @@ class fc_ros_manager:
         
         try:
             # 初始化 ROS2
-            rclpy.init()
+            if not rclpy.ok():
+                rclpy.init(args=None)
             
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
@@ -973,7 +1130,15 @@ ros2_manager = fc_ros_manager()
 5. 預留 MavlinkCommandService 結構（稍後實現）
 
 2026.03.27
-1. 完成 ros2 service 結構與 timesync command_long 協議
+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
@@ -425,10 +433,22 @@ class VehicleViewRegistry:
     def get_all(self) -> Dict[int, VehicleView]:
         """取得所有載具視圖"""
         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

@@ -26,6 +26,7 @@ from .utils import setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "0.60"
 
 # ====================== 分割線 =====================
 

src/fc_network_apps/__init__.py

@@ -1,3 +1,10 @@
-from .changeMode import ChangeModeClient, ChangeModeResult, change_mode
+from .longCommand import CommandLongClient, CommandLongResult
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
 
-__all__ = ["ChangeModeClient", "ChangeModeResult", "change_mode"]
+
+__all__ = [
+    "CommandLongClient",
+    "PositionTargetGlobalIntClient",
+    "CommandLongResult",
+    "PositionTargetGlobalIntResult",
+]

src/fc_network_apps/changeMode.py

@@ -1,127 +0,0 @@
-"""Simple wrapper for mode change via fc_network ROS2 service.
-
-Reference CLI command:
-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}"
-"""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_DO_SET_MODE = 176
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class ChangeModeResult:
-    """Return value for mode change requests."""
-
-    success: bool
-    message: str
-    ack_result: int
-
-
-class ChangeModeClient(Node):
-    """Small ROS2 client dedicated to change flight mode."""
-
-    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
-        super().__init__("fc_change_mode_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 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,
-    ) -> ChangeModeResult:
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_DO_SET_MODE
-        req.confirmation = confirmation
-        req.param1 = float(base_mode)
-        req.param2 = float(custom_mode)
-        req.param3 = 0.0
-        req.param4 = 0.0
-        req.param5 = 0.0
-        req.param6 = 0.0
-        req.param7 = 0.0
-        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 ChangeModeResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return ChangeModeResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-
-
-def change_mode(
-    *,
-    target_sysid: int,
-    custom_mode: float,
-    target_compid: int = 0,
-    base_mode: float = 1.0,
-    confirmation: int = 0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> ChangeModeResult:
-    """One-shot helper for collaborators who want minimal code."""
-    rclpy.init(args=None)
-    node: Optional[ChangeModeClient] = None
-    try:
-        node = ChangeModeClient(service_name=service_name)
-        if not node.wait_for_service(timeout_sec=timeout_sec):
-            return ChangeModeResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-        return node.change_mode(
-            target_sysid=target_sysid,
-            target_compid=target_compid,
-            custom_mode=custom_mode,
-            base_mode=base_mode,
-            confirmation=confirmation,
-            timeout_sec=timeout_sec,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    # Example values are aligned with your terminal command.
-    result = change_mode(target_sysid=3, custom_mode=4)
-    print(
-        f"change_mode success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

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_change_mode.py

@@ -1,29 +0,0 @@
-"""Usage example for collaborators.
-
-Run after sourcing your ROS2 workspace:
-    source install/local_setup.bash
-    python src/fc_network_apps/example_change_mode.py
-"""
-
-from fc_network_apps import change_mode
-
-
-def main() -> None:
-    # Equivalent to:
-    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
-    result = change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=4.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

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, 2, 4]
+        freqs = [0, 1, 4]
+        params = {
+            "SR1_ADSB": freqs[0],
+            "SR1_EXT_STAT": freqs[1],
+            "SR1_EXTRA1": freqs[2],
+            "SR1_EXTRA2": freqs[1],
+            "SR1_EXTRA3": freqs[1],
+            "SR1_POSITION": freqs[2],
+            "SR1_RAW_SENS": freqs[1],
+            "SR1_RC_CHAN": freqs[1]
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
+        
+    '''    
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        params = {
+            "SERIAL1_BAUD": 38
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
+           
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        freqs = [0, 3, 3]
         params = {
             "SR1_ADSB": freqs[0],
             "SR1_EXT_STAT": freqs[1],
@@ -285,26 +313,15 @@ class MAVLinkWorker(QThread):
         for param, value in params.items():
             self.set_param(f"UAV{sysid}", param, value)
     '''
-    def set_sr_params(self, sysid):
+    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

@@ -24,23 +24,34 @@ 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}"
 
-MAV_CMD_DO_SET_MODE (176)
+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
+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()