temp

- 修正 POLL-DONE 程序的 timeout 判定方式
- 修正為 py3.8 語法
- 不在判定要求量跟實收量一致性

.gitignore

@@ -26,3 +26,14 @@ Makefile
 **/*.pyc
 **/*.pyo
 **/.cursor/
+CLAUDE.md
+
+# 本地筆記與硬體 dump
+my_env_notes.md
+notes/
+**/eeprom.bin
+
+# MAVProxy / SITL 執行產生的 log 與參數 dump
+*.tlog
+*.tlog.raw
+mav.parm

README.md

@@ -1,43 +1,50 @@
 這是天雷系統的專案
 
 ===
-專案核心框架
+## 功能簡介
+1. mavlink 多對多串流與交換
+2. 支援 xbee 與 telemetry
+3. 支援 udp 管理
+4. ROS2 介面監控載具
+
+===
+使用限制
+1. 不允許進到 ros 系統有相同 sysid
+2. 假設一台載具上所有 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
 
-===
-必要相依套件 順便記錄我開發時的環境版本
-
-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
+- 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
@@ -50,37 +57,53 @@ colcon build --packages-select mavros_msgs # 這個依賴前面的
 colcon build --packages-select fc_interfaces # 自己定義的
 
 ```
-
-
-=== 
-Package 簡述
-1. unitdev 為各自協作者做開發時的測試區
-    01 -> 晉凱(ken)
-    02 -> 其宇(chiyu)
-    03 -> 文鈞
-    04 -> 倫渝
-2. fc_network_adapter 
-    建立、維護與飛控韌體的連接 
-    構築 mavlink 封包
-    處理無線模組的通訊格式 (XBee)
-    --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-3. fc_interfaces
-    自定義的介面檔
-4. fc_network_apps
-    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
-
-N. logs 是執行時期的記錄檔
-
-===
-主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式
-
-例如 在 ~/AirTrapMine/src/ 資料夾下 
+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
 
+python fc_network_apps/vehicleDiagShow.py
 ```
 
+2. 
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
+
+===
+## 資料夾說明
+1. fc_network_adapter (核心)
+    建立、維護與飛控韌體的連接 
+    構築 mavlink 封包
+    處理無線模組的通訊格式 (XBee)
+2. fc_interfaces (必要)
+    自定義的 ROS2 介面檔 沒有這個核心會運作不了
+3. fc_network_module (重要)
+    非核心 但是支援載具的重要附屬功能 
+    需要該功能時 作為一個 ros2 節點打開
+    例如 : ntrip rtk 訊號轉接
+4. fc_network_apps
+    是 fc_network_adapter 高階包裝API
+    利於開發GUI或其他應用 使用者的外層包裝
+    可以不使用 或者當作一個範例程式來看
+5. someotherpkg
+    如何使用 fc_network_apps 的範例檔案
+6. GUI 
+    由 PyQt6 開發的互動式介面
+7. unitdev 為各自協作者做開發時的測試區
+    01 -> 晉凱(ken)
+    02 -> 其宇(chiyu)
+    03 -> 文鈞
+    04 -> 倫渝
+8. logs 是執行時期的記錄檔
+
+===

src/GUI/comm_panel.py

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

src/GUI/command_sender.py

@@ -3,22 +3,33 @@
 指令發送模組
 負責將目標位置轉成具體的通訊指令發送到飛控
 
-現階段: MavlinkSender (直接 pymavlink 發送)
-未來:   替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
+目前使用 Ros2CommandSender，經 fc_network_adapter 的
+/fc_network/vehicle/pos_global_int service 發送。
+
+MavlinkSender 保留作為直連 SITL 的 fallback 工具，但已不是預設路徑。
 """
+import asyncio
+import traceback
 from abc import ABC, abstractmethod
+
+from PyQt6.QtCore import QObject, pyqtSignal
 from pymavlink import mavutil
 
 
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
 class CommandSender(ABC):
     """指令發送抽象介面"""
 
     @abstractmethod
-    def send_position_global(self, sysid, lat, lon, alt):
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
         """
         發送全球座標位置指令
 
         Args:
+            drone_id: GUI 用的 drone 識別字串 (如 's0_11')
             sysid: 目標無人機的 MAVLink system ID
             lat: 緯度 (度)
             lon: 經度 (度)
@@ -56,18 +67,10 @@ class MavlinkSender(CommandSender):
         """
         self.connection_string = connection_string
         self.mav = mavutil.mavlink_connection(connection_string)
-        print(f"MavlinkSender 已建立連線: {connection_string}")
+        _log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
 
-    def send_position_global(self, sysid, lat, lon, alt):
-        """
-        發送 SET_POSITION_TARGET_GLOBAL_INT
-
-        注意:
-            - coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
-              高度是相對起飛點的高度 (公尺)
-            - 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
-              需要在外部先減去起飛點高度再傳入
-        """
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
+        """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
         self.mav.mav.set_position_target_global_int_send(
             0,                          # time_boot_ms (不使用)
             sysid,                      # target_system
@@ -87,4 +90,80 @@ class MavlinkSender(CommandSender):
         if self.mav:
             self.mav.close()
             self.mav = None
-            print("MavlinkSender 已關閉")
+            _log("INFO", "MavlinkSender 已關閉")
+
+
+class Ros2CommandSender(QObject):
+    """
+    透過 fc_network_apps.PositionTargetGlobalIntClient 送 goto 指令。
+
+    設計重點:
+        - send_position_global 是同步介面，立刻回傳。結果透過 send_result signal 回報
+        - 每台 drone 維護自己的 asyncio.Task pipeline，新的目標會 cancel 掉舊的 in-flight
+        - client 由 DroneMonitor.get_or_create_position_client 管理，per-drone 獨立節點
+    """
+
+    # (drone_id, sysid, success, message)
+    send_result = pyqtSignal(str, int, bool, str)
+
+    DEFAULT_TIMEOUT_SEC = 2.0
+
+    def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
+        super().__init__()
+        self._monitor = monitor
+        self._timeout_sec = timeout_sec
+        self._pending = {}  # drone_id -> asyncio.Task
+
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
+        """同步介面。立刻回，結果透過 send_result signal 回報。"""
+        # 取消該機上一個未完成的 task，避免舊指令覆蓋新指令
+        old = self._pending.get(drone_id)
+        if old is not None and not old.done():
+            old.cancel()
+
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+
+        task = loop.create_task(
+            self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
+        )
+        self._pending[drone_id] = task
+
+    async def _do_send(self, drone_id, sysid, lat, lon, alt):
+        try:
+            client = self._monitor.get_or_create_position_client(drone_id)
+            if client is None:
+                self.send_result.emit(
+                    drone_id, sysid, False,
+                    "PositionTargetGlobalIntClient 無法建立"
+                )
+                return
+
+            result = await client.goto_position_only_async(
+                target_sysid=sysid,
+                target_compid=0,
+                latitude_deg=lat,
+                longitude_deg=lon,
+                alt=alt,
+                timeout_sec=self._timeout_sec,
+            )
+            self.send_result.emit(
+                drone_id, sysid, bool(result.success), str(result.message or "")
+            )
+        except asyncio.CancelledError:
+            # 被新的目標取代，正常狀況，不用回報
+            pass
+        except Exception as e:
+            traceback.print_exc()
+            self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
+
+    def close(self):
+        """取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
+        for task in self._pending.values():
+            if not task.done():
+                task.cancel()
+        self._pending.clear()
+        _log("INFO", "Ros2CommandSender 已關閉")

src/GUI/drone_panel.py

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

src/GUI/map_layout.py

@@ -1,8 +1,15 @@
 #!/usr/bin/env python3
+import json
+
 from PyQt6.QtWebEngineWidgets import QWebEngineView
 from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
 from PyQt6.QtWebChannel import QWebChannel
 
+
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
 class DroneMap:
     """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
     
@@ -11,6 +18,9 @@ class DroneMap:
         self.map_view = QWebEngineView()
         self.map_loaded = False
         self.pending_map_updates = {}
+        self.font_scale = 1.0
+        self.socket_colors = {}
+        self.drone_group_colors = {}
         
         # 創建橋接對象
         self.bridge = MapBridge()
@@ -31,6 +41,7 @@ class DroneMap:
             <script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
             <script src="qrc:///qtwebchannel/qwebchannel.js"></script>
             <style>
+                :root { --ui-font-scale: 1; }
                 html, body, #map { height: 100%; margin: 0; }
                 #map {
                     user-select: none;
@@ -65,7 +76,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     box-shadow: 0 2px 5px rgba(0,0,0,0.2);
                 }
@@ -85,7 +96,7 @@ class DroneMap:
                 }
                 .mission-info-row {
                     margin-bottom: 8px;
-                    font-size: 12px;
+                    font-size: calc(12px * var(--ui-font-scale));
                     color: #333;
                 }
                 .mission-info-label {
@@ -104,7 +115,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     margin-top: 8px;
                 }
@@ -131,7 +142,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -149,7 +160,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -167,7 +178,7 @@ class DroneMap:
                     border: none;
                     border-radius: 4px;
                     cursor: pointer;
-                    font-size: 13px;
+                    font-size: calc(13px * var(--ui-font-scale));
                     font-weight: bold;
                     transition: background-color 0.2s;
                 }
@@ -178,35 +189,8 @@ class DroneMap:
             </head>
             <body>
             <div id="map"></div>
-            <div class="map-controls">
-                <button class="selection-button" onclick="clearAllTrajectories()">清除軌跡</button>
-                <div style="border-top: 1px solid #ddd; padding-top: 5px; margin-top: 5px;">
-                    <label style="font-size: 12px; color: #555; font-weight: bold;">任務模式</label>
-                    <select id="mission-mode-select" onchange="onMissionModeChanged(this.value)" style="width: 100%; padding: 6px; border-radius: 4px; border: 1px solid #ccc; font-size: 12px; margin-top: 3px;">
-                        <option value="M_FORMATION">列隊飛行</option>
-                        <option value="CIRCLE_FORMATION">環狀包圍</option>
-                        <option value="LEADER_FOLLOWER">跟隨模式</option>
-                        <option value="GRID_SWEEP">柵狀偵查</option>
-                    </select>
-                </div>
-                <button class="confirm-route-button" id="confirm-route-btn" onclick="confirmRoute()" style="display: none;">確認路徑</button>
-                <button class="selection-button" id="select-polygon-btn" onclick="togglePolygonSelection()">多點選擇區域 (開發中)</button>
-            </div>
-            <div class="mission-info-panel">
-                <div class="selection-buttons">
-                    <button class="selection-button-blue" onclick="toggleSelectAllDrones()">全選無人機</button>
-                    <button class="selection-button-blue" id="select-drones-btn" onclick="toggleDroneSelection()">框選無人機</button>
-                </div>
-                <div class="mission-info-row">
-                    <span class="mission-info-label">中心點: </span>
-                    <span class="mission-info-value" id="center-position">未設定</span>
-                </div>
-                <div class="mission-info-row">
-                    <span class="mission-info-label">目標點: </span>
-                    <span class="mission-info-value" id="target-position">未設定</span>
-                </div>
-                <button class="mission-start-button" id="start-mission-btn" onclick="startMission()" disabled>開始任務</button>
-                <button class="mission-start-button" id="pause-mission-btn" onclick="pauseMission()">暫停任務</button>
+            <div class="map-controls" id="map-controls-panel" style="display: none;">
+                <button class="confirm-route-button" id="confirm-route-btn" onclick="confirmRoute()">確認路徑</button>
             </div>
 
             <script>
@@ -301,38 +285,37 @@ class DroneMap:
                     }, 100);
                 });
 
-                function createArrowIcon(color) {
+                function createArrowIcon(color, groupColor) {
+                const outlineColor = groupColor || 'transparent';
                 return L.divIcon({
                     className: 'drone-arrow',
                     html: `
                     <div style="
-                        width: 30px; height: 30px;
+                        width: 34px; height: 34px;
                         display: flex;
                         align-items: center;
                         justify-content: center;
                     ">
-                        <svg width="30" height="30" viewBox="0 0 30 30">
-                            <polygon points="15,0 30,30  15,25  0,30" fill="${color || '#FF0000'}" />
+                        <svg width="34" height="34" viewBox="0 0 34 34">
+                            <polygon
+                                points="17,3 31,31 17,26 3,31"
+                                fill="${color || '#FF0000'}"
+                                stroke="${outlineColor}"
+                                stroke-width="3"
+                                stroke-linejoin="round"
+                            />
                         </svg>
                     </div>
                     `,
-                    iconSize: [30, 30],
-                    iconAnchor: [15, 15]
+                    iconSize: [34, 34],
+                    iconAnchor: [17, 17]
                 });
                 }
 
                 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";
+                    const match = /^s([^_]+)_/.exec(id || '');
+                    const socketId = match ? match[1] : 'default';
+                    return socketColors[socketId] || socketColors.default || "#E0E0E0";
                 }
 
                 function createIdIcon(id) {
@@ -349,7 +332,7 @@ class DroneMap:
                     align-items: center;
                     justify-content: center;
                     font-weight: bold;
-                    font-size: 12px;
+                    font-size: calc(12px * var(--ui-font-scale));
                     text-shadow: 1px 1px 2px rgba(255,255,255,0.8), -1px -1px 2px rgba(255,255,255,0.8);
                     ">${sysid}</div>`,
                     iconSize: [16, 16],
@@ -360,7 +343,11 @@ class DroneMap:
                 var markers = {};
                 var idLabels = {};
                 var trajectories = {};
+                const maxTrajectoryPoints = 2000;
+                const trajectoryMinDistanceMeters = 0.05;
                 var trajectoryLines = {};
+                var socketColors = {};
+                var droneGroupColors = {};
                 var focusedId = null;
                 var initialized = false;
                 var trajectoryGroup = L.layerGroup().addTo(map);
@@ -374,6 +361,9 @@ class DroneMap:
                 var missionLine = null;
                 var centerPosition = null;
                 var targetPosition = null;
+
+                // 多群組任務規劃 — 每個 group 各自的 layer group
+                var groupMissionLayers = {};  // group_id → L.layerGroup
                 
                 // 選擇工具變量
                 var selectionMode = null;  // 'drones', 'rect', 'polygon', 'route', null
@@ -385,6 +375,16 @@ class DroneMap:
                 var drawStartPoint = null;
                 var mapDragStarted = false;
                 var clickStartPos = null;
+                var lastManualMapMoveAt = 0;
+                var autoCenteringMap = false;
+
+                function markManualMapMove() {
+                    if (!autoCenteringMap) {
+                        lastManualMapMoveAt = Date.now();
+                    }
+                }
+
+                map.on('dragstart drag dragend zoomstart zoomend', markManualMapMove);
                 
                 // 路徑標記變量 (跟隨模式用)
                 var routePoints = [];
@@ -393,30 +393,8 @@ class DroneMap:
                 var routeLayer = L.layerGroup().addTo(map);
                 // ================================================================================
                 
-                // 更新任務信息面板
-                function updateMissionInfo() {
-                    const centerElem = document.getElementById('center-position');
-                    const targetElem = document.getElementById('target-position');
-                    const startBtn = document.getElementById('start-mission-btn');
-                    
-                    if (centerPosition) {
-                        centerElem.textContent = `${centerPosition.lat.toFixed(6)}°, ${centerPosition.lng.toFixed(6)}°`;
-                    } else {
-                        centerElem.textContent = '未設定';
-                    }
-                    
-                    if (targetPosition) {
-                        targetElem.textContent = `${targetPosition.lat.toFixed(6)}°, ${targetPosition.lng.toFixed(6)}°`;
-                    } else {
-                        targetElem.textContent = '未設定';
-                    }
-                    
-                    if (centerPosition && targetPosition) {
-                        startBtn.disabled = false;
-                    } else {
-                        startBtn.disabled = true;
-                    }
-                }
+                // 任務信息已移至 GroupPanel 顯示
+                function updateMissionInfo() {}
                 
                 // ================================================================================
                 // 選擇工具函數
@@ -429,14 +407,14 @@ class DroneMap:
                 }
                 
                 function toggleDroneSelection() {
-                    clearSelectionMode();
                     if (selectionMode === 'drones') {
                         selectionMode = null;
-                        document.getElementById('select-drones-btn').classList.remove('active');
+                        selectionLayer.clearLayers();
+                        tempRectangle = null;
                         map.dragging.enable();
                     } else {
+                        clearSelectionMode();
                         selectionMode = 'drones';
-                        document.getElementById('select-drones-btn').classList.add('active');
                         map.dragging.disable();
                     }
                 }
@@ -450,7 +428,7 @@ class DroneMap:
                             clearSelectionMode();
                             clearPolygonPoints();
                             selectionMode = null;
-                            document.getElementById('select-polygon-btn').classList.remove('active');
+                            // select-polygon-btn removed from map overlay
                         }
                     } else {
                         clearSelectionMode();
@@ -462,17 +440,12 @@ class DroneMap:
                 }
                 
                 function clearSelectionMode() {
-                    document.getElementById('select-drones-btn').classList.remove('active');
-                    document.getElementById('select-polygon-btn').classList.remove('active');
-                    
                     selectionLayer.clearLayers();
                     tempRectangle = null;
-                    
+
                     // 不清除 routeLayer（由 clearRoutePoints 單獨管理）
-                    
                     map.dragging.enable();
-                    
-                    // 如果離開 route 模式，重置 selectionMode
+
                     if (selectionMode !== 'route') {
                         selectionMode = null;
                     }
@@ -527,7 +500,7 @@ class DroneMap:
                     }
                     
                     selectionMode = null;
-                    document.getElementById('select-polygon-btn').classList.remove('active');
+                    // select-polygon-btn removed from map overlay
                     map.dragging.enable();
                 }
                 
@@ -554,6 +527,10 @@ class DroneMap:
                             }
                         });
                         console.log('框選無人機:', selectedDrones);
+                        // 通知 Python 完整的框選結果
+                        if (bridge && selectedDrones.length > 0) {
+                            bridge.droneBoxSelected(JSON.stringify(selectedDrones));
+                        }
                         
                     } else if (selectionMode === 'rect') {
                         var rectPoints = [
@@ -588,18 +565,7 @@ class DroneMap:
                     
                     // 重置狀態
                     selectionMode = null;
-                    document.getElementById('select-drones-btn').classList.remove('active');
                     map.dragging.enable();
-                    
-                    // 如果仍在 Grid Sweep 模式，重新進入框選
-                    var currentMode = document.getElementById('mission-mode-select').value;
-                    if (currentMode === 'GRID_SWEEP') {
-                        setTimeout(function() {
-                            selectionMode = 'rect';
-                            map.dragging.disable();
-                            console.log('Grid Sweep: 重新進入框選模式');
-                        }, 500);
-                    }
                 }
                 
                 // ================================================================================
@@ -622,7 +588,7 @@ class DroneMap:
                             'align-items: center;' +
                             'justify-content: center;' +
                             'font-weight: bold;' +
-                            'font-size: 11px;' +
+                            'font-size: calc(11px * var(--ui-font-scale));' +
                             'border: 2px solid white;' +
                             'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
                             '">' + idx + '</div>',
@@ -675,6 +641,10 @@ class DroneMap:
                     }
                 }
                 // ================================================================================
+
+                function setFontScale(scale) {
+                    document.documentElement.style.setProperty('--ui-font-scale', scale);
+                }
                 
                 // 開始任務
                 function startMission() {
@@ -708,19 +678,19 @@ class DroneMap:
                     selectionMode = null;
                     clearSelectionMode();
                     clearRoutePoints();
-                    
-                    // 隱藏確認路徑按鈕
-                    document.getElementById('confirm-route-btn').style.display = 'none';
-                    
+
+                    // 預設隱藏控制面板
+                    document.getElementById('map-controls-panel').style.display = 'none';
+
                     if (mode === 'GRID_SWEEP') {
                         // 自動進入框選模式
                         selectionMode = 'rect';
                         map.dragging.disable();
                         console.log('Grid Sweep: 進入框選模式');
                     } else if (mode === 'LEADER_FOLLOWER') {
-                        // 進入路徑標記模式
+                        // 進入路徑標記模式 + 顯示確認路徑按鈕
                         selectionMode = 'route';
-                        document.getElementById('confirm-route-btn').style.display = 'block';
+                        document.getElementById('map-controls-panel').style.display = 'flex';
                         console.log('跟隨模式: 進入路徑標記模式，點擊地圖添加路徑點');
                     }
                     
@@ -739,7 +709,7 @@ class DroneMap:
                             color: color,
                             weight: 3,
                             opacity: 0.7,
-                            smoothFactor: 1
+                            smoothFactor: 0.1
                         }).addTo(trajectoryGroup);
                     }
                 }
@@ -749,24 +719,32 @@ class DroneMap:
                     const point = [lat, lon];
                     trajectories[id].push(point);
                     
-                    if (trajectories[id].length > 1000) {
+                    if (trajectories[id].length > maxTrajectoryPoints) {
                         trajectories[id].shift();
+                        trajectoryLines[id].setLatLngs(trajectories[id]);
+                    } else {
+                        trajectoryLines[id].addLatLng(point);
                     }
-                    
-                    trajectoryLines[id].setLatLngs([...trajectories[id]]);
                 }
 
                 function focusOn(id) {
                     if (!markers[id]) return;
                     focusedId = id;
                     var latlng = markers[id].getLatLng();
+                    autoCenteringMap = true;
                     map.flyTo(latlng, map.getZoom());
+                    setTimeout(() => { autoCenteringMap = false; }, 500);
                 }
 
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
+                        if (Date.now() - lastManualMapMoveAt < 1000) {
+                            return;
+                        }
                         var latlng = markers[focusedId].getLatLng();
+                        autoCenteringMap = true;
                         map.panTo(latlng);
+                        setTimeout(() => { autoCenteringMap = false; }, 300);
                     }
                 }, 1000);
 
@@ -774,7 +752,7 @@ class DroneMap:
                     if (markers[id]) {
                         const lastPos = markers[id].getLatLng();
                         const distance = lastPos.distanceTo([lat, lon]);
-                        if (distance > 1) {
+                        if (distance >= trajectoryMinDistanceMeters) {
                             addTrajectoryPoint(id, lat, lon);
                         }
                         
@@ -788,7 +766,7 @@ class DroneMap:
                         
                         const color = getColorBySocketId(id);
                         markers[id] = L.marker([lat, lon], {
-                            icon: createArrowIcon(color),
+                            icon: createArrowIcon(color, droneGroupColors[id]),
                             rotationAngle: heading,
                             rotationOrigin: 'center'
                         })
@@ -826,6 +804,33 @@ class DroneMap:
                     }
                 }
 
+                function setDroneGroupColors(groupColors) {
+                    droneGroupColors = groupColors || {};
+                    Object.keys(markers).forEach(id => {
+                        markers[id].setIcon(
+                            createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
+                        );
+                        if (typeof markers[id].setRotationAngle === 'function') {
+                            markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
+                        }
+                    });
+                }
+
+                function setSocketColors(colors) {
+                    socketColors = colors || {};
+                    Object.keys(markers).forEach(id => {
+                        markers[id].setIcon(
+                            createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
+                        );
+                        if (typeof markers[id].setRotationAngle === 'function') {
+                            markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
+                        }
+                    });
+                    Object.keys(trajectoryLines).forEach(id => {
+                        trajectoryLines[id].setStyle({ color: getColorBySocketId(id) });
+                    });
+                }
+
                 function clearAllTrajectories() {
                     trajectories = {};
                     Object.values(trajectoryLines).forEach(line => {
@@ -839,76 +844,74 @@ class DroneMap:
                 // 任務規劃視覺化函式
                 // ================================================================================
                 function drawMissionPlan(centerLat, centerLon, targetLat, targetLon) {
-                    clearMissionPlan();
-                    
+                    drawMissionPlanForGroup('_default', '#FF4444', centerLat, centerLon, targetLat, targetLon);
+                }
+
+                function drawMissionPlanForGroup(groupId, color, centerLat, centerLon, targetLat, targetLon) {
+                    clearMissionPlanForGroup(groupId);
+
                     centerPosition = {lat: centerLat, lng: centerLon};
                     targetPosition = {lat: targetLat, lng: targetLon};
                     updateMissionInfo();
-                    
+
+                    var layerGroup = L.layerGroup().addTo(map);
+                    groupMissionLayers[groupId] = layerGroup;
+
                     var centerIcon = L.divIcon({
                         className: 'mission-center',
-                        html: `<div style="
-                            background-color: #FF4444;
-                            color: white;
-                            width: 20px;
-                            height: 20px;
-                            border-radius: 50%;
-                            display: flex;
-                            align-items: center;
-                            justify-content: center;
-                            font-weight: bold;
-                            font-size: 12px;
-                            border: 2px solid white;
-                            box-shadow: 0 2px 5px rgba(0,0,0,0.3);
-                            ">C</div>`,
-                        iconSize: [20, 20],
-                        iconAnchor: [10, 10]
+                        html: '<div style="' +
+                            'background-color: ' + color + ';' +
+                            'color: white;' +
+                            'width: 22px; height: 22px;' +
+                            'border-radius: 50%;' +
+                            'display: flex; align-items: center; justify-content: center;' +
+                            'font-weight: bold; font-size: calc(10px * var(--ui-font-scale));' +
+                            'border: 2px solid white;' +
+                            'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
+                            '">' + groupId + '</div>',
+                        iconSize: [22, 22],
+                        iconAnchor: [11, 11]
                     });
-                    
-                    centerMarker = L.marker([centerLat, centerLon], {
-                        icon: centerIcon,
-                        zIndexOffset: 2000
-                    }).addTo(missionPlanGroup);
-                    
+
+                    L.marker([centerLat, centerLon], {
+                        icon: centerIcon, zIndexOffset: 2000
+                    }).addTo(layerGroup);
+
                     var targetIcon = L.divIcon({
                         className: 'mission-target',
-                        html: `<div style="
-                            background-color: #FFD700;
-                            color: #FF4444;
-                            width: 20px;
-                            height: 20px;
-                            border-radius: 50%;
-                            display: flex;
-                            align-items: center;
-                            justify-content: center;
-                            font-weight: bold;
-                            font-size: 14px;
-                            border: 2px solid white;
-                            box-shadow: 0 2px 5px rgba(0,0,0,0.3);
-                            ">★</div>`,
-                        iconSize: [20, 20],
-                        iconAnchor: [10, 10]
+                        html: '<div style="' +
+                            'background-color: #FFD700;' +
+                            'color: ' + color + ';' +
+                            'width: 22px; height: 22px;' +
+                            'border-radius: 50%;' +
+                            'display: flex; align-items: center; justify-content: center;' +
+                            'font-weight: bold; font-size: calc(14px * var(--ui-font-scale));' +
+                            'border: 2px solid white;' +
+                            'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
+                            '">★</div>',
+                        iconSize: [22, 22],
+                        iconAnchor: [11, 11]
                     });
-                    
-                    targetMarker = L.marker([targetLat, targetLon], {
-                        icon: targetIcon,
-                        zIndexOffset: 2000
-                    }).addTo(missionPlanGroup);
-                    
-                    console.log('任務規劃已繪製: C(' + centerLat + ', ' + centerLon + ') -> T(' + targetLat + ', ' + targetLon + ')');
+
+                    L.marker([targetLat, targetLon], {
+                        icon: targetIcon, zIndexOffset: 2000
+                    }).addTo(layerGroup);
+
+                    console.log('Group ' + groupId + ' 任務規劃已繪製');
                 }
-                
-                function clearMissionPlan() {
-                    if (centerMarker) {
-                        missionPlanGroup.removeLayer(centerMarker);
-                        centerMarker = null;
+
+                function clearMissionPlanForGroup(groupId) {
+                    if (groupMissionLayers[groupId]) {
+                        map.removeLayer(groupMissionLayers[groupId]);
+                        delete groupMissionLayers[groupId];
                     }
-                    
-                    if (targetMarker) {
-                        missionPlanGroup.removeLayer(targetMarker);
-                        targetMarker = null;
+                }
+
+                function clearAllMissionPlans() {
+                    for (var gid in groupMissionLayers) {
+                        map.removeLayer(groupMissionLayers[gid]);
                     }
-                    
+                    groupMissionLayers = {};
                     centerPosition = null;
                     targetPosition = null;
                     updateMissionInfo();
@@ -927,14 +930,17 @@ class DroneMap:
         # 設置地圖更新計時器
         self.map_update_timer = QTimer()
         self.map_update_timer.timeout.connect(self.update_map_positions)
-        self.map_update_timer.start(200)  # 每 200ms 更新一次
+        self.map_update_timer.start(100)  # 每 100ms 更新一次（10Hz）
     
     def _on_map_loaded(self, ok: bool):
         """地圖加載完成回調"""
         if ok:
             self.map_loaded = True
+            self.set_font_scale(self.font_scale)
+            self.set_socket_colors(self.socket_colors)
+            self.set_drone_group_colors(self.drone_group_colors)
         else:
-            print("⚠️ 地圖加載失敗")
+            _log("ERROR", "地圖載入失敗")
     
     def update_drone_position(self, drone_id, lat, lon, heading):
         """更新無人機位置（加入待處理隊列）"""
@@ -947,7 +953,7 @@ class DroneMap:
         
         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});")
+            js_commands.append(f"updateDrone({lat:.8f}, {lon:.8f}, '{drone_id}', {heading:.1f});")
         
         if js_commands:
             combined_js = "\n".join(js_commands)
@@ -959,27 +965,79 @@ class DroneMap:
         """清除所有軌跡"""
         if self.map_loaded:
             self.map_view.page().runJavaScript("clearAllTrajectories();")
+
+    def set_drone_group_colors(self, drone_group_colors):
+        """設定每台無人機的 group 外框顏色；未分組者不傳入即為透明。"""
+        self.drone_group_colors = dict(drone_group_colors or {})
+        if self.map_loaded:
+            payload = json.dumps(self.drone_group_colors)
+            self.map_view.page().runJavaScript(f"setDroneGroupColors({payload});")
+
+    def set_socket_colors(self, socket_colors):
+        """設定 socket 顏色，供地圖箭頭本體與軌跡線使用。"""
+        self.socket_colors = dict(socket_colors or {})
+        if self.map_loaded:
+            payload = json.dumps(self.socket_colors)
+            self.map_view.page().runJavaScript(f"setSocketColors({payload});")
     
     def focus_on_drone(self, drone_id):
         """聚焦到指定無人機"""
         if self.map_loaded:
             self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
+
+    def set_font_scale(self, scale):
+        """設定地圖 HTML 控制項的字體倍率。"""
+        self.font_scale = scale
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"setFontScale({scale:.3f});")
     
     # ================================================================================
     # 任務規劃視覺化方法
     # ================================================================================
     def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon):
-        """在地圖上繪製任務規劃"""
+        """在地圖上繪製任務規劃（舊介面，相容用）"""
+        self.draw_mission_plan_for_group('_default', '#FF4444',
+                                          center_lat, center_lon, target_lat, target_lon)
+
+    def draw_mission_plan_for_group(self, group_id, color,
+                                     center_lat, center_lon, target_lat, target_lon):
+        """在地圖上繪製指定群組的任務規劃（帶顏色區分）"""
         if self.map_loaded:
-            js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});"
+            js_code = (
+                f"drawMissionPlanForGroup("
+                f"'{group_id}', '{color}', "
+                f"{center_lat:.6f}, {center_lon:.6f}, "
+                f"{target_lat:.6f}, {target_lon:.6f});"
+            )
             self.map_view.page().runJavaScript(js_code)
-            print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
-    
+            _log(
+                "INFO",
+                f"地圖已繪製 Group {group_id} 任務規劃: "
+                f"C({center_lat:.6f}, {center_lon:.6f}) -> "
+                f"T({target_lat:.6f}, {target_lon:.6f})",
+            )
+
     def clear_mission_plan(self):
-        """清除地圖上的任務規劃標記"""
+        """清除地圖上所有任務規劃標記"""
         if self.map_loaded:
-            self.map_view.page().runJavaScript("clearMissionPlan();")
-            print("🗑️ 地圖已清除任務規劃")
+            self.map_view.page().runJavaScript("clearAllMissionPlans();")
+            _log("INFO", "地圖已清除所有任務規劃")
+
+    def clear_mission_plan_for_group(self, group_id):
+        """清除指定群組的任務規劃標記"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
+            _log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
+
+    def set_mission_mode(self, mode):
+        """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript(f"onMissionModeChanged('{mode}');")
+
+    def toggle_drone_box_select(self):
+        """切換地圖上的無人機框選模式"""
+        if self.map_loaded:
+            self.map_view.page().runJavaScript("toggleDroneSelection();")
     # ================================================================================
     
     def get_widget(self):
@@ -1029,6 +1087,10 @@ class DroneMap:
     def get_route_confirmed_signal(self):
         """獲取路徑確認信號"""
         return self.bridge.route_confirmed
+
+    def get_drone_box_selected_signal(self):
+        """獲取框選無人機完成信號"""
+        return self.bridge.drone_box_selected
     
 class MapBridge(QObject):
     """JavaScript 和 Python 之間的橋接類"""
@@ -1042,7 +1104,8 @@ class MapBridge(QObject):
     polygon_selected = pyqtSignal(str)
     mission_mode_changed = pyqtSignal(str)
     route_confirmed = pyqtSignal(str)  # 路徑確認 (JSON 字串)
-    
+    drone_box_selected = pyqtSignal(str)  # 框選無人機完成 (JSON 字串)
+
     def __init__(self):
         super().__init__()
     
@@ -1060,46 +1123,56 @@ class MapBridge(QObject):
     def clearAllDroneSelection(self):
         """供 JavaScript 調用的方法 - 清除所有無人機選擇"""
         self.clear_all_drone_selection.emit()
-        print("🗑️ 清除所有無人機選擇")
+        _log("INFO", "已清除所有無人機選擇")
     
     @pyqtSlot()
     def toggleSelectAllDrones(self):
         """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
         self.select_all_drones.emit()
-        print("🔄 切換全選無人機")
+        _log("INFO", "已切換全選無人機")
     
     @pyqtSlot(float, float, float, float)
     def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
         """供 JavaScript 調用的方法 - 開始任務"""
         self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon)
-        print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
+        _log(
+            "INFO",
+            f"已發出開始任務信號: "
+            f"C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})",
+        )
     
     @pyqtSlot()
     def pauseMissionSignal(self):
         """供 JavaScript 調用的方法 - 暫停任務"""
         self.pause_mission_signal.emit()
-        print("⏸️ 暫停任務信號已發出")
+        _log("INFO", "已發出暫停任務信號")
     
     @pyqtSlot(str)
     def rectangleSelected(self, points_json):
         """供 JavaScript 調用的方法 - 矩形選擇完成"""
         self.rectangle_selected.emit(points_json)
-        print(f"📦 矩形區域已選擇: {points_json}")
+        _log("INFO", f"矩形區域已選擇: {points_json}")
     
     @pyqtSlot(str)
     def polygonSelected(self, points_json):
         """供 JavaScript 調用的方法 - 多邊形選擇完成"""
         self.polygon_selected.emit(points_json)
-        print(f"🔷 多邊形區域已選擇: {points_json}")
+        _log("INFO", f"多邊形區域已選擇: {points_json}")
     
     @pyqtSlot(str)
     def missionModeChanged(self, mode):
         """供 JavaScript 調用的方法 - 任務模式切換"""
         self.mission_mode_changed.emit(mode)
-        print(f"🔄 任務模式切換: {mode}")
+        _log("INFO", f"任務模式已切換: {mode}")
     
     @pyqtSlot(str)
     def routeConfirmed(self, points_json):
         """供 JavaScript 調用的方法 - 路徑確認"""
         self.route_confirmed.emit(points_json)
-        print(f"📍 路徑已確認: {points_json}")
+        _log("INFO", f"路徑已確認: {points_json}")
+
+    @pyqtSlot(str)
+    def droneBoxSelected(self, drone_ids_json):
+        """供 JavaScript 調用的方法 - 框選無人機完成"""
+        self.drone_box_selected.emit(drone_ids_json)
+        _log("INFO", f"框選無人機完成: {drone_ids_json}")

src/GUI/mission_executor.py

@@ -6,15 +6,23 @@
 設計:
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 用 QTimer 驅動，在 Qt 主線程執行
-    - 暫停 = 停止發送 setpoint，飛控自動懸停
-    - 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep)
+    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
+    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+    - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
+    - 用 QTimer 驅動到達判定，在 Qt 主線程執行
+    - 暫停 = 停止到達判定 + 停止新指令
 """
+import asyncio
 import math
+import time
 from enum import Enum
 from PyQt6.QtCore import QObject, QTimer, pyqtSignal
 
 
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
 class MissionState(Enum):
     """整體任務狀態"""
     IDLE = "idle"
@@ -22,22 +30,34 @@ class MissionState(Enum):
     PAUSED = "paused"
 
 
+class TaskStatus(Enum):
+    """單機任務狀態"""
+    NORMAL = "normal"
+    RETRYING = "retrying"
+    WAITING_AT_BARRIER = "waiting_at_barrier"
+    FALLBACK_LOITER = "fallback_loiter"
+
+
 class DroneTask:
     """單架無人機的任務資料"""
-    __slots__ = ('drone_id', 'sysid', 'waypoints', 'wp_index', 'done')
+    __slots__ = (
+        'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
+        'sent_current_wp', 'fail_count', 'status', 'waiting_since',
+        'entered_radius_at', 'last_log_at',
+    )
 
     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
+        self.sent_current_wp = False
+        self.fail_count = 0
+        self.status = TaskStatus.NORMAL
+        self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
+        self.entered_radius_at = 0.0  # 進入 arrival_radius 的時間，0 = 尚未進入
+        self.last_log_at = 0.0  # 上次印 distance log 的時間
 
     @property
     def current_target(self):
@@ -52,7 +72,7 @@ class DroneTask:
 
 class MissionExecutor(QObject):
     """
-    任務執行器
+    任務執行器 (事件驅動版 + rendezvous barrier)
 
     planned_waypoints 格式:
         {
@@ -60,39 +80,59 @@ class MissionExecutor(QObject):
             'waypoints': [
                 [(lat,lon,alt), ...],   # drone 0
                 [(lat,lon,alt), ...],   # drone 1
-            ]
+            ],
+            'rendezvous_indices': [3, 7, ...],   # 選填；缺省 = 全程不同步（各自跑）
         }
     """
 
-    # 信號
-    drone_waypoint_reached = pyqtSignal(str, int, int)  # (drone_id, wp_index, total)
+    MAX_RETRY = 3
+
+    drone_waypoint_reached = pyqtSignal(str, int, int)   # (drone_id, wp_index, total)
+    task_status_changed = pyqtSignal(str, str, str)       # (drone_id, status, message)
     mission_completed = pyqtSignal()
 
-    def __init__(self, sender, drone_gps,
-                 arrival_radius=2.0, send_rate_hz=2.0):
+    def __init__(self, sender, drone_gps, monitor=None,
+                 arrival_radius=4.0, tick_rate_hz=2.0,
+                 barrier_timeout_sec=20.0,
+                 hover_stable_sec=2.0,
+                 progress_log_interval_sec=3.0):
         super().__init__()
         self.sender = sender
         self.drone_gps = drone_gps
+        self.monitor = monitor  # 用於失敗 fallback 到 LOITER
         self.arrival_radius = arrival_radius
+        self.barrier_timeout_sec = barrier_timeout_sec
+        # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
+        # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
+        self.hover_stable_sec = hover_stable_sec
+        self.hysteresis_factor = 1.5
+        self.progress_log_interval_sec = progress_log_interval_sec
         self.state = MissionState.IDLE
         self.tasks = {}
+        self.rendezvous_indices = set()
 
-        self._interval_ms = int(1000 / send_rate_hz)
+        self._interval_ms = int(1000 / tick_rate_hz)
         self._timer = QTimer()
         self._timer.timeout.connect(self._tick)
 
+        # 只有 Ros2CommandSender 有 send_result signal；MavlinkSender 沒有
+        if hasattr(sender, 'send_result'):
+            sender.send_result.connect(self._on_send_result)
+
     # ------------------------------------------------------------------ 公開方法
 
     def start(self, planned_waypoints):
-        """啟動任務"""
         if self.state == MissionState.RUNNING:
-            print("任務已在執行中")
+            _log("WARN", "任務已在執行中")
             return
 
         self.tasks.clear()
 
         drone_ids = planned_waypoints['drone_ids']
         waypoints_list = planned_waypoints['waypoints']
+        self.rendezvous_indices = set(
+            planned_waypoints.get('rendezvous_indices', []) or []
+        )
 
         for i, drone_id in enumerate(drone_ids):
             sysid = int(drone_id.split('_')[1])
@@ -102,86 +142,278 @@ class MissionExecutor(QObject):
         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")
+        rv_info = (
+            f"rendezvous WP={sorted(self.rendezvous_indices)}"
+            if self.rendezvous_indices else "無 rendezvous (各自跑)"
+        )
+        _log(
+            "INFO",
+            f"任務已啟動: {len(self.tasks)} 架無人機, "
+            f"共 {total_wps} 個航點, "
+            f"到達半徑={self.arrival_radius}m (hover-stable {self.hover_stable_sec}s), "
+            f"tick 週期={self._interval_ms}ms, "
+            f"barrier timeout={self.barrier_timeout_sec}s, "
+            f"{rv_info}",
+        )
 
     def pause(self):
-        """暫停任務"""
         if self.state == MissionState.RUNNING:
             self._timer.stop()
             self.state = MissionState.PAUSED
-            print("任務暫停")
+            _log("INFO", "任務已暫停")
 
     def resume(self):
-        """恢復任務"""
         if self.state == MissionState.PAUSED:
             self._timer.start(self._interval_ms)
             self.state = MissionState.RUNNING
-            print("任務恢復")
+            _log("INFO", "任務已恢復")
 
     def stop(self):
-        """停止任務"""
         self._timer.stop()
         self.tasks.clear()
+        self.rendezvous_indices = set()
         self.state = MissionState.IDLE
-        print("任務停止")
+        _log("INFO", "任務已停止")
 
     # ------------------------------------------------------------------ 控制迴圈
 
     def _tick(self):
-        """控制迴圈"""
-        all_done = True
+        """
+        控制 tick：
+            Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING
+            Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout）
+            Phase 3: 每台未送過當前航點的機送一次
+            Phase 4: 全部完成檢查
+        """
+        now = time.monotonic()
 
+        # ---- Phase 1: 到達判定（hover-stable） ----
         for task in self.tasks.values():
-            if task.done:
+            if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+                continue
+            # 已在 barrier 等待的不再跑到達判定
+            if task.status == TaskStatus.WAITING_AT_BARRIER:
                 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
+            tgt_lat, tgt_lon, _ = target
             distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
 
-            # 到達判定
+            # hover-stable 判定：
+            #   - 首次進入 radius → 記錄時間
+            #   - 在 radius * hysteresis 內持續 → 保留計時
+            #   - 離開 hysteresis → 重置
+            #   - 在 radius 內穩定 hover_stable_sec 秒 → 視為到達
+            hysteresis_radius = self.arrival_radius * self.hysteresis_factor
             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})")
+                if task.entered_radius_at == 0.0:
+                    task.entered_radius_at = now
+                stable_for = now - task.entered_radius_at
+                if stable_for < self.hover_stable_sec:
+                    self._maybe_log_progress(task, now, distance, stable_for)
                     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
+            elif distance < hysteresis_radius and task.entered_radius_at > 0.0:
+                # 還在 hysteresis 內，計時不重置（容忍 GPS 抖動跨越邊界）
+                self._maybe_log_progress(task, now, distance, now - task.entered_radius_at)
+                continue
+            else:
+                # 離開 hysteresis：重置計時，繼續等待到達
+                task.entered_radius_at = 0.0
+                self._maybe_log_progress(task, now, distance, 0.0)
+                continue
 
-            # 發送 setpoint
+            # 到達當前 wp_index
+            task.entered_radius_at = 0.0  # 重置給下個 wp 用
+            if (task.wp_index in self.rendezvous_indices
+                    and task.wp_index < task.total_waypoints - 1):
+                # rendezvous 點 → 不推進，進入 barrier 等待
+                task.status = TaskStatus.WAITING_AT_BARRIER
+                task.waiting_since = now
+                _log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index}，等待同伴")
+                self.task_status_changed.emit(
+                    task.drone_id, task.status.value,
+                    f"waiting at WP {task.wp_index}"
+                )
+                continue
+
+            # 一般點 → 直接推進
+            self._advance_waypoint(task, distance)
+
+        # ---- Phase 2: barrier 釋放檢查 ----
+        self._check_barriers(now)
+
+        # ---- Phase 3: 發送未送過的目標 ----
+        for task in self.tasks.values():
+            if task.done or task.status in (
+                TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
+            ):
+                continue
+            if task.sent_current_wp:
+                continue
+            target = task.current_target
+            if target is None:
+                continue
+            tgt_lat, tgt_lon, tgt_alt = target
+            task.sent_current_wp = True
             self.sender.send_position_global(
-                task.sysid, tgt_lat, tgt_lon, tgt_alt
+                task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
             )
 
-        # 全部完成檢查
+        # ---- Phase 4: 完成檢查 ----
+        all_done = all(
+            t.done or t.status == TaskStatus.FALLBACK_LOITER
+            for t in self.tasks.values()
+        )
         if all_done and self.tasks:
             self._timer.stop()
             self.state = MissionState.IDLE
             self.mission_completed.emit()
-            print("===== 任務全部完成 =====")
+            _log("INFO", "任務全部完成")
+
+    def _maybe_log_progress(self, task, now, distance, stable_for):
+        """週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
+        if now - task.last_log_at < self.progress_log_interval_sec:
+            return
+        task.last_log_at = now
+        if stable_for > 0:
+            stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
+        else:
+            stable_info = ""
+        _log(
+            "INFO",
+            f"{task.drone_id} → WP {task.wp_index}: "
+            f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
+        )
+
+    def _advance_waypoint(self, task, arrived_distance):
+        """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
+        task.wp_index += 1
+        task.sent_current_wp = False
+        task.fail_count = 0
+        task.entered_radius_at = 0.0
+        task.last_log_at = 0.0
+        if task.wp_index >= task.total_waypoints:
+            task.done = True
+            self.drone_waypoint_reached.emit(
+                task.drone_id, task.wp_index, task.total_waypoints
+            )
+            _log(
+                "INFO",
+                f"{task.drone_id} 已完成所有航點 "
+                f"({task.total_waypoints}/{task.total_waypoints})",
+            )
+            return
+        self.drone_waypoint_reached.emit(
+            task.drone_id, task.wp_index, task.total_waypoints
+        )
+        _log(
+            "INFO",
+            f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
+            f"(到達距離: {arrived_distance:.1f}m)",
+        )
+
+    def _check_barriers(self, now):
+        """檢查每個有人在等的 barrier 能不能釋放。"""
+        # 蒐集目前有哪些 barrier 有人卡著
+        waiting_groups = {}  # barrier_idx -> [task, ...]
+        for task in self.tasks.values():
+            if task.status == TaskStatus.WAITING_AT_BARRIER:
+                waiting_groups.setdefault(task.wp_index, []).append(task)
+
+        if not waiting_groups:
+            return
+
+        for barrier_idx, waiting_tasks in waiting_groups.items():
+            # 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
+            everyone_arrived = True
+            for task in self.tasks.values():
+                if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+                    continue  # 失敗機/完成機不擋
+                if (task.status == TaskStatus.WAITING_AT_BARRIER
+                        and task.wp_index == barrier_idx):
+                    continue  # 在這裡等
+                if task.wp_index > barrier_idx:
+                    continue  # 早已越過（barrier 先前已對他釋放）
+                # 還在路上
+                everyone_arrived = False
+                break
+
+            force_reason = None
+            if not everyone_arrived:
+                # 檢查 timeout：以該 barrier 上最早進入等待的時間為準
+                oldest_wait = min(t.waiting_since for t in waiting_tasks)
+                if now - oldest_wait >= self.barrier_timeout_sec:
+                    force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
+                    _log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
+                else:
+                    continue  # 還沒到齊、也還沒 timeout → 繼續等
+
+            # 釋放：把所有在此 barrier 等待的機一起推進
+            tag = "全員到齊" if force_reason is None else force_reason
+            _log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
+            for task in waiting_tasks:
+                task.status = TaskStatus.NORMAL
+                msg = f"barrier WP {barrier_idx} released ({tag})"
+                self.task_status_changed.emit(
+                    task.drone_id, task.status.value, msg
+                )
+                self._advance_waypoint(task, self.arrival_radius)
+
+    # ------------------------------------------------------------------ 結果回呼
+
+    def _on_send_result(self, drone_id, sysid, success, message):
+        """Ros2CommandSender.send_result 的 slot"""
+        task = self.tasks.get(drone_id)
+        if task is None or task.done:
+            return
+        # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
+        if task.status == TaskStatus.WAITING_AT_BARRIER:
+            return
+
+        if success:
+            if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
+                task.status = TaskStatus.NORMAL
+                self.task_status_changed.emit(drone_id, task.status.value, "recovered")
+            task.fail_count = 0
+            return
+
+        task.fail_count += 1
+        _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
+
+        if task.fail_count < self.MAX_RETRY:
+            task.status = TaskStatus.RETRYING
+            task.sent_current_wp = False  # 下個 tick 會重送
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+            )
+        else:
+            task.status = TaskStatus.FALLBACK_LOITER
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
+            )
+            _log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER")
+            self._fallback_to_loiter(drone_id)
+
+    def _fallback_to_loiter(self, drone_id):
+        """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
+        if self.monitor is None:
+            _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
+            return
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+        loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
 
 
 # ------------------------------------------------------------------ 工具函式
@@ -196,4 +428,4 @@ def _haversine(lat1, lon1, lat2, lon2):
 
     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))
+    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

src/GUI/mission_group.py

@@ -0,0 +1,640 @@
+#!/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 = [
+    "#E53935",  # Deep Red
+    "#FB8C00",  # Deep Orange
+    "#FDD835",  # Deep Yellow
+    "#43A047",  # Deep Green
+    "#1E88E5",  # Deep Blue
+    "#8E24AA",  # Deep Purple
+    "#D81B60",  # Deep Pink
+]
+
+DEFAULT_MISSION_PARAM_VALUES = {
+    'spacing': '5.0',
+    'base_altitude': '10.0',
+    'altitude_diff': '2.0',
+    'radius': '10.0',
+    'altitude': '10.0',
+    'start_angle': '0',
+    'lateral_offset': '3.0',
+    'longitudinal_spacing': '5.0',
+    'line_spacing': '5.0',
+    # 執行參數（所有任務類型共用）
+    'arrival_radius': '4.0',     # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2
+    'hover_stable_sec': '2.0',   # 在 radius 內穩定停留多久才視為到達 (s)
+}
+
+
+# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換）
+EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
+
+
+class MissionGroup:
+    """單一任務群組的資料"""
+
+    def __init__(self, group_id, color):
+        self.group_id = group_id             # 'A', 'B', 'C', ...
+        self.color = color                   # 顏色 hex
+        
+        # 唯一真實資料來源：該 group 選中的 drone ID
+        # 代表：group 擁有的 drone、被選中的 drone、可以操作的 drone
+        self.selected_drone_ids = set()
+        
+        self.mission_type = 'M_FORMATION'    # 預設任務類型
+        self.planned_waypoints = None        # 規劃結果 dict
+        self.executor = None                 # MissionExecutor 實例（延遲建立）
+        self.center_origin = None            # 規劃原點
+        self.leader_drone_id = None          # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
+
+    @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=None):
+        """
+        Args:
+            parent: 父 widget
+            all_drone_ids: 所有可用無人機 ID 列表
+            current_assigned: 當前群組已分配的無人機 set
+            other_assigned: (忽略 — 現在允許多 group 分配)
+        """
+        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; }
+        """)
+
+        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)
+
+            # 所有 drone 都能被選摘（支持多 group 分配）
+            if drone_id in current_assigned:
+                cb.setChecked(True)
+
+            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
+    reboot_requested = pyqtSignal(str)                 # group_id
+    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, default_params=None):
+        super().__init__(parent)
+        self.group = group
+        self.all_btn_ref = None  # 保存全選按鈕的參考
+        self.default_params = dict(DEFAULT_MISSION_PARAM_VALUES)
+        if default_params:
+            self.default_params.update(default_params)
+        self._build_ui()
+
+    def _make_sep(self):
+        """建立垂直分隔線"""
+        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))
+        self.delete_group_btn = QPushButton("刪除群組")
+        self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        self.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(self.delete_group_btn)
+        right.addLayout(grid_r2)
+
+        right.addStretch()
+
+        # ============================
+        # 飛控操作
+        # ============================
+        reboot_col = QVBoxLayout()
+        reboot_col.setSpacing(3)
+
+        reboot_title = QLabel("飛控操作")
+        reboot_title.setStyleSheet(TITLE)
+        reboot_col.addWidget(reboot_title)
+
+        reboot_btn = QPushButton("重啟飛控")
+        reboot_btn.setStyleSheet(BTN.format(bg='#8A3A3A', fg='#FFF', hover='#A84646'))
+        reboot_btn.clicked.connect(
+            lambda: self.reboot_requested.emit(self.group.group_id))
+        reboot_col.addWidget(reboot_btn)
+        reboot_col.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)', self.default_params['spacing']),
+                ('base_altitude', '基準高度 (m)', self.default_params['base_altitude']),
+                ('altitude_diff', '高低差 (m)', self.default_params['altitude_diff']),
+            ],
+            'CIRCLE_FORMATION': [
+                ('radius', '半徑 (m)', self.default_params['radius']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('start_angle', '起始角 (°)', self.default_params['start_angle']),
+            ],
+            'LEADER_FOLLOWER': [
+                ('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
+                ('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+            ],
+            'GRID_SWEEP': [
+                ('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+            ],
+        }
+
+        # 所有 mission type 共用的執行參數（不隨類型切換隱藏）
+        self._exec_param_defs = [
+            ('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
+            ('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
+        ]
+
+        # 建立所有參數列的 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)
+
+        # 執行參數列（所有 mission type 共用，永遠顯示）
+        for key, label_text, default in self._exec_param_defs:
+            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)
+
+        # LEADER_FOLLOWER 專用：領隊下拉選單
+        self._leader_row = QWidget()
+        leader_layout = QHBoxLayout(self._leader_row)
+        leader_layout.setContentsMargins(0, 0, 0, 0)
+        leader_layout.setSpacing(3)
+        leader_lbl = QLabel("領隊")
+        leader_lbl.setStyleSheet(LBL)
+        self._leader_combo = QComboBox()
+        self._leader_combo.setStyleSheet(COMBO)
+        self._leader_combo.setFixedWidth(70)
+        self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
+        leader_layout.addWidget(leader_lbl, 1)
+        leader_layout.addWidget(self._leader_combo)
+        param_col.addWidget(self._leader_row)
+
+        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.addWidget(self._make_sep())
+        cols.addLayout(reboot_col, 0)
+        cols.addStretch()  # 填充剩餘空間，使欄位置左
+
+        layout.addLayout(cols)
+
+    def update_drone_list(self):
+        """更新無人機列表顯示"""
+        if not self.group.selected_drone_ids:
+            self.drone_list_label.setText("尚未分配")
+            self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        else:
+            sorted_ids = sorted(self.group.selected_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;")
+        self._refresh_leader_options()
+
+    def _refresh_leader_options(self):
+        """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
+        sorted_ids = sorted(self.group.selected_drone_ids,
+                            key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+        current = self.group.leader_drone_id
+        self._leader_combo.blockSignals(True)
+        self._leader_combo.clear()
+        self._leader_combo.addItems(sorted_ids)
+        if current and current in sorted_ids:
+            self._leader_combo.setCurrentText(current)
+        elif sorted_ids:
+            self._leader_combo.setCurrentText(sorted_ids[0])
+            self.group.leader_drone_id = sorted_ids[0]
+        else:
+            self.group.leader_drone_id = None
+        self._leader_combo.blockSignals(False)
+
+    def _on_leader_changed(self, drone_id):
+        self.group.leader_drone_id = drone_id if drone_id else None
+
+    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.selected_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_delete_enabled(self, enabled):
+        """啟用或禁用刪除群組按鈕"""
+        self.delete_group_btn.setEnabled(enabled)
+
+    def _update_param_visibility(self, _=None):
+        """根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。"""
+        mission_type = self.type_combo.currentText()
+        mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        exec_keys = {d[0] for d in self._exec_param_defs}
+        visible_keys = mission_keys | exec_keys
+        for key, (row_w, _inp) in self._param_widgets.items():
+            row_w.setVisible(key in visible_keys)
+        self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
+
+    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 get_execution_params(self):
+        """讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict"""
+        params = {}
+        for key, _label, default in self._exec_param_defs:
+            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 set_param_value(self, key, value):
+        """更新指定參數欄位的文字值。"""
+        if key not in self._param_widgets:
+            return
+        _row_w, inp = self._param_widgets[key]
+        inp.setText(str(value))
+
+    def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
+        """更新中心點 / 目標點顯示"""
+        info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
+        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

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """
 飛行任務規劃模組
-支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep
+支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
 """
 import math
 from typing import List, Tuple, Optional, Dict, Any
@@ -72,16 +72,22 @@ class FormationPlanner:
                                mission_type: MissionType = MissionType.M_FORMATION,
                                params: Optional[Dict[str, Any]] = None
                                ) -> Tuple[List[List[Tuple[float, float, float]]],
-                                          Tuple[float, float, float]]:
+                                          Tuple[float, float, float],
+                                          List[int]]:
+        """
+        回傳 (waypoints_gps, origin, rendezvous_indices)。
+        rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
+        """
         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)
+        # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
+        # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
+        self.current_origin = (center_lat, center_lon, 0.0)
+        self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
 
         drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
         target_local = self.converter.gps_to_local(*target_gps)
@@ -89,10 +95,14 @@ class FormationPlanner:
         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))]
+            # 起點集合後一起出發到 stage2
+            rendezvous_indices = [0]
 
         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))]
+            # 半程集合後一起進最終圓環位置
+            rendezvous_indices = [0]
 
         elif mission_type == MissionType.LEADER_FOLLOWER:
             params = params or {}
@@ -103,7 +113,9 @@ class FormationPlanner:
                 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)
+            waypoints_local, rendezvous_indices = self._calculate_leader_follower(
+                drone_local, route_wps_local, params
+            )
 
         elif mission_type == MissionType.GRID_SWEEP:
             params = params or {}
@@ -114,7 +126,9 @@ class FormationPlanner:
                 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)
+            waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
+                drone_local, rect_corners_local, params
+            )
         else:
             raise ValueError(f"不支援的任務類型: {mission_type}")
 
@@ -123,7 +137,7 @@ class FormationPlanner:
             gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
             waypoints_gps.append(gps_wps)
 
-        return waypoints_gps, self.current_origin
+        return waypoints_gps, self.current_origin, rendezvous_indices
 
     # ------------------------------------------------------------------ M-Formation
 
@@ -179,187 +193,177 @@ class FormationPlanner:
             final_y = center_y + radius * math.sin(angle_rad)
             final_z = altitude
 
-            current_x, current_y, current_z = drone_positions[i]
+            current_x, current_y, _current_z = drone_positions[i]
+            # stage1 高度直接用 final_z：先爬升到目標高度再水平集合，
+            # 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高）
             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
+                final_z,
             ))
             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. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
+        center_path = 使用者畫的路徑點（直線段連接，不插弧），
+        每架 drone 沿弧長 s 參數化做隊形偏移：
+            s_follower = s_leader - lon_k
+            position = lookup(s_follower) + lat_k × perp(tangent)
+        s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
 
+        Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
         隊形 (俯視):
-            |     (前進方向)         |
-            |           D1          |  ← 左偏移, 後 0m
-            |                D2     |  ← 右偏移, 後 5m
-            |           D3          |  ← 左偏移, 後 10m
-            |                D4     |  ← 右偏移, 後 15m
+            D0 (lat=-L, lon=0)   ← leader
+                D1 (lat=+L, lon=5)
+            D2 (lat=-L, lon=10)
+                D3 (lat=+L, lon=15)
         """
         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 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
+        center_path, barrier_indices = self._build_center_path(route_wps_local)
 
-        # 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
+        s_list = [pt['s'] for pt in center_path]
+        s_max = s_list[-1]
+        n_pts = len(center_path)
 
-        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()
+        def lookup(s_target):
+            """在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。"""
+            if n_pts == 1:
+                pt = center_path[0]
+                return pt['x'], pt['y'], pt['dir']
 
-        # Step 3: 偏移
+            if s_target <= 0.0:
+                a = center_path[0]
+                b = center_path[1]
+                sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+                slen = math.hypot(sdx, sdy)
+                if slen > 1e-6:
+                    dx, dy = sdx / slen, sdy / slen
+                    return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
+                return a['x'], a['y'], a['dir']
+
+            if s_target >= s_max:
+                a = center_path[-2]
+                b = center_path[-1]
+                sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+                slen = math.hypot(sdx, sdy)
+                if slen > 1e-6:
+                    dx, dy = sdx / slen, sdy / slen
+                    overshoot = s_target - s_max
+                    return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
+                return b['x'], b['y'], b['dir']
+
+            # Binary search：s_list[lo] <= s_target < s_list[hi]
+            lo, hi = 0, n_pts - 1
+            while lo + 1 < hi:
+                mid = (lo + hi) // 2
+                if s_list[mid] <= s_target:
+                    lo = mid
+                else:
+                    hi = mid
+            a = center_path[lo]
+            b = center_path[hi]
+            ds = s_list[hi] - s_list[lo]
+            if ds < 1e-9:
+                return a['x'], a['y'], a['dir']
+            t = (s_target - s_list[lo]) / ds
+            x = a['x'] + t * (b['x'] - a['x'])
+            y = a['y'] + t * (b['y'] - a['y'])
+            sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+            slen = math.hypot(sdx, sdy)
+            if slen > 1e-6:
+                return x, y, (sdx / slen, sdy / slen)
+            return x, y, a['dir']
+
+        # Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort
+        # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
         all_waypoints = [None] * N
 
-        for rank, (_, original_idx) in enumerate(projections):
+        for rank in range(N):
             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:
+            for i in range(n_pts):
+                s_follower = s_list[i] - lon
+                x, y, (dx, dy) = lookup(s_follower)
                 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))
+                waypoints.append((
+                    x + lat * perp_x,
+                    y + lat * perp_y,
+                    altitude,
+                ))
 
-            all_waypoints[original_idx] = waypoints
+            all_waypoints[rank] = waypoints
 
-        return all_waypoints
+        return all_waypoints, barrier_indices
 
-    def _build_center_path(self, waypoints, turn_margin, curve_resolution):
+    def _build_center_path(self, waypoints):
         """
-        建立帶 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 插值點數
+        每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
+        飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。
 
         Returns:
-            [(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向
+            (path, barrier_indices)
         """
         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))
+            return [{
+                'seg': 'straight',
+                'x': waypoints[0][0], 'y': waypoints[0][1],
+                'dir': (0.0, 1.0),
+                's': 0.0,
+            }], []
 
         path = []
+        barrier_indices = []
 
-        # 第一個航點
-        path.append((waypoints[0][0], waypoints[0][1],
-                      segments[0][0], segments[0][1]))
+        for i in range(num_wps):
+            if i < num_wps - 1:
+                dx = waypoints[i + 1][0] - waypoints[i][0]
+                dy = waypoints[i + 1][1] - waypoints[i][1]
+            else:
+                dx = waypoints[i][0] - waypoints[i - 1][0]
+                dy = waypoints[i][1] - waypoints[i - 1][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]
+            length = math.hypot(dx, dy)
+            if length < 0.01:
+                direction = (0.0, 1.0)
+            else:
+                direction = (dx / length, dy / length)
 
-            # 切入距離 T: 取相鄰兩段中較短的 × turn_margin
-            T = min(len_in, len_out) * turn_margin
+            path.append({
+                'seg': 'straight',
+                'x': waypoints[i][0], 'y': waypoints[i][1],
+                'dir': direction,
+            })
 
-            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
+            if 0 < i < num_wps - 1:
+                barrier_indices.append(len(path) - 1)
 
-            # P0 = pre_turn (弧線起始)
-            p0_x = waypoints[i][0] - d_in_x * T
-            p0_y = waypoints[i][1] - d_in_y * T
+        path[0]['s'] = 0.0
+        for i in range(1, len(path)):
+            prev = path[i - 1]
+            cur = path[i]
+            cur['s'] = prev['s'] + math.hypot(
+                cur['x'] - prev['x'], cur['y'] - prev['y']
+            )
 
-            # 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
+        return path, barrier_indices
 
     # ------------------------------------------------------------------ 柵狀掃描
 
@@ -474,7 +478,15 @@ class FormationPlanner:
 
             all_waypoints[original_idx] = waypoints_list
 
-        return all_waypoints
+        # 每一條掃描線的「起點」都做一次同步：
+        #   wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
+        #   wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
+        # 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始）
+        # 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致），
+        # 所以用同一份 rendezvous_indices
+        rendezvous_indices = [2 * i + 1 for i in range(num_lines)]
+
+        return all_waypoints, rendezvous_indices
 
 
 # ================================================================================
@@ -492,12 +504,12 @@ if __name__ == "__main__":
     target = (24.12400, 120.56795, 10.0)
 
     # M-Formation
-    wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+    wps, origin, rv = 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 轉彎)
+    # Leader-Follower (虛擬領隊 / 弧長參數化)
     route = [
         (24.12345, 120.56780),
         (24.12370, 120.56800),
@@ -505,18 +517,18 @@ if __name__ == "__main__":
         (24.12400, 120.56800),
         (24.12420, 120.56790),
     ]
-    wps_lf, origin_lf = planner.plan_formation_mission(
+    wps_lf, origin_lf, rv_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,
+            'min_inner_radius': 3.0,
+            'arc_resolution': 8,
             'altitude': 10.0
         }
     )
-    print(f"\nLeader-Follower (Bezier turns):")
+    print(f"\nLeader-Follower (arc-length virtual leader):")
     for i, wp_list in enumerate(wps_lf):
         print(f"  Drone {i}: {len(wp_list)} waypoints")
         for j, wp in enumerate(wp_list):
@@ -529,7 +541,7 @@ if __name__ == "__main__":
         (24.1240, 120.5683),
         (24.1240, 120.5679)
     ]
-    wps2, origin2 = planner.plan_formation_mission(
+    wps2, origin2, rv2 = planner.plan_formation_mission(
         drones, target, MissionType.GRID_SWEEP,
         params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
     )

src/GUI/overview_table.py

@@ -6,8 +6,9 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向",
-                          "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
+                          "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
+                          "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {
         "mode": 0,
@@ -15,20 +16,24 @@ class OverviewTable(QTableWidget):
         "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
+        "fix_type": 5,
+        "satellites_visible": 6,
+        "eph": 7,
+        "epv": 8,
+        "altitude": 9,
+        "local": 10,
+        "velocity": 11,
+        "groundspeed": 12,
+        "heading": 13,
+        "airspeed": 14,
+        "throttle": 15,
+        "hud_alt": 16,
+        "climb": 17,
+        "roll": 18,
+        "pitch": 19,
+        "yaw": 20,
+        "loss_rate": 21,
+        "ping": 22
     }
     
     def __init__(self, info_types=None, info_type_map=None, parent=None):

src/GUI/validation/test_mission.py

@@ -210,7 +210,7 @@ def main():
         target_gps = (target_lat, target_lon, BASE_ALTITUDE)
         print(f"  目標點: ({target_lat:.6f}, {target_lon:.6f})")
 
-        waypoints_per_drone, origin = planner.plan_formation_mission(
+        waypoints_per_drone, origin, _ = planner.plan_formation_mission(
             drone_gps_positions, target_gps, MissionType.M_FORMATION
         )
 
@@ -232,7 +232,7 @@ def main():
         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(
+        waypoints_per_drone, origin, _ = planner.plan_formation_mission(
             drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
             params={
                 'rect_corners': rect_corners,

src/GUI/validation/verify_waypoints.py

@@ -365,7 +365,7 @@ def visualize_standalone():
     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)
+    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))],
@@ -380,7 +380,7 @@ def visualize_standalone():
     # 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(
+    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}
     )
@@ -403,7 +403,7 @@ def visualize_standalone():
         (24.12410, 120.56800),
         (24.12420, 120.56790),
     ]
-    wps_lf, origin_lf = planner.plan_formation_mission(
+    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}

src/fc_interfaces/CMakeLists.txt

@@ -8,10 +8,18 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/AttitudeRaw.msg"
+  "msg/GnssRaw.msg"
+  "msg/SystemDiagnosticsRaw.msg"
+  "msg/ServiceAckResult.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
+  "srv/MavPositionTargetGlobalInt.srv"
+  DEPENDENCIES std_msgs builtin_interfaces
   )
 
 ament_package()

src/fc_interfaces/msg/AttitudeRaw.msg

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

src/fc_interfaces/msg/GnssRaw.msg

@@ -0,0 +1,8 @@
+builtin_interfaces/Time stamp
+float64 latitude
+float64 longitude
+float64 altitude
+uint8 fix_type
+uint8 satellites_visible
+float32 eph
+float32 epv

src/fc_interfaces/msg/ServiceAckResult.msg

@@ -0,0 +1,43 @@
+# ServiceAckResult.msg
+# ACK code definition for command-like service responses.
+# 這邊只是定義常數的檔案
+#
+# Design:
+# - 0~10   : MAVLink MAV_RESULT compatible range (autopilot/firmware ACK) 大多不會用到 直接吃飛控回傳值
+# - 50~89  : System-level ACK codes (local bridge/orchestrator status)
+# - 90~99  : Unknown/reserved fallback
+#
+# Rule:
+# - If FC ACK is received, return MAV_RESULT_* code.
+# - If FC ACK is not reachable / local pipeline failed, return SYS_* code.
+
+# ---- MAVLink MAV_RESULT (standard) ---- 詳細請自行去網路找 mavlink MAV_RESULT 
+uint8 MAV_RESULT_ACCEPTED=0             # 封包正常接受並執行
+uint8 MAV_RESULT_TEMPORARILY_REJECTED=1 # 封包正常接受 但是飛控很忙 不執行 需要等待後重試
+uint8 MAV_RESULT_DENIED=2               # 封包不正常 參數可能給錯了
+uint8 MAV_RESULT_UNSUPPORTED=3          # 封包不正常 根本認不出是啥
+uint8 MAV_RESULT_FAILED=4               # 封包正常接受 但是載具的狀態異常 不執行
+uint8 MAV_RESULT_IN_PROGRESS=5
+uint8 MAV_RESULT_CANCELLED=6
+uint8 MAV_RESULT_COMMAND_LONG_ONLY=7
+uint8 MAV_RESULT_COMMAND_INT_ONLY=8
+uint8 MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME=9
+uint8 MAV_RESULT_NOT_IN_CONTROL=10
+
+# ---- System-level result codes (custom) ----
+# 50 : 有接到預期的封包 但是訊息建議再確認
+# 51 : some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair 
+# 
+# 52 : mavlink communication timeout 可能是通訊丟包了
+# 53 : there already has a command being execution for this sysid, try later
+# 54 : this sysid is not found
+
+uint8 FCSYS_GET_EXPECTED_FEEDBACK=50
+uint8 FCSYS_NO_EXPECTED_FEEDBACK=51
+uint8 FCSYS_MAVLINK_TIMEOUT=52
+uint8 FCSYS_BUSY=53
+uint8 FCSYS_DEVICE_NOT_FOUND=54
+
+
+# ---- Fallback ----
+uint8 FCSYS_UNKNOWN=90

src/fc_interfaces/msg/SystemDiagnosticsRaw.msg

@@ -0,0 +1,11 @@
+builtin_interfaces/Time stamp
+uint32 sensors_install_mask
+uint32 sensors_enabled_mask
+uint32 sensors_health_mask
+uint16 mcu_load
+uint16 bus_error_rate
+uint16 bus_error_count
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4

src/fc_interfaces/package.xml

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

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/fc_network_adapter.md

@@ -6,6 +6,7 @@
 # 開發此專案的注意事項
 - 預設 autopilot 的 component id = 1
 - 不允許 system id 重複
+- 預設同一載具僅會使用相同的 socket
 - 增加一個固定數值監控然後要到 ros2 topic 
   - mavlinkROS2Node.py 檔案內 
     - PublishRateController.topic_intervals 建立
@@ -36,6 +37,7 @@
 - *self.bridge* 存放 mavlink_bridge 實例 
 - *self.plumber* 存放 serial_manager 實例
 - *self.vehicle_registry* 存放 vehicle_registry 實例
+- *self.fc_ros_manager* 存放 fc_ros_manager 實例 
 
 - *self.panel_thread* 面板的執行緒
 - *self.panelState* 暫存面板與調配者互動的資料流動區
@@ -126,6 +128,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 +173,61 @@
 ## mavlinkVehicleView.py
     這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用
 
+## mavlinkROS2Nodes.py
+    這裡支持了所有 ROS2 框架的對應功能 包涵 
+    1. 將載具的串流更新的狀態更新到對應的 topic 
+    2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
+    3. 訂閱外部 RTCM 資料並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
+
+### **[Class]** fc_ros_manager
+    MAVLink ROS2 節點管理器 管理三個獨立的 Node
+    會開一個執行緒 讓三個 Node 都跑在裡面
+    然後用 spin_once 保持 Node 的活性
+- *self.status_publisher* 物件實例
+- *self.command_service* 物件實例
+- *self.rtcm_relay* 物件實例
+- *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 的具體方法
+
+### **[Class]** RtcmRelay
+    訂閱 /fc_network/rtcm_in (mavros_msgs/msg/RTCM) 並轉發為 MAVLink GPS_RTCM_DATA
+    topic 的 QoS：BEST_EFFORT / KEEP_LAST depth=1 / VOLATILE（低延遲優先，丟舊包不排隊）
+    處理順序：過期丟棄(1s) → blake2s hash 去重 → 最短轉發間隔節流 → 分片轉發
+    分片：GPS_RTCM_DATA 每片 180 bytes，超過自動分片，最多 4 片 (720 bytes)，超長直接丟棄
+    失敗語意：不做舊包重送，送失敗就放棄等下一包
+- *self.counters* 觀測用計數器 (received/forwarded/dropped_stale/dropped_duplicate/dropped_throttle/dropped_oversize/no_route/send_fail)
+- *self.clock_offset_sec* 與 RTCM 來源的時鐘偏移量（秒）
+---
+- *_rtcm_callback()* 收到 RTCM 封包時的處理流程
+- *_send_rtcm()* 對單一載具編碼並送出 GPS_RTCM_DATA（含分片邏輯）
+- *set_clock_offset()* 設定時鐘偏移量
+- *get_counters()* 取得觀測計數器
+
+
 # 開發記錄
 
 ## 已實現功能
@@ -182,9 +240,16 @@
 7. 終端機介面控制
 8. 基礎載具流量觀測
 9. 載具狀態收集與彙整
-10. a. ros2 topic 應用開發介面
+10. a. ros2 topic 應用開發介面 (基礎框架)
+        - GNSS (alt lan fix_type epv eph)
+        - attitude position_ned (pitch yaw row local_position)
+        - battery
+    b. ros2 service 應用開發介面 (基礎框架)
+    c. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
+11. ros2 topic service rtcm Node 重啟功能
+12. 可以定義 ROS_DOMAIN_ID
+
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令
-5-2. 模組化 serial 連線機制 以利後期擴容其他模組
-10. a. ros2  應用開發介面
+5-2. 模組化 serial 連線機制 以利後期擴容其他模組

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -28,9 +28,6 @@ mavlink_bridge:
     專門處理 stream_bridge_ring 裡面的訊息流
     會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖
 
-
-
-
 '''
 
 # 基礎功能的 import
@@ -50,17 +47,19 @@ from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
-    vehicle_registry,
-    VehicleView,
+    vehicle_registry, # 儲存全部物件的地方
+    VehicleView, # 代表每台載具的最外層
     VehicleComponent,
     ComponentType,
-    ConnectionType
+    StatusTextEntry,
 )
 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)
@@ -109,12 +108,15 @@ class mavlink_bridge:
     def _init_message_handlers(self):
         """初始化訊息處理器映射表，提高處理效率"""
         self.message_handlers = {
-            0: self._handle_heartbeat,        # HEARTBEAT
-            30: self._handle_attitude,        # ATTITUDE
-            32: self._handle_local_position,  # LOCAL_POSITION_NED
-            33: self._handle_global_position, # GLOBAL_POSITION_INT
-            74: self._handle_vfr_hud,         # VFR_HUD
-            147: self._handle_battery_status, # BATTERY_STATUS
+            0: self._handle_heartbeat,          # HEARTBEAT
+            1: self._handle_vehicle_sys_status, # SYS_STATUS
+            24: self._handle_gps_raw_int,       # GPS_RAW_INT
+            30: self._handle_attitude,          # ATTITUDE
+            32: self._handle_local_position,    # LOCAL_POSITION_NED
+            33: self._handle_global_position,   # GLOBAL_POSITION_INT
+            74: self._handle_vfr_hud,           # VFR_HUD
+            147: self._handle_battery_status,   # BATTERY_STATUS
+            253: self._handle_status_text,      # STATUSTEXT
         }
 
     def start(self):
@@ -231,6 +233,30 @@ class mavlink_bridge:
             'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
         }
 
+    def _handle_vehicle_sys_status(self, vehicle, component, msg, timestamp):
+        """處理 SYS_STATUS 訊息 (msg_id: 1)"""
+        diag = component.status.sys_diag
+        diag.sensors_install_mask = msg.onboard_control_sensors_present
+        diag.sensors_enabled_mask = msg.onboard_control_sensors_enabled
+        diag.sensors_health_mask = msg.onboard_control_sensors_health
+        diag.mcuLoad = msg.load
+        diag.busErrorRate = msg.drop_rate_comm
+        diag.busErrorCount = msg.errors_comm
+        diag.errors_count1 = msg.errors_count1
+        diag.errors_count2 = msg.errors_count2
+        diag.errors_count3 = msg.errors_count3
+        diag.errors_count4 = msg.errors_count4
+        diag.timestamp = timestamp
+
+    def _handle_status_text(self, vehicle, component, msg, timestamp):
+        """處理 STATUSTEXT 訊息 (msg_id: 253)"""
+        text = msg.text.rstrip('\x00') if msg.text else ''
+        if not text:
+            return
+        component.status.status_text_queue.append(
+            StatusTextEntry(text=text, severity=msg.severity, timestamp=timestamp)
+        )
+
     def _handle_global_position(self, vehicle, component, msg, timestamp):
         """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
         component.status.position.latitude = msg.lat / 1e7  # 轉換為度
@@ -239,6 +265,14 @@ class mavlink_bridge:
         component.status.position.relative_altitude = msg.relative_alt / 1000.0  # 轉換為公尺
         component.status.position.timestamp = timestamp
 
+    def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
+        """處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
+        component.status.gps.fix_type = msg.fix_type
+        component.status.gps.satellites_visible = msg.satellites_visible
+        component.status.gps.eph = None if msg.eph == 65535 else msg.eph
+        component.status.gps.epv = None if msg.epv == 65535 else msg.epv
+        component.status.gps.timestamp = timestamp
+
     def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
         """處理 VFR_HUD 訊息 (msg_id: 74)"""
         component.status.vfr.airspeed = msg.airspeed
@@ -386,7 +420,9 @@ 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
+        # 0 HEARTBEAT, 1 SYS_STATUS, 24 GPS_RAW_INT, 30 ATTITUDE, 
+        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 253 STATUSTEXT
+        self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147, 253])  
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表
@@ -823,5 +859,8 @@ if __name__ == '__main__':
 1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects
 2. async_io_manager 的 _stop_event 無效變數移除
 
+2026年 06月 10日
+1. 增加 SYS_STATUS 與 STATUSTEXT 訊息的處理機制
+
 '''
 

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,14 +5,15 @@ VehicleView - Pure State Container
 """
 
 import os
-from typing import Dict, Optional, Any
+from collections import deque
+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.10"
 
 # ====================== Enums =====================
 
@@ -117,6 +118,30 @@ class VFR:
     timestamp: Optional[float] = None  # 時間戳記
 
 
+@dataclass
+class StatusTextEntry:
+    """飛控狀態文字（來源：MAVLink STATUSTEXT）"""
+    text: str
+    severity: Optional[int] = None
+    timestamp: Optional[float] = None
+
+
+@dataclass
+class SystemDiagnostics:
+    """系統診斷資訊（來源：MAVLink SYS_STATUS，不含電池欄位）"""
+    sensors_install_mask: Optional[int] = None
+    sensors_enabled_mask: Optional[int] = None
+    sensors_health_mask: Optional[int] = None
+    mcuLoad: Optional[int] = None          # 單位 1%
+    busErrorRate: Optional[int] = None     # 單位 0.1%
+    busErrorCount: Optional[int] = None
+    errors_count1: Optional[int] = None
+    errors_count2: Optional[int] = None
+    errors_count3: Optional[int] = None
+    errors_count4: Optional[int] = None
+    timestamp: Optional[float] = None
+
+
 @dataclass
 class ComponentStatus:
     """組件狀態容器"""
@@ -127,6 +152,8 @@ class ComponentStatus:
     ekf: EKF = field(default_factory=EKF)
     gps: GPS = field(default_factory=GPS)
     vfr: VFR = field(default_factory=VFR)
+    sys_diag: SystemDiagnostics = field(default_factory=SystemDiagnostics)
+    status_text_queue: deque = field(default_factory=lambda: deque(maxlen=64))
     
     # 系統狀態
     system_status: Optional[int] = None  # MAV_STATE
@@ -160,6 +187,7 @@ class RFStatus:
 @dataclass
 class SocketInfo:
     """Socket連接資訊"""
+    src64_addr: Optional[bytes] = None # 模組的物理定址
     ip: Optional[str] = None  # IP位址
     port: Optional[int] = None  # 埠號
     local_ip: Optional[str] = None  # 本地IP
@@ -283,6 +311,7 @@ class RFModule:
 
 class VehicleView:
     """
+    最上層
     載具視圖 - 純狀態容器
     
     特點:
@@ -402,11 +431,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 +454,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 +462,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 +498,8 @@ vehicle_registry = VehicleViewRegistry()
 2026.01.16
 1. 新增 重置指定組件的封包統計 功能
 
+2026.04.13
+1. 新增 VehicleViewRegistry.read_all 方法 讓 ROS2 Node 組件更有效率的讀取其中的資料
+
 '''
 

src/fc_network_adapter/fc_network_adapter/utils/pollStrategy.py

@@ -0,0 +1,48 @@
+"""
+POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
+
+不依賴 serialManager，僅接受裝置快照與排程狀態。
+"""
+
+from dataclasses import dataclass
+from typing import List
+
+
+@dataclass(frozen=True)
+class PollDevice:
+    """從 esp32AddrMapping 抽出的唯讀快照。"""
+    address_64: bytes
+    # system_id: int
+    remain_bytes: int
+    last_done_time: float
+
+
+@dataclass
+class PollSchedulerState:
+    """每條 serial link 一份，保存 round-robin 索引。"""
+    round_robin_index: int = 0
+
+
+def pick_next(
+    devices: List[PollDevice],
+    scheduler_state: PollSchedulerState
+):
+    """
+    選下一個 POLL 目標。
+
+    回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
+    """
+    if not devices:
+        return None, 0
+
+    device_count = len(devices)
+    selected_index = scheduler_state.round_robin_index % device_count
+    selected_device = devices[selected_index]
+    scheduler_state.round_robin_index += 1
+
+    grant_bytes = 0
+    if selected_device.remain_bytes > 0:
+        grant_bytes = min( 65535, max(0, selected_device.remain_bytes))
+
+    return selected_device.address_64, grant_bytes
+

src/fc_network_adapter/package.xml

@@ -7,6 +7,13 @@
   <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
   <license>TODO: License declaration</license>
 
+  <exec_depend>fc_interfaces</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>mavros_msgs</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -17,6 +17,7 @@ import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
 import mavros_msgs.msg
+import fc_interfaces.msg
 
 # 專案 imports
 from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -39,7 +40,7 @@ class TestSubscriber(Node):
         
         self.sysid = sysid
         self.received_messages = {
-            'position': [],
+            'position_gnss': [],
             'attitude': [],
             'velocity': [],
             'battery': [],
@@ -60,9 +61,9 @@ class TestSubscriber(Node):
         
         # Position
         self.create_subscription(
-            sensor_msgs.msg.NavSatFix,
-            f'{base_topic}/position',
-            lambda msg: self._on_message('position', msg),
+            fc_interfaces.msg.GnssRaw,
+            f'{base_topic}/position_gnss',
+            lambda msg: self._on_message('position_gnss', msg),
             10
         )
         
@@ -121,7 +122,7 @@ class TestSubscriber(Node):
     
     def _format_msg(self, topic_name: str, msg) -> str:
         """格式化消息以便顯示"""
-        if topic_name == 'position':
+        if topic_name == 'position_gnss':
             return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
         elif topic_name == 'attitude':
             return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -264,7 +265,7 @@ def test_basic_publishing():
     # 檢查收到的消息
     print("\n--- 消息統計 ---")
     total_messages = 0
-    for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
         count = test_sub.get_message_count(topic)
         total_messages += count
         print(f"  {topic:15s}: {count:3d} 條消息")
@@ -304,7 +305,7 @@ def test_frequency_control():
     # 修改頻率設定
     publisher_node = ros2_manager.status_publisher
     publisher_node.rate_controller.topic_intervals = {
-        'position': 1.5,   
+        'position_gnss': 1.5,   
         'attitude': 1.0,    
         'velocity': 1.0,     
         'battery': 1.0,   
@@ -329,7 +330,7 @@ def test_frequency_control():
     print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
     
     print("2Hz Topics (預期 ~6 條):")
-    for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
     for topic in ['battery', 'mode', 'summary']:

src/fc_network_apps/__init__.py

@@ -1,14 +1,10 @@
-from .longCommand import CommandLongClient, ChangeModeResult
-from .changeMode import change_mode
-from .arm_disarm import arm_disarm
-from .takeoff import takeoff
-from .land import land
+from .longCommand import CommandLongClient, CommandLongResult
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
+
 
 __all__ = [
     "CommandLongClient",
-    "ChangeModeResult",
-    "change_mode",
-    "arm_disarm",
-    "takeoff",
-    "land",
+    "PositionTargetGlobalIntClient",
+    "CommandLongResult",
+    "PositionTargetGlobalIntResult",
 ]

src/fc_network_apps/arm_disarm.py

@@ -1,93 +0,0 @@
-"""Simple wrapper for arm/disarm via fc_network ROS2 service (MAV_CMD_COMPONENT_ARM_DISARM)."""
-
-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_COMPONENT_ARM_DISARM = 400
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class ArmDisarmResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def arm_disarm(
-    *,
-    target_sysid: int,
-    arm: bool,
-    target_compid: int = 0,
-    confirmation: int = 0,
-    param2: float = 0.0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> ArmDisarmResult:
-    """One-shot MAV_CMD_COMPONENT_ARM_DISARM (400) wrapper.
-
-    param1: 1.0 to arm, 0.0 to disarm.
-    param2: usually 0. Some stacks use 21196 for force-arm (ArduPilot); pass via param2 if needed.
-    """
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_arm_disarm_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return ArmDisarmResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_COMPONENT_ARM_DISARM
-        req.confirmation = confirmation
-        req.param1 = 1.0 if arm else 0.0
-        req.param2 = float(param2)
-        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 = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return ArmDisarmResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return ArmDisarmResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    r = arm_disarm(target_sysid=3, arm=True)
-    print(
-        f"arm_disarm success={r.success}, "
-        f"ack_result={r.ack_result}, message='{r.message}'"
-    )

src/fc_network_apps/changeMode.py

@@ -1,99 +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
-
-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[Node] = None
-    try:
-        node = Node("fc_change_mode_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return ChangeModeResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        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 = client.call_async(req)
-        rclpy.spin_until_future_complete(node, 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,
-        )
-    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/land.py

@@ -1,90 +0,0 @@
-"""Simple wrapper for land via fc_network ROS2 service."""
-
-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_NAV_LAND = 21
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class LandResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def land(
-    *,
-    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,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> LandResult:
-    """One-shot MAV_CMD_NAV_LAND wrapper."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_land_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return LandResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_NAV_LAND
-        req.confirmation = 0
-        req.param1 = 0.0
-        req.param2 = 0.0
-        req.param3 = 0.0
-        req.param4 = float(yaw_deg)
-        req.param5 = float(latitude) if latitude is not None else 0.0
-        req.param6 = float(longitude) if longitude is not None else 0.0
-        req.param7 = float(altitude_m)
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return LandResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return LandResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    result = land(target_sysid=3)
-    print(
-        f"land success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/fc_network_apps/longCommand.py

@@ -1,8 +1,10 @@
+"""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
@@ -10,30 +12,83 @@ 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_PREFLIGHT_REBOOT_SHUTDOWN = 246
+COMMAND_COMPONENT_ARM_DISARM = 400
+
 DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
 DEFAULT_TIMEOUT_SEC = 2.0
 
 
 @dataclass
-class ChangeModeResult:
-    """Return value for mode change requests."""
-
+class CommandLongResult:
     success: bool
     message: str
     ack_result: int
 
-
 class CommandLongClient(Node):
-    """Small ROS2 client dedicated to change flight mode."""
+    """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
 
-    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
-        rclpy.init()
-        super().__init__("fc_change_mode_client")
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        # 使用提供的 node_name，或使用帶時間戳的預設名稱以避免名稱衝突
+        if node_name is None:
+            import time
+            node_name = f"fc_command_long_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
         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,
         *,
@@ -42,34 +97,331 @@ class CommandLongClient(Node):
         target_compid: int = 0,
         base_mode: float = 1.0,
         confirmation: int = 0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC ) -> ChangeModeResult:
+        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,
+        )
+
+    def reboot_autopilot(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
+            confirmation=0,
+            param1=1.0,
+            param2=0.0,
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    # ============================================================================
+    # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
+    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
+    # ============================================================================
+
+    # ============================================================================
+    # 【重新實現】正確的非阻塞 async 方法（不使用 ThreadPoolExecutor）
+    # 使用 asyncio 的 polling 機制，避免在線程中調用 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_command_long_async(
+        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:
+        """非阻塞的 async 版本 - 使用 asyncio polling 而非 spin"""
         req = MavCommandLong.Request()
         req.target_sysid = target_sysid
         req.target_compid = target_compid
-        req.command = COMMAND_DO_SET_MODE
+        req.command = command
         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.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 ChangeModeResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
+        
+        # 使用 asyncio.sleep 進行 polling，而非 spin_until_future_complete
+        # 使用 10ms 輪詢間隔以匹配 GUI 執行器的 spin_once() 頻率
+        timeout = timeout_sec + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+        
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    response = future.result()
+                    if response is None:
+                        return CommandLongResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    return CommandLongResult(
+                        success=response.success,
+                        message=response.message,
+                        ack_result=response.ack_result,
+                    )
+                except Exception as e:
+                    return CommandLongResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+            
+            # 讓出控制權給事件循環，允許 GUI executor 執行
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+        
+        return CommandLongResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
 
-        response = future.result()
-        return ChangeModeResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
+    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"""
+        return await self._send_command_long_async(
+            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,
+        )
+
+    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"""
+        return await self._send_command_long_async(
+            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 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"""
+        return await self._send_command_long_async(
+            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,
+        )
+
+    async def reboot_autopilot_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 autopilot reboot."""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
+            confirmation=0,
+            param1=1.0,
+            param2=0.0,
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            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"""
+        return await self._send_command_long_async(
+            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,
+        )

src/fc_network_apps/navigation.py

@@ -0,0 +1,418 @@
+"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
+
+from __future__ import annotations
+
+import asyncio
+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, node_name: str = None) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        if node_name is None:
+            import time
+            node_name = f"fc_position_target_global_int_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
+        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,
+        )
+
+    # ============================================================================
+    # 非阻塞 async 版本（asyncio polling，對齊 longCommand.py 的模板）
+    # 避免在 Qt 事件迴圈或主執行緒中呼叫 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_position_target_global_int_async(
+        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:
+        """非阻塞 async 版本 - 使用 asyncio polling 而非 spin"""
+        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)
+
+        timeout = float(timeout_sec) + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    resp = future.result()
+                    if resp is None:
+                        return PositionTargetGlobalIntResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    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,
+                    )
+                    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),
+                    )
+                except Exception as e:
+                    return PositionTargetGlobalIntResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+
+        return PositionTargetGlobalIntResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
+
+    async def goto_position_only_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        latitude_deg: float,
+        longitude_deg: float,
+        alt: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> PositionTargetGlobalIntResult:
+        """非阻塞 async 版本的 goto_position_only"""
+        lat_int = int(round(latitude_deg * 1e7))
+        lon_int = int(round(longitude_deg * 1e7))
+
+        return await self._send_position_target_global_int_async(
+            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/fc_network_apps/takeoff.py

@@ -1,91 +0,0 @@
-"""Simple wrapper for takeoff via fc_network ROS2 service."""
-
-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_NAV_TAKEOFF = 22
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class TakeoffResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def takeoff(
-    *,
-    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,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> TakeoffResult:
-    """One-shot MAV_CMD_NAV_TAKEOFF wrapper."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_takeoff_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return TakeoffResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_NAV_TAKEOFF
-        req.confirmation = 0
-        req.param1 = float(min_pitch_deg)
-        req.param2 = 0.0
-        req.param3 = 0.0
-        req.param4 = float(yaw_deg)
-        req.param5 = float(latitude) if latitude is not None else 0.0
-        req.param6 = float(longitude) if longitude is not None else 0.0
-        req.param7 = float(altitude_m)
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return TakeoffResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return TakeoffResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    result = takeoff(target_sysid=3, altitude_m=10.0)
-    print(
-        f"takeoff success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/fc_network_apps/vehicleDiagShow.py

@@ -0,0 +1,544 @@
+#!/usr/bin/env python3
+"""Standalone curses TUI for fc_network vehicle diagnostics (sys_diags, status_text, summary)."""
+
+from __future__ import annotations
+
+import curses
+import json
+import re
+import sys
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+from fc_interfaces.msg import SystemDiagnosticsRaw
+
+VEHICLE_TOPIC_RE = re.compile(
+    r'^/fc_network/vehicle/(sys\d+)/(sys_diags|status_text|summary)$'
+)
+STATUS_TEXT_RE = re.compile(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$')
+
+STATUS_TEXT_TTL_SEC = 30.0
+STALE_SEC = 3.0
+RESCAN_SEC = 2.0
+OFFLINE_SEC = 10.0
+DRAW_HZ = 5.0
+
+# MAV_SYS_STATUS_SENSOR bit labels (MAVLink common)
+SENSOR_BITS: Tuple[Tuple[int, str], ...] = (
+    (268435456, 'PreArm'),
+    (1, 'Gyro'),
+    (2, 'Accel'),
+    (4, 'Mag'),
+    (8, 'Baro'),
+    (16, 'DiffPress'),
+    (32, 'GPS'),
+    (64, 'OptFlow'),
+    (128, 'Vision'),
+    (256, 'Laser'),
+    (512, 'ExtGT'),
+    (1024, 'AngRate'),
+    (2048, 'AttStab'),
+    (4096, 'YawPos'),
+    (8192, 'AltCtrl'),
+    (16384, 'XYCtrl'),
+    (32768, 'Motor'),
+    (65536, 'RC'),
+    (131072, 'Gyro2'),
+    (262144, 'Accel2'),
+    (524288, 'Mag2'),
+    (1048576, 'Geofence'),
+    (2097152, 'AHRS'),
+    (4194304, 'Terrain'),
+    (8388608, 'RevMotor'),
+    (16777216, 'Log'),
+    (33554432, 'Batt'),
+    (67108864, 'Prox'),
+    (134217728, 'Satcom'),
+    (536870912, 'ObsAvoid'),
+    (1073741824, 'Propulsion'),
+)
+
+MAV_STATE_LABELS = {
+    0: 'UNINIT',
+    1: 'BOOT',
+    2: 'CAL',
+    3: 'STANDBY',
+    4: 'ACTIVE',
+    5: 'CRITICAL',
+    6: 'EMERGENCY',
+    7: 'OFF',
+    8: 'TERM',
+}
+
+SEVERITY_LABELS = {
+    0: 'EMERG',
+    1: 'ALERT',
+    2: 'CRIT',
+    3: 'ERROR',
+    4: 'WARN',
+    5: 'NOTICE',
+    6: 'INFO',
+    7: 'DEBUG',
+}
+
+
+@dataclass
+class StatusTextEntry:
+    received_at: float
+    severity: int
+    text: str
+    raw: str
+    sys_key: str
+
+
+@dataclass
+class VehicleDiagState:
+    sys_key: str
+    sysid: int
+    summary: Optional[dict] = None
+    diag: Optional[SystemDiagnosticsRaw] = None
+    status_entries: List[StatusTextEntry] = field(default_factory=list)
+    last_summary_ts: float = 0.0
+    last_diag_ts: float = 0.0
+    last_topic_seen_ts: float = 0.0
+    offline: bool = False
+
+
+def sys_key_to_id(sys_key: str) -> int:
+    return int(sys_key.replace('sys', '', 1))
+
+
+def purge_status_entries(entries: List[StatusTextEntry], now: float, ttl: float = STATUS_TEXT_TTL_SEC) -> None:
+    cutoff = now - ttl
+    entries[:] = [e for e in entries if e.received_at >= cutoff]
+
+
+def parse_status_text(raw: str, sys_key: str, received_at: float) -> StatusTextEntry:
+    match = STATUS_TEXT_RE.match(raw.strip())
+    if match:
+        severity = int(match.group(2))
+        text = match.group(3)
+    else:
+        severity = -1
+        text = raw
+    return StatusTextEntry(
+        received_at=received_at,
+        severity=severity,
+        text=text,
+        raw=raw,
+        sys_key=sys_key,
+    )
+
+
+def decode_sensors(install: int, enabled: int, health: int) -> List[Tuple[str, str]]:
+    """Return list of (label, status) where status is OK / FAIL / OFF."""
+    items: List[Tuple[str, str]] = []
+    for bit, label in SENSOR_BITS:
+        if not (install & bit):
+            continue
+        if not (enabled & bit):
+            items.append((label, 'OFF'))
+        elif health & bit:
+            items.append((label, 'OK'))
+        else:
+            items.append((label, 'FAIL'))
+    return items
+
+
+def format_age(seconds: float) -> str:
+    if seconds < 0:
+        return 'never'
+    if seconds < 1.0:
+        return f'{seconds * 1000:.0f}ms'
+    if seconds < 60.0:
+        return f'{seconds:.1f}s'
+    return f'{seconds / 60:.1f}m'
+
+
+def truncate(text: str, width: int) -> str:
+    if width <= 0:
+        return ''
+    if len(text) <= width:
+        return text
+    if width <= 3:
+        return text[:width]
+    return text[: width - 3] + '...'
+
+
+class VehicleDiagNode(Node):
+    """ROS2 node: discover vehicles and subscribe to diagnostic topics."""
+
+    def __init__(self) -> None:
+        super().__init__('vehicle_diag_show')
+        self._lock = threading.Lock()
+        self._vehicles: Dict[str, VehicleDiagState] = {}
+        self._subs: Dict[str, Dict[str, object]] = {}
+        self._scan_timer = self.create_timer(RESCAN_SEC, self._rescan_topics)
+        self._rescan_topics()
+
+    def snapshot(self) -> Dict[str, VehicleDiagState]:
+        now = time.monotonic()
+        with self._lock:
+            for state in self._vehicles.values():
+                purge_status_entries(state.status_entries, now)
+                if state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+            return {k: state for k, state in self._vehicles.items()}
+
+    def _get_or_create_state(self, sys_key: str) -> VehicleDiagState:
+        if sys_key not in self._vehicles:
+            self._vehicles[sys_key] = VehicleDiagState(
+                sys_key=sys_key,
+                sysid=sys_key_to_id(sys_key),
+            )
+        return self._vehicles[sys_key]
+
+    def _rescan_topics(self) -> None:
+        now = time.monotonic()
+        found: set = set()
+        for topic_name, _ in self.get_topic_names_and_types():
+            match = VEHICLE_TOPIC_RE.match(topic_name)
+            if not match:
+                continue
+            sys_key = match.group(1)
+            found.add(sys_key)
+            with self._lock:
+                state = self._get_or_create_state(sys_key)
+                state.last_topic_seen_ts = now
+                state.offline = False
+            self._ensure_subscriptions(sys_key)
+
+        with self._lock:
+            for sys_key, state in self._vehicles.items():
+                if sys_key not in found and state.last_topic_seen_ts > 0:
+                    state.offline = (now - state.last_topic_seen_ts) > OFFLINE_SEC
+
+    def _ensure_subscriptions(self, sys_key: str) -> None:
+        if sys_key in self._subs:
+            return
+        base = f'/fc_network/vehicle/{sys_key}'
+        subs = {
+            'summary': self.create_subscription(
+                String,
+                f'{base}/summary',
+                lambda msg, sk=sys_key: self._on_summary(sk, msg),
+                10,
+            ),
+            'sys_diags': self.create_subscription(
+                SystemDiagnosticsRaw,
+                f'{base}/sys_diags',
+                lambda msg, sk=sys_key: self._on_sys_diags(sk, msg),
+                10,
+            ),
+            'status_text': self.create_subscription(
+                String,
+                f'{base}/status_text',
+                lambda msg, sk=sys_key: self._on_status_text(sk, msg),
+                10,
+            ),
+        }
+        with self._lock:
+            self._subs[sys_key] = subs
+
+    def _on_summary(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        try:
+            summary = json.loads(msg.data)
+        except json.JSONDecodeError:
+            summary = {'raw': msg.data}
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.summary = summary
+            state.last_summary_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_sys_diags(self, sys_key: str, msg: SystemDiagnosticsRaw) -> None:
+        now = time.monotonic()
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.diag = msg
+            state.last_diag_ts = now
+            state.last_topic_seen_ts = now
+
+    def _on_status_text(self, sys_key: str, msg: String) -> None:
+        now = time.monotonic()
+        entry = parse_status_text(msg.data, sys_key, now)
+        with self._lock:
+            state = self._get_or_create_state(sys_key)
+            state.status_entries.append(entry)
+            purge_status_entries(state.status_entries, now)
+            state.last_topic_seen_ts = now
+
+
+class DiagCursesApp:
+    """Curses front-end for vehicle diagnostics."""
+
+    def __init__(self, node: VehicleDiagNode) -> None:
+        self._node = node
+        self._running = True
+
+    def run(self) -> None:
+        curses.wrapper(self._main)
+
+    def _init_colors(self) -> None:
+        curses.start_color()
+        curses.use_default_colors()
+        curses.init_pair(1, curses.COLOR_GREEN, -1)    # OK / DISARMED / INFO
+        curses.init_pair(2, curses.COLOR_RED, -1)        # FAIL / ARMED / ERROR+
+        curses.init_pair(3, curses.COLOR_YELLOW, -1)   # WARN / OFF
+        curses.init_pair(4, curses.COLOR_CYAN, -1)     # header / INFO
+        curses.init_pair(5, curses.COLOR_WHITE, -1)    # dim / offline
+        curses.init_pair(6, curses.COLOR_MAGENTA, -1)  # NOTICE
+
+    def _main(self, stdscr) -> None:
+        curses.curs_set(0)
+        stdscr.nodelay(True)
+        stdscr.keypad(True)
+        self._init_colors()
+
+        while self._running and rclpy.ok():
+            try:
+                self._draw(stdscr)
+            except curses.error:
+                pass
+            key = stdscr.getch()
+            if key in (ord('q'), ord('Q'), 27):
+                self._running = False
+            time.sleep(1.0 / DRAW_HZ)
+
+    def _draw(self, stdscr) -> None:
+        stdscr.erase()
+        height, width = stdscr.getmaxyx()
+        now = time.monotonic()
+        vehicles = self._node.snapshot()
+        sorted_keys = sorted(vehicles.keys(), key=sys_key_to_id)
+
+        header = f' Vehicle Diagnostics Monitor  vehicles:{len(sorted_keys)}  Q:quit '
+        stdscr.attron(curses.color_pair(4) | curses.A_BOLD)
+        stdscr.addstr(0, 0, truncate(header.ljust(width - 1), width - 1))
+        stdscr.attroff(curses.color_pair(4) | curses.A_BOLD)
+
+        row = 1
+        if not sorted_keys:
+            msg = ' Waiting for vehicles... (need /fc_network/vehicle/sysN/* topics) '
+            if row < height - 1:
+                stdscr.addstr(row, max(0, (width - len(msg)) // 2), truncate(msg, width - 1))
+            row += 2
+
+        vehicle_rows_end = max(row, height - 8)
+        for sys_key in sorted_keys:
+            if row >= vehicle_rows_end:
+                break
+            state = vehicles[sys_key]
+            row = self._draw_vehicle_block(stdscr, row, width, state, now)
+            if row < height - 1:
+                try:
+                    stdscr.addstr(row, 0, truncate('-' * (width - 1), width - 1))
+                except curses.error:
+                    pass
+                row += 1
+
+        log_title = ' Status Text (last 30s) '
+        if row < height - 1:
+            stdscr.attron(curses.color_pair(4))
+            stdscr.addstr(row, 0, truncate(log_title.ljust(width - 1), width - 1))
+            stdscr.attroff(curses.color_pair(4))
+            row += 1
+
+        all_entries: List[StatusTextEntry] = []
+        for sys_key in sorted_keys:
+            all_entries.extend(vehicles[sys_key].status_entries)
+        all_entries.sort(key=lambda e: e.received_at)
+
+        log_rows = height - row - 1
+        if log_rows <= 0:
+            stdscr.refresh()
+            return
+
+        if not all_entries:
+            empty = ' (no messages in last 30s) '
+            if row < height - 1:
+                stdscr.addstr(row, 0, truncate(empty, width - 1))
+        else:
+            visible = all_entries[-log_rows:]
+            for entry in visible:
+                if row >= height - 1:
+                    break
+                self._draw_status_line(stdscr, row, width, entry)
+                row += 1
+
+        stdscr.refresh()
+
+    def _draw_vehicle_block(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        state: VehicleDiagState,
+        now: float,
+    ) -> int:
+        summary = state.summary or {}
+        mode = summary.get('mode_name', 'UNKNOWN')
+        armed = summary.get('armed', False)
+        conn = summary.get('connection_type', '?')
+        socket_id = summary.get('socket_id', -1)
+        mav_status = summary.get('mav_status', 0)
+        mav_label = MAV_STATE_LABELS.get(int(mav_status), str(mav_status))
+
+        last_update = max(state.last_summary_ts, state.last_diag_ts)
+        age_sec = (now - last_update) if last_update > 0 else -1.0
+        stale = age_sec >= 0 and age_sec > STALE_SEC
+
+        armed_str = 'ARMED' if armed else 'DISARMED'
+        flags = []
+        if state.offline:
+            flags.append('OFFLINE')
+        if stale and not state.offline:
+            flags.append('STALE')
+        flag_str = f"  [{' '.join(flags)}]" if flags else ''
+
+        line1 = (
+            f' {state.sys_key} (MAV {state.sysid})  {mode}  {armed_str}  '
+            f'{conn}  socket:{socket_id}  {mav_label}  '
+            f'updated {format_age(age_sec)}{flag_str}'
+        )
+        if row < stdscr.getmaxyx()[0] - 1:
+            if state.offline:
+                stdscr.attron(curses.color_pair(5))
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                stdscr.attroff(curses.color_pair(5))
+            else:
+                stdscr.addstr(row, 0, truncate(line1, width - 1))
+                armed_pos = line1.find(armed_str)
+                if armed_pos >= 0:
+                    pair = curses.color_pair(2) if armed else curses.color_pair(1)
+                    stdscr.attron(pair | curses.A_BOLD)
+                    stdscr.addstr(row, armed_pos, armed_str)
+                    stdscr.attroff(pair | curses.A_BOLD)
+        row += 1
+
+        if state.diag is not None and row < stdscr.getmaxyx()[0] - 1:
+            load_pct = state.diag.mcu_load / 10.0
+            drop_pct = state.diag.bus_error_rate / 10.0
+            line2 = f' MCU {load_pct:.1f}%  CommDrop {drop_pct:.1f}%'
+            stdscr.addstr(row, 0, truncate(line2, width - 1))
+            row += 1
+
+            sensors = decode_sensors(
+                state.diag.sensors_install_mask,
+                state.diag.sensors_enabled_mask,
+                state.diag.sensors_health_mask,
+            )
+            row = self._draw_sensor_line(stdscr, row, width, sensors)
+        elif row < stdscr.getmaxyx()[0] - 1:
+            stdscr.addstr(row, 0, truncate(' (no sys_diags yet)', width - 1))
+            row += 1
+
+        return row
+
+    def _draw_sensor_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        sensors: List[Tuple[str, str]],
+    ) -> int:
+        max_row, _ = stdscr.getmaxyx()
+        if not sensors:
+            if row < max_row - 1:
+                stdscr.addstr(row, 0, truncate(' Sensors: (none installed)', width - 1))
+            return row + 1
+
+        lines: List[List[Tuple[str, str]]] = [[]]
+        line_len = len(' Sensors: ')
+        for label, status in sensors:
+            token = f'{label}:{status}  '
+            if lines[-1] and line_len + len(token) >= width - 1:
+                lines.append([])
+                line_len = 2
+            lines[-1].append((label, status))
+            line_len += len(token)
+
+        for idx, line_items in enumerate(lines):
+            if row >= max_row - 1:
+                break
+            prefix = ' Sensors: ' if idx == 0 else '  '
+            col = 0
+            stdscr.addstr(row, col, prefix)
+            col += len(prefix)
+            for label, status in line_items:
+                token = f'{label}:{status}  '
+                if col + len(token) >= width - 1:
+                    stdscr.addstr(row, col, truncate('...', width - col - 1))
+                    return row + 1
+                pair = curses.A_NORMAL
+                if status == 'OK':
+                    pair = curses.color_pair(1)
+                elif status == 'FAIL':
+                    pair = curses.color_pair(2) | curses.A_BOLD
+                elif status == 'OFF':
+                    pair = curses.color_pair(3)
+                stdscr.attron(pair)
+                stdscr.addstr(row, col, token)
+                stdscr.attroff(pair)
+                col += len(token)
+            row += 1
+        return row
+
+    def _draw_status_line(
+        self,
+        stdscr,
+        row: int,
+        width: int,
+        entry: StatusTextEntry,
+    ) -> None:
+        sev_label = SEVERITY_LABELS.get(entry.severity, str(entry.severity))
+        prefix = f'[{entry.sys_key:5}] [{entry.severity} {sev_label:5}] '
+        body = truncate(entry.text, max(0, width - len(prefix) - 2))
+        line = prefix + body
+
+        pair = curses.A_NORMAL
+        if entry.severity <= 3:
+            pair = curses.color_pair(2)
+        elif entry.severity == 4:
+            pair = curses.color_pair(3)
+        elif entry.severity == 5:
+            pair = curses.color_pair(6)
+        elif entry.severity == 6:
+            pair = curses.color_pair(1)
+        elif entry.severity == 7:
+            pair = curses.color_pair(5)
+
+        stdscr.attron(pair)
+        stdscr.addstr(row, 0, truncate(line, width - 1))
+        stdscr.attroff(pair)
+
+
+def main() -> int:
+    if not sys.stdout.isatty():
+        print('vehicleDiagShow requires an interactive terminal (TTY).', file=sys.stderr)
+        return 1
+
+    rclpy.init()
+    node = VehicleDiagNode()
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+
+    try:
+        DiagCursesApp(node).run()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        spin_thread.join(timeout=2.0)
+
+    return 0
+
+
+if __name__ == '__main__':
+    sys.exit(main())

src/fc_network_module/fc_network_module/ntrip_client.py

@@ -0,0 +1,256 @@
+import socket
+import base64
+import threading
+import time
+from typing import Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
+from mavros_msgs.msg import RTCM
+
+# TODO: ROS_DOMAIN_ID 要補一下
+
+class GGA_stream():
+
+    @classmethod
+    def nmea_checksum(cls, body: str) -> str:
+        """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
+        value = 0
+        for ch in body:
+            value ^= ord(ch)
+        return f"{value:02X}"
+
+    @classmethod
+    def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> Tuple[str, str]:
+        """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
+        if is_latitude:
+            hemi = "N" if deg >= 0 else "S"
+            deg_width = 2
+        else:
+            hemi = "E" if deg >= 0 else "W"
+            deg_width = 3
+        deg = abs(deg)
+        d = int(deg)
+        m = (deg - d) * 60.0
+        return f"{d:0{deg_width}d}{m:07.4f}", hemi
+
+    @classmethod
+    def build_gga_sentence(cls, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
+        """
+        組一筆 $GPGGA 句子（含 checksum) 回傳 bytes 可直接 sock.sendall。
+
+        lat_deg / lon_deg : 十進位經緯度(北、東為正)。
+        測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
+        """
+        utc = time.gmtime()
+        t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
+
+        lat_dm, ns = cls.decimal_to_nmea_dm(lat_deg, is_latitude=True)
+        lon_dm, ew = cls.decimal_to_nmea_dm(lon_deg, is_latitude=False)
+
+        # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
+        body = (
+            f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
+            f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
+        )
+        sentence = f"${body}*{cls.nmea_checksum(body)}\r\n"
+        return sentence.encode("ascii")
+
+    @classmethod
+    def send_gga(cls, sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
+        """送出 GGA，回傳可讀句子供列印。"""
+        payload = cls.build_gga_sentence(lat_deg, lon_deg, alt_m)
+        sock.sendall(payload)
+        return payload.decode("ascii").strip()
+
+class NtripClientNode(Node):
+    """連線 NTRIP caster, 接收 RTCM3 資料流並發布至 ROS2 topic """
+
+    RECONNECT_BASE_SEC = 10.0
+    RECONNECT_MAX_SEC = 60.0
+
+    def __init__(self):
+        super().__init__('ntrip_client')
+
+        self.declare_parameter('host', 'rtk2go.com')
+        self.declare_parameter('port', 2101)
+        self.declare_parameter('mountpoint', '')
+        self.declare_parameter('username', 'template@mail.com')
+        self.declare_parameter('password', '')
+
+        self.declare_parameter('gga_on', False)
+        self.declare_parameter('gga_lat', None)
+        self.declare_parameter('gga_lon', None)
+        self.declare_parameter('gga_alt_m', 100)
+        self.declare_parameter('gga_interval_sec', 60)
+
+        rtcm_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
+
+        self._stop_event = threading.Event()
+        self._sock_lock = threading.Lock()
+        self._active_sock: Optional[socket.socket] = None
+        self._thread = threading.Thread(target=self._receive_loop, daemon=True)
+        self._thread.start()
+
+        self.get_logger().info('NtripClientNode 已啟動')
+
+    # ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
+
+    def _request_stop(self):
+        self._stop_event.set()
+        with self._sock_lock:
+            if self._active_sock is not None:
+                try:
+                    self._active_sock.close()
+                except OSError:
+                    pass
+
+    def _receive_loop(self):
+        backoff = self.RECONNECT_BASE_SEC
+
+        host = self.get_parameter('host').value
+        port = self.get_parameter('port').value
+        mountpoint = self.get_parameter('mountpoint').value
+        user = self.get_parameter('username').value
+        pwd = self.get_parameter('password').value
+
+        if not mountpoint:
+            self.get_logger().error('mountpoint 參數未設定...')
+            return
+
+        while not self._stop_event.is_set():
+
+            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            sock.settimeout(10)
+            with self._sock_lock:
+                self._active_sock = sock
+
+            self.gga_lat = self.get_parameter('gga_lat').value
+            self.gga_lon = self.get_parameter('gga_lon').value
+            self.gga_alt_m = self.get_parameter('gga_alt_m').value
+            self.gga_interval_sec = self.get_parameter('gga_interval_sec').value
+            self.gga_on = self.get_parameter('gga_on').value
+            self.gga_on = self.gga_lat is not None and self.gga_lon is not None and self.gga_on
+
+
+            try:
+                self.get_logger().info(f'正在連線 {host}:{port}/{mountpoint} ...')
+                sock.connect((host, port))
+
+                auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
+                request = (
+                    f'GET /{mountpoint} HTTP/1.0\r\n'
+                    f'User-Agent: NTRIP PythonClient\r\n'
+                    f'Authorization: Basic {auth}\r\n'
+                    f'Connection: close\r\n\r\n'
+                )
+                sock.sendall(request.encode())
+
+                resp = sock.recv(4096)
+                if b'ICY 200 OK' not in resp:
+                    self.get_logger().error(
+                        f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
+                    )
+                    raise ConnectionError('handshake failed')
+
+                self.get_logger().info(f'已成功連接至 {mountpoint}')
+
+
+                backoff = self.RECONNECT_BASE_SEC
+
+                self._read_stream(sock)
+
+            except Exception as e:
+                if self._stop_event.is_set():
+                    break
+                self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
+                if self._stop_event.wait(timeout=backoff):
+                    break
+                backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
+            finally:
+                with self._sock_lock:
+                    if self._active_sock is sock:
+                        self._active_sock = None
+                try:
+                    sock.close()
+                except OSError:
+                    pass
+
+    def _read_stream(self, sock: socket.socket):
+        buf = b''
+        last_gga_time = 0.0
+
+        while not self._stop_event.is_set():
+            now = time.time()
+            if self.gga_on and (now - last_gga_time >= self.gga_interval_sec):
+                line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m)
+                self.get_logger().info(f"[GGA] {line}")
+                last_gga_time = now
+
+            try:
+                chunk = sock.recv(4096)
+            except socket.timeout:
+                if self._stop_event.is_set():
+                    break
+                continue
+            except OSError:
+                if self._stop_event.is_set():
+                    break
+                raise
+            if not chunk:
+                if self._stop_event.is_set():
+                    break
+                raise ConnectionError("stream ended. Empty Chunk")
+            buf += chunk
+
+            while len(buf) >= 6:
+                if buf[0] != 0xD3:
+                    buf = buf[1:]
+                    continue
+
+                payload_len = ((buf[1] & 0x03) << 8) | buf[2]
+                frame_len = payload_len + 6  # header(3) + payload + CRC(3)
+
+                if len(buf) < frame_len:
+                    break
+
+                packet = buf[:frame_len]
+                buf = buf[frame_len:]
+
+                self._publish_rtcm(packet)
+
+    def _publish_rtcm(self, raw_packet: bytes):
+        msg = RTCM()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.data = list(raw_packet)
+        self.publisher_.publish(msg)
+
+    def destroy_node(self):
+        self._request_stop()
+        if self._thread.is_alive():
+            self._thread.join(timeout=5.0)
+            if self._thread.is_alive():
+                self.get_logger().warn('receive thread 未在時限內結束')
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NtripClientNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/fc_network_module/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fc_network_module</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>mavros_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/fc_network_module/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fc_network_module
+[install]
+install_scripts=$base/lib/fc_network_module

src/fc_network_module/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'fc_network_module'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='picars',
+    maintainer_email='chiyu1468@hotmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'ntrip_client = fc_network_module.ntrip_client:main',
+        ],
+    },
+)

src/fc_network_module/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/fc_network_module/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/fc_network_module/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/someotherpkg/src/example2_change_mode.py

@@ -1,55 +0,0 @@
-
-
-from fc_network_apps import CommandLongClient
-import time
-
-def main():
-    # Equivalent to:
-    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
-    commandAPI = CommandLongClient()
-    result = commandAPI.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}")
-
-    time.sleep(1)
-
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=3.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}")
-
-    time.sleep(1)
-    
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=5.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_arm_disarm.py

@@ -1,30 +0,0 @@
-"""Usage example for arm/disarm helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_arm_disarm
-"""
-
-from fc_network_apps import arm_disarm
-
-
-def main() -> None:
-    # MAV_CMD_COMPONENT_ARM_DISARM (command=400)
-    # param1: 1 = arm, 0 = disarm
-    result = arm_disarm(
-        target_sysid=3,
-        target_compid=0,
-        arm=True,
-        timeout_sec=2.0,
-    )
-
-    print("=== arm result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    # To disarm instead:
-    # result = arm_disarm(target_sysid=3, target_compid=0, arm=False, timeout_sec=2.0)
-
-
-if __name__ == "__main__":
-    main()

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

@@ -1,27 +0,0 @@
-"""Usage example for land helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_land
-"""
-
-from fc_network_apps import land
-
-
-def main() -> None:
-    # MAV_CMD_NAV_LAND (command=21)
-    # This example asks vehicle sysid=3 to land.
-    result = land(
-        target_sysid=3,
-        target_compid=0,
-        yaw_deg=0.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== land 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_takeoff.py

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

src/unitdev01/unitdev01/xbee.py

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

src/unitdev02/unitdev02/devnote.txt

@@ -1,21 +1,25 @@
 備選需要的功能
- - serail 對於 telemetry 的支援
  - serial_manager.serial_object.transport 這些變數可能不需要
 
 不用動
  - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async
- - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) 
 
 這一步
- 研究 ros2 service
-
-下一步
-
-下下一步
+ RTK
+   跟 RTK2go 抓取列表 Done
+   從特定 mount point 得到數據 Done
+   做一個 ros2 service 接到數據並包裝 Done
+   RTK 實機測試
  
 
+下一步
+  重啟做好了 但是還沒自動化
+
+下下一步
+  整合 xbee api-api 功能 
+
 後面
- rssi 資訊提取s
+
 
 
 
@@ -24,25 +28,44 @@ 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
 
+python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com
+
+python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart
+
+python3 -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart -p gga_on:=True -p gga_lat:=24.155792000 -p gga_lon:=120.630679000 -p gga_interval_sec:=60
+
+watch GPS_RAW_INT
+
+export ROS_DOMAIN_ID=1
+ros2 daemon stop
+ros2 daemon start
+
 ros2 topic list
-ros2 topic echo 
+ros2 topic echo /fc_network/vehicle/sys3/battery
+ros2 topic echo hz /fc_network/vehicle/sys3/battery
+
+ros2 topic bw /fc_network/vehicle/sys3/attitude
+ros2 topic hz /fc_network/vehicle/sys3/attitude
 
 
 /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: 10, 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
+
+

src/unitdev02/unitdev02/lifecycle_node.py

@@ -1,45 +0,0 @@
-
-
-
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-import time
-
-# import mavros_msgs.srv
-
-class TalkerNode(Node):
-    def __init__(self):
-        start_time = time.time()
-        super().__init__('talker_node')
-        end_time = time.time()
-        print(f"Node initialization took {end_time - start_time:.2f} seconds")
-
-        self.publisher_ = self.create_publisher(String, 'hahatest/_1', 10)
-        self.timer = self.create_timer(1.0, self.timer_callback)  # 每秒執行一次
-        self.get_logger().info('TalkerNode has been started.')
-
-    def timer_callback(self):
-        msg = String()
-        msg.data = 'Hello, ROS 2!'
-        self.publisher_.publish(msg)
-        self.get_logger().info(f'Published: "{msg.data}"')
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = TalkerNode()
-
-    print("Before sleep")
-    time.sleep(5)  # 等待 5 秒鐘
-    print("After sleep")
-    try:
-        start_time = time.time()
-        while time.time() - start_time < 10:  # 持續 10 秒鐘
-            rclpy.spin_once(node)
-            time.sleep(1)  # 每秒執行一次
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/unitdev02/unitdev02/ping_test.py

@@ -0,0 +1,188 @@
+import time
+from pymavlink import mavutil
+
+# # 強制 source_system=255
+# master = mavutil.mavlink_connection('udpout:localhost:14561', source_system=255)
+
+# print("Starting Force-Ping Test...")
+
+# while True:
+#     # 1. 發送 Heartbeat (證明連線活著)
+#     master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, 
+#                              mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
+    
+#     # 2. 強制發送 PING (target 1, 1)
+#     # 我們加一個計數器，方便在 tcpdump 裡辨識
+#     ping_seq = int(time.time()) % 100
+#     master.mav.ping_send(int(time.time() * 1e6), ping_seq, 1, 1)
+    
+#     print(f"Sent Heartbeat and PING (seq {ping_seq})")
+    
+#     # 3. 非阻塞讀取，避免卡死
+#     msg = master.recv_match(blocking=False)
+#     if msg:
+#         print(f"--> Received something: {msg.get_type()}")
+        
+#     time.sleep(1) # 每秒噴一次
+
+# -------------------------------------------------------------------
+
+# # 改成 udpin，這會讓你的程式守在 14561 等模擬器送資料進來
+# # 這樣你就能收到模擬器的 Heartbeat，建立連線後再發 PING
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# print("Waiting for simulator heartbeat...")
+# # 這一行很重要：先收到 Heartbeat，pymavlink 才會設定好 target 地址
+# master.wait_heartbeat()
+# print(f"Heartbeat from system {master.target_system} component {master.target_component}")
+
+# while True:
+#     # 既然收到了，現在發 PING 就會順著原路回去
+#     master.mav.ping_send(int(time.time() * 1e6), 1, master.target_system, master.target_component)
+#     print("PING sent!")
+    
+#     msg = master.recv_match(type='PING', blocking=True, timeout=2)
+#     if msg:
+#         print(f"Got PING response: {msg}")
+#     time.sleep(1)
+
+
+# -------------------------------------------------------------------
+
+# import time
+# from pymavlink import mavutil
+
+# # 核心修改：因為 MAVProxy 正在往 14561 噴資料，我們必須「接住」它
+# # 使用 udpin 會讓你的程式在 14561 監聽
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=254)
+
+# print("正在等待 MAVProxy 轉發的 Heartbeat...")
+
+# # 只要這行跑過，代表你跟 MAVProxy 握手成功了
+# master.wait_heartbeat()
+# print(f"成功連線到系統 {master.target_system}！")
+
+# while True:
+#     # 1. 務必發送 Heartbeat，讓 MAVProxy 知道要把回應送回這個 Port
+#     master.mav.heartbeat_send(
+#         mavutil.mavlink.MAV_TYPE_GCS, 
+#         mavutil.mavlink.MAV_AUTOPILOT_INVALID, 
+#         0, 0, 0
+#     )
+
+#     # 2. 修改 target_component 為 1 (飛控核心)
+#     # 注意：這裡刻意將 seq 設為動態，方便你在 watch PING 裡觀察
+#     p_seq = int(time.time()) % 255
+#     master.mav.ping_send(
+#         int(time.time() * 1e6), 
+#         p_seq, 
+#         1, 
+#         1  # <--- 改成 1 試試看！
+#     )
+#     print(f"Sent PING seq {p_seq} to 1/1")
+
+#     # 3. 讀取所有訊息，看看有沒有 PING 回來
+#     # 有時候回應的 target_system 可能是 255 (你的 ID)
+#     msg = master.recv_match(type='PING', blocking=True, timeout=1.0)
+#     if msg:
+#         print(f"🎉 成功！收到回應: {msg}")
+    
+#     time.sleep(1)
+
+
+
+# -------------------------------------------------------------------
+
+
+# import time
+# from pymavlink import mavutil
+
+# # 建議一樣用 udpin 監聽 MAVProxy 的輸出
+# master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# def get_time_ns():
+#     return int(time.time() * 1e9) # 轉為奈秒
+
+# print("開始測試 TIMESYNC 響應...")
+
+# while True:
+#     now_ns = get_time_ns()
+    
+#     # 發送 TIMESYNC 請求
+#     # tc1=0 代表這是請求，ts1=當前時間
+#     master.mav.timesync_send(0, now_ns)
+    
+#     # 等待回應
+#     msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+    
+#     if msg and msg.tc1 != 0:
+#         # 收到回應，msg.tc1 應該等於我們剛才送出的 now_ns
+#         rtt_ms = (get_time_ns() - msg.tc1) / 1e6
+#         print(f"🕒 TIMESYNC RTT: {rtt_ms:.3f} ms")
+#         break # 測試一次就好
+#     else:
+#         print("等待 TIMESYNC 回應中...")
+        
+#     time.sleep(1)
+
+# -------------------------------------------------------------------
+
+import time
+import statistics
+from pymavlink import mavutil
+
+# 連接到 MAVProxy 轉發的端口
+master = mavutil.mavlink_connection('udpin:localhost:14561', source_system=255)
+
+# 測試設定
+TARGET_SYS = 2  # 你想測試的載具 ID
+SAMPLES = 20    # 取 20 次樣本來算平均和抖動
+rtt_history = []
+
+print(f"開始測試對 System {TARGET_SYS} 的通道品質...")
+
+def get_time_ns():
+    return int(time.time() * 1e9)
+
+try:
+    while len(rtt_history) < SAMPLES:
+        ts1_send = get_time_ns()
+        
+        # 發送 TIMESYNC (V1/V2 格式相容)
+        # 針對特定系統發送
+        master.mav.timesync_send(0, ts1_send)
+        
+        # 等待回應 (過濾目標系統的訊息)
+        msg = master.recv_match(type='TIMESYNC', blocking=True, timeout=1.0)
+        
+        if msg and msg.get_srcSystem() == TARGET_SYS and msg.tc1 != 0:
+            ts1_recv = get_time_ns()
+            rtt_ns = ts1_recv - msg.tc1
+            rtt_ms = rtt_ns / 1e6
+            rtt_history.append(rtt_ms)
+            print(f"Sample {len(rtt_history)}: RTT = {rtt_ms:.3f} ms")
+        else:
+            print("Request timeout or mismatched ID...")
+            
+        time.sleep(0.1) # 快速採樣
+
+    # 4. 計算統計數據
+    avg_latency = statistics.mean(rtt_history)
+    std_dev = statistics.stdev(rtt_history) # 這就是 Jitter 的一種表現
+    max_rtt = max(rtt_history)
+    min_rtt = min(rtt_history)
+
+    print("\n--- 物理通道品質分析報告 ---")
+    print(f"目標載具  : System {TARGET_SYS}")
+    print(f"平均延遲 (Avg) : {avg_latency:.3f} ms")
+    print(f"最小延遲 (Min) : {min_rtt:.3f} ms")
+    print(f"最大延遲 (Max) : {max_rtt:.3f} ms")
+    print(f"抖動幅度 (Jitter): {std_dev:.3f} ms (標準差)")
+    
+    if std_dev > 10:
+        print("警告：網路抖動過高，可能影響控制穩定性！")
+    else:
+        print("通道品質：良好")
+
+except KeyboardInterrupt:
+    print("測試終止。")

src/unitdev02/unitdev02/pymav_nullbuffer.py

@@ -0,0 +1,46 @@
+from pymavlink.dialects.v20 import common as mavlink
+
+class NullBuffer:
+    def write(self, data): pass
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+class CapturingBuffer:
+    def __init__(self):
+        self.last_data = b''
+    def write(self, data):
+        self.last_data = bytes(data)
+    def seek(self, pos): pass
+    def tell(self): return 0
+
+# 1. 初始化（僅作為編碼器）
+# file 參數可以是任何擁有 write() 方法的物件，這裡用 BytesIO 模擬
+# 初始化方法一：使用 BytesIO
+# import io
+# out_buf = io.BytesIO()
+# mav = mavlink.MAVLink(out_buf, srcSystem=1, srcComponent=191)
+
+# 初始化方法二：使用自定義的 NullBuffer
+mav = mavlink.MAVLink(NullBuffer(), srcSystem=1, srcComponent=191)
+mav.seq = 254
+
+# 2. 建立心跳包並取得二進制數據
+# 連續產出「不同」的訊息物件
+for i in range(3):
+    msg = mav.heartbeat_encode(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = msg.pack(mav)
+    mav.seq = (mav.seq + 1) & 0xFF
+    # MAVLink 2 的序列號在第 3 個 Byte (Index 2)
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")
+
+
+
+print("分隔線")
+
+buf = CapturingBuffer()
+mav_buf2 = mavlink.MAVLink(buf, srcSystem=1, srcComponent=191)
+
+for i in range(3):
+    mav_buf2.heartbeat_send(mavlink.MAV_TYPE_GCS, 0, 0, 0, 0)
+    data = buf.last_data
+    print(f"第 {i} 次發送, Seq: {data[4]}, Raw: {data.hex()}")

src/unitdev02/unitdev02/serial_udp_ad.py

@@ -0,0 +1,71 @@
+import asyncio
+import serial_asyncio
+
+SERIAL_PORT = '/dev/ttyUSB0'   # 修改為你的 serial port
+SERIAL_BAUDRATE = 115200       # 修改為你的 baudrate
+UDP_REMOTE_IP = '192.168.1.100' # 修改為目標 IP
+UDP_REMOTE_PORT = 5005         # 修改為目標 port
+UDP_LOCAL_PORT = 5006          # 本地 UDP 監聽 port
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_transport):
+        self.udp_transport = udp_transport
+
+    def connection_made(self, transport):
+        self.transport = transport
+
+    def data_received(self, data):
+        # Serial 收到資料，轉發到 UDP
+        self.udp_transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def write_to_serial(self, data):
+        self.transport.write(data)
+
+class UDPToSerial(asyncio.DatagramProtocol):
+    def __init__(self, serial_proto):
+        self.serial_proto = serial_proto
+
+    def datagram_received(self, data, addr):
+        # UDP 收到資料，轉發到 Serial
+        self.serial_proto.write_to_serial(data)
+
+async def main():
+    loop = asyncio.get_running_loop()
+
+    # 定義協議工廠函數
+    def create_empty_protocol():
+        return asyncio.DatagramProtocol()
+
+    # 建立 UDP1 傳輸
+    udp_transport, _ = await loop.create_datagram_endpoint(
+        create_empty_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 建立 Serial 傳輸
+    serial_proto = SerialToUDP(udp_transport)
+    def get_serial_protocol():
+        return serial_proto
+
+    _, serial_transport = await serial_asyncio.create_serial_connection(
+        loop, get_serial_protocol, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+    )
+
+    # 建立 UDP2 監聽
+    udp_proto = UDPToSerial(serial_proto)
+    def get_udp_protocol():
+        return udp_proto
+
+    udp_listen_transport, _ = await loop.create_datagram_endpoint(
+        get_udp_protocol,
+        local_addr=('0.0.0.0', UDP_LOCAL_PORT)
+    )
+
+    # 保持運行
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+if __name__ == '__main__':
+    asyncio.run(main())

src/unitdev02/unitdev02/sslChech.sh

@@ -1,27 +0,0 @@
-#!/bin/bash
-
-# 網站清單
-DOMAINS=("google.com" "smarter.nchu.edu.tw")
-
-echo "網站 SSL 憑證剩餘天數："
-echo "---------------------------"
-
-for domain in "${DOMAINS[@]}"; do
-    end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null |
-        openssl x509 -noout -enddate | cut -d= -f2)
-
-    end_timestamp=$(date -d "$end_date" +%s)
-    now_timestamp=$(date +%s)
-
-    remaining_days=$(( (end_timestamp - now_timestamp) / 86400 ))
-
-    if [ $remaining_days -lt 0 ]; then
-        status="已過期 ❌"
-    elif [ $remaining_days -lt 15 ]; then
-        status="即將到期 ⚠️"
-    else
-        status="正常 ✅"
-    fi
-
-    printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status"
-done

src/unitdev02/unitdev02/udp_unix_ad.py

@@ -0,0 +1,40 @@
+import socket
+import sys
+import select
+
+# 設定來源 IP 與 Port
+SRC_IP = '127.0.0.1'    # 監聽所有介面
+SRC_PORT = 16661      # 請自行修改
+
+# 建立 UDP 監聽 socket
+src_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+src_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+src_sock.bind((SRC_IP, SRC_PORT))
+print(f"Listening for UDP on {SRC_IP}:{SRC_PORT}")
+
+# 設定目標 Unix socket 路徑
+UNIX_SOCKET_PATH = '/tmp/unix_socket_mavlink.sock'  # 請自行修改
+
+# 建立 Unix socket 連線
+unix_sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+try:
+    unix_sock.connect(UNIX_SOCKET_PATH)
+except Exception as e:
+    print(f"Failed to connect to unix socket: {e}")
+    sys.exit(1)
+
+while True:
+    # 使用 select 監聽兩個 socket
+    readable, _, _ = select.select([src_sock, unix_sock], [], [])
+    for sock in readable:
+        if sock is src_sock:
+            data, addr = src_sock.recvfrom(4096)
+            if data:
+                unix_sock.sendall(data)
+                # print(f"Received UDP data from {addr}: {data}") # debug
+                # break # debug
+        elif sock is unix_sock:
+            data = unix_sock.recv(4096)
+            if data:
+                # 回送到最近收到資料的 UDP client
+                src_sock.sendto(data, addr)

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

src/unitdev04/esp32-final.py

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

src/unitdev04/test01.py

@@ -0,0 +1,229 @@
+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
+
+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("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
+    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)
+            packet_count += 1
+        except socket.timeout:
+            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 # [0 0 0 0 0 0 0 0 0.....]
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # 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()
+    print('Start sending data to Gazebo')
+
+    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)
+
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
+
+print('Start')
+# fdm_switch_example_two()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+mavlink_btye_generator_test()

src/unitdev04/udptest4.py

@@ -0,0 +1,193 @@
+import asyncio
+import serial_asyncio
+import struct
+import os
+import sys
+import serial
+import signal
+import traceback
+from pymavlink import mavutil
+
+# === 設定區 ===
+SERIAL_PORT = 'COM15'  # 手動指定
+SERIAL_BAUDRATE = 57600
+UDP_REMOTE_IP = '127.0.0.1'
+UDP_REMOTE_PORT = 14550
+DEBUG_MODE = False
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # 廣播
+
+# === 工具函數 ===
+def check_serial_port():
+    try:
+        ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
+        ser.close()
+        return True
+    except serial.SerialException as e:
+        print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
+        return False
+    except Exception as e:
+        print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
+        return False
+
+def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
+    frame_type = 0x10
+    dest_addr16 = b'\xFF\xFE'
+    broadcast_radius = 0x00
+    options = 0x00
+
+    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+    frame += dest_addr64 + dest_addr16
+    frame += struct.pack(">BB", broadcast_radius, options) + data
+    checksum = 0xFF - (sum(frame) & 0xFF)
+    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+# === Serial Protocol 實作 ===
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol):
+        self.udp_protocol = udp_protocol
+        self.buffer = bytearray()
+
+    def connection_made(self, transport):
+        self.transport = transport
+        if hasattr(self.udp_protocol, 'set_serial_transport'):
+            self.udp_protocol.set_serial_transport(self)
+        print(f"Serial connection established on {SERIAL_PORT}")
+
+    def data_received(self, data):
+        self.buffer.extend(data)
+        while True:
+            if len(self.buffer) < 3:
+                return
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+            if len(self.buffer) < full_length:
+                return
+            frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+            if hasattr(self.udp_protocol, 'send_udp'):
+                self.udp_protocol.send_udp(bytes(frame))
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            pass
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[TX Error] 無法封裝或傳送資料: {e}")
+
+# === UDP Protocol 實作 ===
+class UDPHandler(asyncio.DatagramProtocol):
+    def __init__(self):
+        self.serial_transport = None
+        self.transport = None
+        self.mav_decoder = mavutil.mavlink.MAVLink(None)
+
+    def connection_made(self, transport):
+        self.transport = transport
+        print("UDP transport ready.")
+
+    def set_serial_transport(self, serial_transport):
+        self.serial_transport = serial_transport
+
+    def datagram_received(self, data, addr):
+        if self.serial_transport:
+            self.serial_transport.write_to_serial(data)
+
+    def send_udp(self, data):
+        decoded_data = self.decapsulate_data(data)
+        if decoded_data is None:
+            pass
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def decapsulate_data(self, data):
+        try:
+            if not data or data[0] != 0x7E:
+                return None
+            length = (data[1] << 8) | data[2]
+            if len(data) < length + 4:
+                return None
+            frame_type = data[3]
+            if frame_type == 0x90:
+                rf_data_start = 3 + 12
+                return data[rf_data_start:3 + length]
+            else:
+                return None
+        except Exception as e:
+            print(f"[XBee 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            msg = self.mav_decoder.parse_char(data)
+            if msg:
+                if msg.get_type() == "HEARTBEAT":
+                    pass  # 不輸出任何訊息
+                else:
+                    pass
+        except Exception as e:
+            print(f"[MAVLink Decode Error] {e}")
+
+# === 主流程 ===
+async def main():
+    if not check_serial_port():
+        print("程式終止：串口檢查失敗")
+        return
+
+    loop = asyncio.get_running_loop()
+    if os.name != 'nt':  # Windows 不支援 add_signal_handler
+        for sig in (signal.SIGINT, signal.SIGTERM):
+            loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
+
+    udp_handler = UDPHandler()
+
+    try:
+        udp_transport, _ = await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"無法創建 UDP 端點：{str(e)}")
+        return
+
+    sock = udp_transport.get_extra_info('socket')
+    print(f"UDP listening on {sock.getsockname()}")
+
+    try:
+        serial_proto = SerialToUDP(udp_handler)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"無法建立串口連接：{str(e)}")
+        traceback.print_exc()
+        udp_transport.close()
+        return
+
+    print("等待串口資料...")
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+async def shutdown(loop):
+    print("Shutting down...")
+    tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
+    for task in tasks:
+        task.cancel()
+    await asyncio.gather(*tasks, return_exceptions=True)
+    loop.stop()
+
+if __name__ == '__main__':
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print("程式被使用者中斷")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()

src/unitdev04/udptest8-2.py

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

src/unitdev04/udptest8.py

@@ -0,0 +1,384 @@
+# === import 保持不變 ===
+import asyncio
+import serial_asyncio
+import struct
+import serial
+import traceback
+import time
+from collections import deque, defaultdict
+from pymavlink import mavutil
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import threading
+
+# === 多組設備設定 ===
+CONFIGS = [
+    {"serial_port": "/dev/ttyUSB2", "udp_port": 14551},
+    {"serial_port": "COM43", "udp_port": 14552},
+    {"serial_port": "COM15", "udp_port": 14553},
+]
+
+SERIAL_BAUDRATE = 115200
+UDP_REMOTE_IP = '127.0.0.1'
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
+
+# === 數據存儲 ===
+rssi_history = defaultdict(lambda: deque(maxlen=5000))
+time_history = defaultdict(lambda: deque(maxlen=5000))
+
+# === 新增：丟包率追蹤 ===
+packet_loss_history = defaultdict(lambda: deque(maxlen=1000))
+packet_loss_time_history = defaultdict(lambda: deque(maxlen=1000))
+mavlink_sequence_tracker = defaultdict(dict)  # {sysid: {compid: {'last_seq': x, 'total_packets': y, 'lost_packets': z}}}
+packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0})
+
+serial_to_sysid = {}
+serial_last_mavlink_time = {}  # 優化 1：追蹤各 serial port 最近的 MAVLink 時間
+
+def calculate_packet_loss(sysid, compid, current_seq):
+    """計算丟包率"""
+    global mavlink_sequence_tracker, packet_loss_stats
+    
+    tracker = mavlink_sequence_tracker[sysid]
+    if compid not in tracker:
+        # 第一次收到這個component的消息
+        tracker[compid] = {
+            'last_seq': current_seq,
+            'total_packets': 1,
+            'lost_packets': 0,
+            'last_update': time.time()
+        }
+        return 0.0
+    
+    comp_tracker = tracker[compid]
+    last_seq = comp_tracker['last_seq']
+    
+    # 計算序列號差異（處理255溢出）
+    if current_seq > last_seq:
+        expected_packets = current_seq - last_seq
+    elif current_seq < last_seq:
+        # 序列號溢出（0-255循環）
+        expected_packets = (255 - last_seq) + current_seq + 1
+    else:
+        # 重複的序列號，忽略
+        return comp_tracker.get('loss_rate', 0.0)
+    
+    # 更新統計
+    comp_tracker['total_packets'] += expected_packets
+    lost_packets = expected_packets - 1  # 實際收到1個，應該收到expected_packets個
+    comp_tracker['lost_packets'] += max(0, lost_packets)
+    comp_tracker['last_seq'] = current_seq
+    comp_tracker['last_update'] = time.time()
+    
+    # 計算丟包率
+    total_expected = comp_tracker['total_packets']
+    total_lost = comp_tracker['lost_packets']
+    loss_rate = (total_lost / total_expected) * 100.0 if total_expected > 0 else 0.0
+    comp_tracker['loss_rate'] = loss_rate
+    
+    # 更新全局統計（按sysid匯總所有component）
+    total_received = 0
+    total_lost_all = 0
+    total_expected_all = 0
+    
+    for comp_id, comp_data in tracker.items():
+        total_expected_all += comp_data['total_packets']
+        total_lost_all += comp_data['lost_packets']
+        total_received += comp_data['total_packets'] - comp_data['lost_packets']
+    
+    overall_loss_rate = (total_lost_all / total_expected_all) * 100.0 if total_expected_all > 0 else 0.0
+    
+    packet_loss_stats[sysid] = {
+        'loss_rate': overall_loss_rate,
+        'total_received': total_received,
+        'total_lost': total_lost_all
+    }
+    
+    # 記錄到歷史數據
+    now = time.time()
+    packet_loss_history[sysid].append(overall_loss_rate)
+    packet_loss_time_history[sysid].append(now)
+    
+    return overall_loss_rate
+
+def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
+    frame_type = 0x10
+    dest_addr16 = b'\xFF\xFE'
+    broadcast_radius = 0x00
+    options = 0x00
+    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+    frame += dest_addr64 + dest_addr16
+    frame += struct.pack(">BB", broadcast_radius, options) + data
+    checksum = 0xFF - (sum(frame) & 0xFF)
+    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol, serial_port):
+        self.udp_protocol = udp_protocol
+        self.serial_port = serial_port
+        self.buffer = bytearray()
+
+    def connection_made(self, transport):
+        self.transport = transport
+        if hasattr(self.udp_protocol, 'set_serial_transport'):
+            self.udp_protocol.set_serial_transport(self)
+        print(f"[{self.serial_port}] Serial connection established.")
+
+    def data_received(self, data):
+        self.buffer.extend(data)
+        while True:
+            if len(self.buffer) < 3:
+                return
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+            if len(self.buffer) < full_length:
+                return
+            frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+
+            if hasattr(self.udp_protocol, 'send_udp'):
+                self.udp_protocol.send_udp(bytes(frame))
+
+            if frame[3] == 0x88 and frame[5:7] == b'DB':
+                status = frame[7]
+                if status == 0x00 and len(frame) > 8:
+                    rssi_value = frame[8]
+                    now = time.time()
+
+                    # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
+                    last_time = serial_last_mavlink_time.get(self.serial_port, 0)
+                    if now - last_time <= 0.5:
+                        sysid = serial_to_sysid.get(self.serial_port, None)
+                        if sysid is not None:
+                            rssi_history[sysid].append(-rssi_value)
+                            time_history[sysid].append(now)
+                            # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
+                        else:
+                            print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+                    else:
+                        print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+                else:
+                    print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[{self.serial_port} TX Error] 無法封裝或傳送資料: {e}")
+
+    def send_at_command_db(self):
+        try:
+            frame_type = 0x08
+            frame_id = 0x52
+            at_command = b'DB'
+            parameter = b''
+            frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + at_command + parameter
+            checksum = 0xFF - (sum(frame) & 0xFF)
+            api_frame = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[{self.serial_port}] 發送 DB 指令失敗: {e}")
+
+class UDPHandler(asyncio.DatagramProtocol):
+    def __init__(self, udp_port):
+        self.udp_port = udp_port
+        self.serial_transport = None
+        self.transport = None
+        self.mav_decoder = mavutil.mavlink.MAVLink(None)
+
+    def connection_made(self, transport):
+        self.transport = transport
+        print(f"[UDP:{self.udp_port}] transport ready.")
+
+    def set_serial_transport(self, serial_transport):
+        self.serial_transport = serial_transport
+
+    def datagram_received(self, data, addr):
+        if self.serial_transport:
+            self.serial_transport.write_to_serial(data)
+
+    def send_udp(self, data):
+        decoded_data = self.decapsulate_data(data)
+        if decoded_data is None:
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, self.udp_port))
+
+    def decapsulate_data(self, data):
+        try:
+            if not data or data[0] != 0x7E:
+                return None
+            length = (data[1] << 8) | data[2]
+            if len(data) < length + 4:
+                return None
+            frame_type = data[3]
+            if frame_type == 0x90:
+                rf_data_start = 3 + 12
+                return data[rf_data_start:3 + length]
+            else:
+                return None
+        except Exception as e:
+            print(f"[UDP:{self.udp_port} 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            # 逐字節解析MAVLink
+            for byte in data:
+                msg = self.mav_decoder.parse_char(bytes([byte]))
+                if msg:
+                    sysid = msg.get_srcSystem()
+                    compid = msg.get_srcComponent()
+                    seq = msg.get_seq()
+                    
+                    if sysid == 0:
+                        continue
+                        
+                    if self.serial_transport:
+                        serial_to_sysid[self.serial_transport.serial_port] = sysid
+                        serial_last_mavlink_time[self.serial_transport.serial_port] = time.time()
+                        self.serial_transport.send_at_command_db()
+                    
+                    # === 新增：計算丟包率 ===
+                    try:
+                        loss_rate = calculate_packet_loss(sysid, compid, seq)
+                        # print(f"[SYSID:{sysid}] 丟包率: {loss_rate:.1f}%, SEQ: {seq}")
+                    except Exception as e:
+                        print(f"[UDP:{self.udp_port}] 丟包率計算錯誤: {e}")
+                        
+        except Exception as e:
+            print(f"[UDP:{self.udp_port} MAVLink Decode Error] {e}")
+
+def start_plotting():
+    # 創建子圖：上方RSSI，下方丟包率
+    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
+
+    def update(frame):
+        ax1.clear()
+        ax2.clear()
+        
+        # === RSSI 圖 ===
+        ax1.set_title("RSSI", fontsize=14)
+        ax1.set_xlabel("Time (s ago)")
+        ax1.set_ylabel("RSSI (dBm)")
+        ax1.set_xlim(30, 0)  # 顯示最近10秒
+        ax1.set_ylim(-100, -10)
+        ax1.grid(True, alpha=0.3)
+
+        # === 丟包率圖 ===
+        ax2.set_title("Packet Loss Rate", fontsize=14)
+        ax2.set_xlabel("Time (s ago)")
+        ax2.set_ylabel("Loss Rate (%)")
+        ax2.set_xlim(10, 0) 
+        ax2.set_ylim(0, 100)
+        ax2.grid(True, alpha=0.3)
+
+        now = time.time()
+        
+        # 顏色列表
+        colors = ['blue', 'red', 'green', 'orange', 'purple', 'brown', 'pink', 'gray']
+        
+        for i, sysid in enumerate(sorted(rssi_history.keys())):
+            color = colors[i % len(colors)]
+            
+            # === 繪製 RSSI ===
+            rssi_recent_indices = [
+                idx for idx, ts in enumerate(time_history[sysid]) if now - ts <= 30
+            ]
+            if rssi_recent_indices:
+                rssi_t = [now - time_history[sysid][idx] for idx in rssi_recent_indices]
+                rssi_r = [rssi_history[sysid][idx] for idx in rssi_recent_indices]
+                if rssi_t and rssi_r:
+                    ax1.plot(rssi_t, rssi_r, label=f"SYSID:{sysid}", color=color, linewidth=2)
+                    
+                    # 在RSSI圖上顯示當前數值
+                    if rssi_r:
+                        latest_rssi = rssi_r[-1]
+                        ax1.text(2, latest_rssi, f'{latest_rssi:.0f}dBm', 
+                                bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
+                                fontsize=10, fontweight='bold', color='white')
+
+            # === 繪製丟包率 ===
+            if sysid in packet_loss_history:
+                loss_recent_indices = [
+                    idx for idx, ts in enumerate(packet_loss_time_history[sysid]) if now - ts <= 10
+                ]
+                if loss_recent_indices:
+                    loss_t = [now - packet_loss_time_history[sysid][idx] for idx in loss_recent_indices]
+                    loss_r = [packet_loss_history[sysid][idx] for idx in loss_recent_indices]
+                    if loss_t and loss_r:
+                        ax2.plot(loss_t, loss_r, label=f"SYSID:{sysid}", color=color, linewidth=2, marker='o', markersize=3)
+                        
+                        # 在丟包率圖上顯示當前數值和統計
+                        if loss_r:
+                            latest_loss = loss_r[-1]
+                            stats = packet_loss_stats[sysid]
+                            
+                            # 顯示丟包率百分比
+                            ax2.text(2, latest_loss + (i * 8), f'{latest_loss:.1f}%', 
+                                    bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
+                                    fontsize=10, fontweight='bold', color='white')
+                            
+                            # 在圖表左側顯示詳細統計
+                            stats_text = f"SYSID {sysid}:\nRecieved: {stats['total_received']}\nLoss: {stats['total_lost']}\nLoss Rate: {latest_loss:.1f}%"
+                            ax2.text(28, 85 - (i * 20), stats_text,
+                                    bbox=dict(boxstyle="round,pad=0.5", facecolor=color, alpha=0.8),
+                                    fontsize=9, color='white', verticalalignment='top')
+
+        # 設置圖例
+        ax1.legend(loc="upper right", fontsize=10)
+        ax2.legend(loc="upper right", fontsize=10)
+        
+    ani = animation.FuncAnimation(fig, update, interval=1000)
+    plt.tight_layout()
+    plt.show()
+
+async def setup_bridge(config):
+    serial_port = config["serial_port"]
+    udp_port = config["udp_port"]
+    try:
+        ser = serial.Serial(serial_port, SERIAL_BAUDRATE)
+        ser.close()
+    except Exception as e:
+        print(f"[{serial_port}] 串口打開失敗: {e}")
+        return
+    loop = asyncio.get_running_loop()
+    udp_handler = UDPHandler(udp_port)
+    try:
+        await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"[{serial_port}] 無法創建 UDP: {e}")
+        return
+    try:
+        serial_proto = SerialToUDP(udp_handler, serial_port)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, serial_port, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"[{serial_port}] 無法建立串口連接：{e}")
+        traceback.print_exc()
+        return
+    print(f"[{serial_port}] Serial <--> UDP:{udp_port} bridge ready.")
+
+async def main():
+    tasks = [setup_bridge(cfg) for cfg in CONFIGS]
+    await asyncio.gather(*tasks)
+    await asyncio.Future()
+
+if __name__ == '__main__':
+    try:
+        threading.Thread(target=lambda: asyncio.run(main()), daemon=True).start()
+        start_plotting()
+    except KeyboardInterrupt:
+        print("使用者中斷程式")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()