2.9.1: new mission_orch

- gui.py: _handle_group_mode_change 與 _on_engagement_apply_speed 由
  單一 async 函式內 for 迴圈逐台 await（序列化，最壞 N×timeout）改為
  asyncio.gather 並發，各自等各自 ACK；行為語意不變。
- communication.py: set_mode/arm_drone/set_speed 前 3 處過期註解
  「避免多機並行時的競態條件」更正為共用 client + target_sysid 路由
  （OOM 修復後已非 per-drone、無 per-drone 狀態）。

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

.gitignore

@@ -8,7 +8,8 @@
 # 編譯的資料夾
 **/build/
 **/install/
-log/
+**/log/
+**/logs/
 
 # CMAKE 的衍生檔
 CMakeCache.txt
@@ -24,5 +25,15 @@ Makefile
 **/*.class
 **/*.pyc
 **/*.pyo
-logs/
+**/.cursor/
-src/logs/
+CLAUDE.md
+
+# 本地筆記與硬體 dump
+my_env_notes.md
+notes/
+**/eeprom.bin
+
+# MAVProxy / SITL 執行產生的 log 與參數 dump
+*.tlog
+*.tlog.raw
+mav.parm

.gitmodules

@@ -0,0 +1,7 @@
+[submodule "src/external/angles"]
+	path = src/external/angles
+	url = https://github.com/ros/angles.git
+[submodule "src/external/geographic_info"]
+	path = src/external/geographic_info
+	url = https://github.com/ros-geographic-info/geographic_info.git
+	branch = ros2

README.md

@@ -1,30 +1,109 @@
 這是天雷系統的專案
 
 ===
-專案核心環境
+## 功能簡介
+1. mavlink 多對多串流與交換
+2. 支援 xbee 與 telemetry
+3. 支援 udp 管理
+4. ROS2 介面監控載具
+
+===
+使用限制
+1. 不允許進到 ros 系統有相同 sysid
+2. 假設一台載具上所有 component 共用同一 socket
+
+===
+## 運行環境
+
+### 專案核心框架
 1. ROS2 Humble
-2. Python
+2. Python 3.8.10
-3. Ardupilot
 
-===
+### 必要相依套件及版本
-必要相依套件
+- Python
+[Package] fc_network_adapter
+    1. pymavlink -> Version: 2.4.42
+    2. conda-forge 中的 pyserial-asyncio
+    3. importlib_metadata -> Version: 8.5.0
+    4. setuptools -> Version: 58.2.0 (版本太新不行)
+    5. pyserial-asyncio
+[Package] GUI
+    1. testresources
+    2. websockets
+    3. PyQt6
+    4. PyQt6-WebEngine
 
+- ROS2
+1. source ~/ros2_humble/install/setup.bash
+2. geographic_info (https://github.com/ros-geographic-info/geographic_info.git) 已經作為 submodule 放在 external
+3. angles (https://github.com/ros/angles.git) 已經作為 submodule 放在 external
+4. mavros_msgs (https://github.com/mavlink/mavros) 這個專案中的一個資料夾 這邊手動複製的
 
-===
+### 開發用輔助專案
-建議的開發測試
 1. Gazebo Garden
+2. Ardupilot
 
-=== 
+===
-Package 簡述
+## 使用說明
-1. unitdev 為各自協作者做開發時的測試區
+Clone 專案後 請先執行這些指令
+```bash
+# 1.同步 submodule
+cd ~/AirTrapMine
+git submodule init
+git submodule update
+# 2. build 需要的 package
+colcon build --packages-select angles geographic_msgs
+colcon build --packages-select mavros_msgs # 這個依賴前面的
+colcon build --packages-select fc_interfaces # 自己定義的
+
+```
+1. 
+主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 路徑下 以模組化啟動程式 
+```bash
+# 記得先開啟 依賴 Package 到 overlay
+. ./install/local_setup.bash
+# 範例
+cd ~/AirTrapMine/src/ # 這是範例!!!
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.tests.demo_integration
+python -m someotherpkg.src.example_wholeMoving
+
+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 -> 倫渝
-2. fc_network_adapter 
+8. logs 是執行時期的記錄檔
-    建立、維護與飛控韌體的連接 
-    構築 mavlink 封包
-    同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息
-    處理無線模組的通訊格式 (XBee)
-
 
+===

src/GUI/comm_panel.py

@@ -1,10 +1,78 @@
 #!/usr/bin/env python3
-from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
-                            QPushButton, QLineEdit, QComboBox)
+                            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

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+"""
+指令發送模組
+負責將目標位置轉成具體的通訊指令發送到飛控
+
+目前使用 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, drone_id, sysid, lat, lon, alt):
+        """
+        發送全球座標位置指令
+
+        Args:
+            drone_id: GUI 用的 drone 識別字串 (如 's0_11')
+            sysid: 目標無人機的 MAVLink system ID
+            lat: 緯度 (度)
+            lon: 經度 (度)
+            alt: 高度 (公尺)
+        """
+        pass
+
+    @abstractmethod
+    def close(self):
+        """關閉連線"""
+        pass
+
+
+class MavlinkSender(CommandSender):
+    """
+    MAVLink 直接發送 (驗證階段用)
+    透過 SET_POSITION_TARGET_GLOBAL_INT 發送目標位置
+
+    使用方式:
+        sender = MavlinkSender("udpout:127.0.0.1:14550")
+        sender.send_position_global(sysid=1, lat=24.123, lon=120.567, alt=10.0)
+    """
+
+    # type_mask: 只使用位置 (lat, lon, alt)
+    # 忽略速度 (bit 3,4,5)、加速度 (bit 6,7,8)、yaw (bit 10)、yaw_rate (bit 11)
+    TYPE_MASK_POSITION_ONLY = (
+        0b0000_1101_1111_1000  # = 0x0DF8
+    )
+
+    def __init__(self, connection_string="udpout:127.0.0.1:14550"):
+        """
+        Args:
+            connection_string: MAVLink 連線字串
+                SITL 範例: "udpout:127.0.0.1:14550"
+        """
+        self.connection_string = connection_string
+        self.mav = mavutil.mavlink_connection(connection_string)
+        _log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
+
+    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
+            1,                          # target_component (autopilot)
+            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            self.TYPE_MASK_POSITION_ONLY,
+            int(lat * 1e7),             # lat_int
+            int(lon * 1e7),             # lon_int
+            float(alt),                 # alt
+            0, 0, 0,                    # vx, vy, vz (忽略)
+            0, 0, 0,                    # afx, afy, afz (忽略)
+            0, 0                        # yaw, yaw_rate (忽略)
+        )
+
+    def close(self):
+        """關閉 MAVLink 連線"""
+        if self.mav:
+            self.mav.close()
+            self.mav = None
+            _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)
+
+    # 閉環設計：goto setpoint 由 mission_executor 週期性重送、換點靠 GPS 到達判定，
+    # 所以這裡的 timeout 只影響「echo 驗證要等多久」，不影響指令是否送達飛控
+    # （adapter 是先送 setpoint 再等 echo）。故意設短，讓 adapter 序列化的 service
+    # 在 echo miss 時快速放棄、換下一台，避免多機時排隊塞爆 client poll。
+    DEFAULT_TIMEOUT_SEC = 0.5
+
+    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.setFixedHeight(160)
-        self.setStyleSheet("""
+        _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)
+        self.id_label = QLabel(self.display_id)
-        id_label.setStyleSheet("""
+        _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 = 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 = 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 = 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 = 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 = 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 = 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 = 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):
     """
@@ -486,7 +607,7 @@ class AttitudeIndicator(QWidget):
         # ---- rotate + translate canvas for roll & pitch
         p.save()
         p.translate(cx, cy)
-        p.rotate(self.roll)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
         pitch_offset = self.pitch * ppd
 
         # sky (above horizon)
@@ -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
@@ -535,7 +658,7 @@ class AttitudeIndicator(QWidget):
             p.drawLine(QPointF(x1, y1), QPointF(x2, y2))
 
         # roll pointer triangle (rotates with roll)
-        p.rotate(self.roll)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
         ptr_r = arc_r - 1
         tri = QPolygonF([
             QPointF(0, -ptr_r),
@@ -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/mission_executor.py

@@ -0,0 +1,496 @@
+#!/usr/bin/env python3
+"""
+任務執行模組
+管理多架無人機的 GUIDED 模式飛行控制迴圈
+
+設計 (閉環 / closed-loop):
+    - 每架無人機持有一個航點序列，逐點推進
+    - 各自到達就各自切換到下一個航點
+    - 閉環發送：以固定週期「持續重送」當前目標 setpoint（GUIDED 位置目標冪等），
+      掉包會在下個週期自癒；換點完全靠 GPS 到達判定驅動，不依賴通訊 ACK
+    - send_result（echo 驗證）僅作觀測/UI 顯示，不影響飛行控制：
+      驗證失敗不會 fallback LOITER，指令其實已送達飛控，交給 GPS 續飛
+    - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
+    - 用 QTimer 驅動到達判定，在 Qt 主線程執行
+    - 暫停 = 停止到達判定 + 停止新指令
+
+背景：adapter 的 pos_global_int service 被序列化 + 阻塞等 msg87 echo，實飛慢速鏈路下
+多機會只有一台在 client timeout 內拿到回應。閉環設計繞過此瓶頸（見專案記憶）。
+"""
+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"
+    RUNNING = "running"
+    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',
+        'last_sent_at', 'fail_count', 'status', 'waiting_since',
+        'entered_radius_at', 'last_log_at',
+    )
+
+    def __init__(self, drone_id, sysid, waypoints):
+        self.drone_id = drone_id
+        self.sysid = sysid
+        self.waypoints = waypoints
+        self.wp_index = 0
+        self.done = len(waypoints) == 0
+        # 上次送出當前目標的 monotonic 時間；0.0 = 尚未送過（下個 tick 立即送）
+        self.last_sent_at = 0.0
+        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):
+        if self.done or self.wp_index >= len(self.waypoints):
+            return None
+        return self.waypoints[self.wp_index]
+
+    @property
+    def total_waypoints(self):
+        return len(self.waypoints)
+
+
+class MissionExecutor(QObject):
+    """
+    任務執行器 (事件驅動版 + rendezvous barrier)
+
+    planned_waypoints 格式:
+        {
+            'drone_ids': ['s0_1', 's0_2', ...],
+            'waypoints': [
+                [(lat,lon,alt), ...],   # drone 0
+                [(lat,lon,alt), ...],   # drone 1
+            ],
+            'rendezvous_indices': [3, 7, ...],   # 選填；缺省 = 全程不同步（各自跑）
+        }
+    """
+
+    MAX_RETRY = 3  # 已停用：閉環下 echo 驗證失敗不再累計切 LOITER（保留常數以防外部引用）
+
+    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, 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,
+                 resend_interval_sec=2.5,
+                 complete_stops=True):
+        super().__init__()
+        self.sender = sender
+        # complete_stops=False：全員到達也不停（hover-hold 持續重送），供
+        # orchestrator 的瞬時隊形追蹤使用（靠 update_plan 持續餵新 slot）。
+        self.complete_stops = complete_stops
+        self.drone_gps = drone_gps
+        self.monitor = monitor  # 保留：僅供外部手動 fallback 使用，閉環下不自動切 LOITER
+        self.arrival_radius = arrival_radius
+        # 閉環重送週期：每隔這麼久重送一次當前目標 setpoint（掉包自癒）。
+        # 注意：需 > N架 × client_timeout，否則多機序列化會塞不完（見 command_sender）。
+        self.resend_interval_sec = resend_interval_sec
+        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 / 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:
+            _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])
+            self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i])
+
+        self.state = MissionState.RUNNING
+        self._timer.start(self._interval_ms)
+
+        total_wps = sum(t.total_waypoints for t in self.tasks.values())
+        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"重送週期={self.resend_interval_sec}s (閉環，GPS 驅動換點), "
+            f"barrier timeout={self.barrier_timeout_sec}s, "
+            f"{rv_info}",
+        )
+
+    def update_plan(self, planned_waypoints):
+        """
+        熱替換航點（moving-target 用）。保留 RUNNING 狀態與 timer 不中斷，
+        只換各機的目標座標並重置到達/送出狀態，讓下個 tick 立即朝新目標。
+
+        - 未在執行中 → 等同 start()
+        - 沿用 start() 的 planned_waypoints 格式
+        - drone 集合可變動：新機補 task、消失的機移除；FALLBACK_LOITER 的機不強制復原
+        """
+        if self.state != MissionState.RUNNING:
+            self.start(planned_waypoints)
+            return
+
+        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):
+            wps = waypoints_list[i]
+            task = self.tasks.get(drone_id)
+            if task is None:
+                sysid = int(drone_id.split('_')[1])
+                self.tasks[drone_id] = DroneTask(drone_id, sysid, wps)
+                continue
+            task.waypoints = wps
+            task.wp_index = 0
+            task.done = (len(wps) == 0)
+            task.last_sent_at = 0.0       # 下個 tick 立即重送新目標
+            task.entered_radius_at = 0.0
+            if task.status != TaskStatus.FALLBACK_LOITER:
+                task.status = TaskStatus.NORMAL
+
+        # 移除不在新計畫中的機
+        keep = set(drone_ids)
+        for drone_id in list(self.tasks.keys()):
+            if drone_id not in keep:
+                del self.tasks[drone_id]
+
+    def pause(self):
+        if self.state == MissionState.RUNNING:
+            self._timer.stop()
+            self.state = MissionState.PAUSED
+            _log("INFO", "任務已暫停")
+
+    def resume(self):
+        if self.state == MissionState.PAUSED:
+            self._timer.start(self._interval_ms)
+            self.state = MissionState.RUNNING
+            _log("INFO", "任務已恢復")
+
+    def stop(self):
+        self._timer.stop()
+        self.tasks.clear()
+        self.rendezvous_indices = set()
+        self.state = MissionState.IDLE
+        _log("INFO", "任務已停止")
+
+    # ------------------------------------------------------------------ 控制迴圈
+
+    def _tick(self):
+        """
+        控制 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 or task.status == TaskStatus.FALLBACK_LOITER:
+                continue
+            # 已在 barrier 等待的不再跑到達判定
+            if task.status == TaskStatus.WAITING_AT_BARRIER:
+                continue
+
+            target = task.current_target
+            if target is None:
+                continue
+
+            gps = self.drone_gps.get(task.drone_id)
+            if gps is None:
+                continue
+
+            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:
+                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
+            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
+
+            # 到達當前 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: 週期性重送當前目標（閉環）----
+        # 每 resend_interval_sec 重送一次目前 wp 的 setpoint，不靠 send_result 決定前進。
+        # GUIDED 位置目標冪等，重送安全；掉包會在下個週期自癒。first send: last_sent_at=0 → 立即送。
+        for task in self.tasks.values():
+            if task.done or task.status in (
+                TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
+            ):
+                continue
+            target = task.current_target
+            if target is None:
+                continue
+            if now - task.last_sent_at < self.resend_interval_sec:
+                continue
+            tgt_lat, tgt_lon, tgt_alt = target
+            task.last_sent_at = now
+            self.sender.send_position_global(
+                task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
+            )
+
+        # ---- Phase 4: 完成檢查 ----
+        if not self.complete_stops:
+            return  # hover-hold 模式：不自動結束，持續重送（Phase 3 已處理）
+        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()
+            _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.last_sent_at = 0.0  # 新 wp 下個 tick 立即送
+        task.fail_count = 0
+        task.entered_radius_at = 0.0
+        task.last_log_at = 0.0
+        if task.status == TaskStatus.RETRYING:
+            task.status = TaskStatus.NORMAL  # 清掉上個 wp 殘留的「送出未驗證」狀態
+        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（閉環：僅觀測，不影響飛行控制）。
+
+        指令在 adapter 端等 echo 前就已送達飛控，echo 驗證失敗不代表沒送到；
+        因此這裡不再累計失敗切 LOITER，只更新 UI 狀態。真正的前進由 GPS 到達判定驅動，
+        重送由 Phase 3 的週期性重送負責。
+        """
+        task = self.tasks.get(drone_id)
+        if task is None or task.done:
+            return
+        # 若已在 barrier 等待，舊指令的遲到回應不要再干擾狀態
+        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, "send verified")
+            task.fail_count = 0
+            return
+
+        # echo 驗證失敗：僅記錄 + UI 提示，不改變飛行（交給 GPS 續飛、下個週期重送）
+        task.fail_count += 1
+        task.status = TaskStatus.RETRYING  # 純觀測用途，Phase 1/3 不會因此停飛
+        _log(
+            "WARN",
+            f"{drone_id} 送出未驗證 x{task.fail_count}（指令已送達飛控，由 GPS 續飛）: {message}",
+        )
+        self.task_status_changed.emit(
+            drone_id, task.status.value,
+            f"send unverified x{task.fail_count} (GPS 續飛): {message}"
+        )
+
+    def _fallback_to_loiter(self, drone_id):
+        """
+        用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。
+
+        閉環改版後不再由 _on_send_result 自動呼叫（送指令失敗不代表沒送到）；
+        保留此方法供外部（例如 GUI 手動安全按鈕）需要時使用。
+        """
+        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"))
+
+
+# ------------------------------------------------------------------ 工具函式
+
+def _haversine(lat1, lon1, lat2, lon2):
+    """計算兩個 GPS 座標間的水平距離 (公尺)"""
+    R = 6371000.0
+    lat1_r = math.radians(lat1)
+    lat2_r = math.radians(lat2)
+    dlat = math.radians(lat2 - lat1)
+    dlon = math.radians(lon2 - lon1)
+
+    a = (math.sin(dlat / 2) ** 2 +
+         math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
+    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

src/GUI/mission_group.py

@@ -0,0 +1,683 @@
+#!/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': '30.0',
+    'altitude_diff': '2.0',
+    'radius': '10.0',
+    'altitude': '30.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.mission_target = None           # (lat, lon)，最近點擊的地圖目標供 XBee GOTO 使用
+        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
+    xbee_command_requested = pyqtSignal(str, str)      # group_id, START/STOP/GOTO
+    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", "25", "30"])
+        self.alt_input.setCurrentText("30")
+        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()
+
+        # ============================
+        # XBee JSON 指令（GOTO 使用最近點擊的地圖目標點）
+        # ============================
+        xbee_col = QVBoxLayout()
+        xbee_col.setSpacing(3)
+
+        xbee_title = QLabel("XBee/WS 指令")
+        xbee_title.setStyleSheet(TITLE)
+        xbee_col.addWidget(xbee_title)
+
+        xbee_buttons = QVBoxLayout()
+        xbee_buttons.setSpacing(3)
+        start_xbee_btn = QPushButton("START")
+        start_xbee_btn.setStyleSheet(
+            BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
+        start_xbee_btn.clicked.connect(
+            lambda: self.xbee_command_requested.emit(
+                self.group.group_id, 'START'))
+        stop_xbee_btn = QPushButton("STOP")
+        stop_xbee_btn.setStyleSheet(
+            BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
+        stop_xbee_btn.clicked.connect(
+            lambda: self.xbee_command_requested.emit(
+                self.group.group_id, 'STOP'))
+        goto_xbee_btn = QPushButton("GOTO")
+        goto_xbee_btn.setToolTip("前往目前 Group 最近一次點擊的地圖目標點")
+        goto_xbee_btn.setStyleSheet(
+            BTN.format(bg='#1565C0', fg='white', hover='#1976D2'))
+        goto_xbee_btn.clicked.connect(
+            lambda: self.xbee_command_requested.emit(
+                self.group.group_id, 'GOTO'))
+        xbee_buttons.addWidget(start_xbee_btn)
+        xbee_buttons.addWidget(stop_xbee_btn)
+        xbee_buttons.addWidget(goto_xbee_btn)
+        xbee_col.addLayout(xbee_buttons)
+        xbee_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)
+
+        # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 > XBee ──
+        # 使用伸展因子 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.addWidget(self._make_sep())
+        cols.addLayout(xbee_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):
+        """更新中心點 / 目標點顯示"""
+        self.group.mission_target = (float(target_lat), float(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.group.mission_target = None
+        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 = 30.0
+        self.takeoff_requested.emit(self.group.group_id, alt)

src/GUI/mission_orchestrator1.py

@@ -0,0 +1,710 @@
+#!/usr/bin/env python3
+"""
+任務編排器 (MissionOrchestrator) —— ABC 對抗任務的階段狀態機。
+
+角色：坐在既有 MissionExecutor 之上的「任務大腦」。偵察飛行由操作員用既有的
+群組執行按鈕控制（A 組 GRID_SWEEP、B 組待機）；orchestrator 只做兩件事：
+  1. 被動監看「A↔敵機」距離，達 R_detect 持續 dwell → latch，提示操作員。
+  2. 操作員按「重編隊」→ 接管 A+B 四機，用「瞬時隊形」追移動敵機、足夠近時展開包圍。
+
+追蹤採「瞬時隊形」而非路徑跟隨：每 tick 給每台一個目標點（此刻該站的隊形位置），
+敵機移動就更新。避免了路徑跟隨在移動目標下 index 歸零/往後外推造成的往返振盪。
+
+階段：IDLE → WATCH（監看偵測）→ DETECTED（latch，等確認）
+      → PURSUE（4 機瞬時 leader-follow 朝敵，B 在後方 slot 自然追上會合）
+      → ENCIRCLE（形心距敵 ≤ R_engage 展開包圍，圓心追敵；逃出 R_engage_exit 退回）
+
+設計定案見 memory: design_mission_orchestrator。修改限定 src/GUI/（scope_gui_only）。
+"""
+import math
+import time
+from enum import Enum
+
+from PyQt6.QtWidgets import (
+    QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, QPushButton,
+    QDoubleSpinBox
+)
+from PyQt6.QtCore import Qt, QObject, pyqtSignal
+
+
+def _log(level, message):
+    print(f"[{level}] {message}", flush=True)
+
+
+def _haversine(lat1, lon1, lat2, lon2):
+    """兩經緯度間的水平地面距離 (m)。"""
+    R = 6371000.0
+    p1 = math.radians(lat1)
+    p2 = math.radians(lat2)
+    dphi = math.radians(lat2 - lat1)
+    dlam = math.radians(lon2 - lon1)
+    a = (math.sin(dphi / 2) ** 2
+         + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2) ** 2)
+    return 2 * R * math.asin(min(1.0, math.sqrt(a)))
+
+
+def _ll_to_m(lat, lon, ref_lat, ref_lon):
+    """經緯度 → 以 ref 為原點的本地平面公尺 (x=東, y=北)。等距近似，小範圍夠用。"""
+    x = math.radians(lon - ref_lon) * 6371000.0 * math.cos(math.radians(ref_lat))
+    y = math.radians(lat - ref_lat) * 6371000.0
+    return x, y
+
+
+def _m_to_ll(x, y, ref_lat, ref_lon):
+    """本地平面公尺 → 經緯度。"""
+    lat = ref_lat + math.degrees(y / 6371000.0)
+    lon = ref_lon + math.degrees(x / (6371000.0 * math.cos(math.radians(ref_lat))))
+    return lat, lon
+
+
+# 預設參數
+DEFAULT_R_DETECT = 40.0        # 偵測範圍 (m)
+DEFAULT_DETECT_DWELL = 1.0     # 偵測 dwell (s)
+
+DEFAULT_R_ENGAGE = 25.0        # 接戰半徑 (m)：形心↔敵 ≤ 此值 → 切包圍
+DEFAULT_R_ENGAGE_EXIT = 35.0   # 包圍退出半徑 (m)：> 此值 → 退回前進（hysteresis）
+
+DEFAULT_REPLAN_MOVE_M = 2.0    # 敵機位移 ≥ 此值即重算（moving target）
+DEFAULT_REPLAN_CAP_SEC = 1.0   # 重算週期上限 (s)
+
+DEFAULT_CIRCLE_RADIUS = 15.0   # 包圍半徑 (m)
+DEFAULT_LON_SPACING = 8.0      # 隊形縱向間距 (m)
+DEFAULT_LAT_OFFSET = 5.0       # 隊形橫向錯開 (m)   
+
+DEFAULT_FORMATION_ALT = 30.0   # 隊形基準高度 (m，相對 home)
+DEFAULT_ALT_LAYER_GAP = 10.0   # A/B 上下層垂直間隔 (m)：A=基準+gap/2、B=基準-gap/2
+DEFAULT_MIN_ALT_FLOOR = 10.0    # 相對 home 的最低安全高度 (m)：B 下層不得低於此值
+
+DEFAULT_LEAD_TIME_CAP = 4.0    # 前置攔截 lead_time 上限 (s)
+DEFAULT_FRIENDLY_SPEED = 5.0   # 友機假設水平速度 (m/s)：估 lead_time = gap/此值
+DEFAULT_ENEMY_VEL_WINDOW = 1.0 # 敵速估計的 GPS 差分視窗 (s)
+
+_NONE_LABEL = "（未選擇）"
+
+
+class MissionPhase(Enum):
+    IDLE = "idle"          # 未設定敵機/A 組
+    WATCH = "watch"        # 監看偵測（偵察飛行由操作員的群組任務負責）
+    DETECTED = "detected"  # 偵測 latch，等操作員確認重編隊
+    PURSUE = "pursue"      # 4 機瞬時 leader-follow 朝敵機
+    ENCIRCLE = "encircle"  # 4 機包圍敵機
+
+
+class MissionOrchestrator(QObject):
+    """
+    階段狀態機。透過注入的 callback 與 GUI 解耦（便於 headless 測試）：
+      make_executor()  -> MissionExecutor（complete_stops=False，hover-hold）
+      gps_provider()   -> {drone_id: {'lat','lon','alt'}}
+      stop_groups_fn() -> 停掉操作員的 A/B 群組任務（重編隊時避免雙重送點）
+    """
+
+    phase_changed = pyqtSignal(str)      # MissionPhase.value
+    detection_prompt = pyqtSignal(str)   # 偵測提示（含最近友機/距離）
+    status_message = pyqtSignal(str)
+
+    def __init__(self, make_executor, gps_provider, stop_groups_fn=None,
+                 r_detect=DEFAULT_R_DETECT,
+                 detect_dwell_sec=DEFAULT_DETECT_DWELL,
+                 r_engage=DEFAULT_R_ENGAGE,
+                 r_engage_exit=DEFAULT_R_ENGAGE_EXIT,
+                 replan_move_m=DEFAULT_REPLAN_MOVE_M,
+                 replan_cap_sec=DEFAULT_REPLAN_CAP_SEC,
+                 circle_radius=DEFAULT_CIRCLE_RADIUS,
+                 lon_spacing=DEFAULT_LON_SPACING,
+                 lat_offset=DEFAULT_LAT_OFFSET,
+                 formation_alt=DEFAULT_FORMATION_ALT,
+                 alt_layer_gap=DEFAULT_ALT_LAYER_GAP,
+                 min_alt_floor=DEFAULT_MIN_ALT_FLOOR,
+                 lead_time_cap_sec=DEFAULT_LEAD_TIME_CAP,
+                 friendly_speed=DEFAULT_FRIENDLY_SPEED,
+                 enemy_vel_window_sec=DEFAULT_ENEMY_VEL_WINDOW,
+                 parent=None):
+        super().__init__(parent)
+        self._make_executor = make_executor
+        self._gps_provider = gps_provider
+        self._stop_groups_fn = stop_groups_fn
+        self.r_detect = float(r_detect)
+        self.detect_dwell_sec = float(detect_dwell_sec)
+        self.r_engage = float(r_engage)
+        self.r_engage_exit = float(r_engage_exit)
+        self.replan_move_m = float(replan_move_m)
+        self.replan_cap_sec = float(replan_cap_sec)
+        self.circle_radius = float(circle_radius)
+        self.lon_spacing = float(lon_spacing)
+        self.lat_offset = float(lat_offset)
+        self.formation_alt = float(formation_alt)
+        self.alt_layer_gap = float(alt_layer_gap)
+        self.min_alt_floor = float(min_alt_floor)
+        self.lead_time_cap_sec = float(lead_time_cap_sec)
+        self.friendly_speed = float(friendly_speed)
+        self.enemy_vel_window_sec = float(enemy_vel_window_sec)
+
+        self.phase = MissionPhase.IDLE
+        self.a_drone_ids = []   # 偵察組
+        self.b_drone_ids = []   # 待機組
+        self.enemy_id = None
+
+        self._detect_since = 0.0
+        self._merged_ids = []           # PURSUE/ENCIRCLE 的 4 機（領隊優先，固定順序）
+        self._merged_ex = None          # 合併 executor（hover-hold）
+        self._last_replan_at = 0.0
+        self._last_plan_enemy = None    # 上次重算用的敵機 (lat, lon)
+        self._last_heading = (0.0, 1.0)  # 上次隊形朝向（敵機太近時沿用，避免抖動）
+        self._enemy_hist = []           # [(t, lat, lon)] 敵機軌跡（估速用）
+        self._enemy_vel = (0.0, 0.0)    # 估得的敵速 (east, north) m/s
+
+    # ------------------------------------------------------------- 設定（可連續呼叫）
+    def configure(self, a_drone_ids, b_drone_ids, enemy_id):
+        """設定 A/B 組與敵機。監看/待機階段可隨時更新；進入 PURSUE 後鎖定不受影響。"""
+        if self.phase in (MissionPhase.PURSUE, MissionPhase.ENCIRCLE):
+            return False
+        self.a_drone_ids = list(a_drone_ids)
+        self.b_drone_ids = list(b_drone_ids)
+        new_enemy = enemy_id or None
+        if new_enemy != self.enemy_id:
+            self._detect_since = 0.0  # 換敵機重置 dwell
+        self.enemy_id = new_enemy
+        return True
+
+    def _configured(self):
+        return bool(self.enemy_id) and bool(self.a_drone_ids) \
+            and self.enemy_id not in set(self.a_drone_ids) | set(self.b_drone_ids) \
+            and not (set(self.a_drone_ids) & set(self.b_drone_ids))
+
+    # ------------------------------------------------------------- 重編隊 / 停止
+    def confirm_detection(self):
+        """DETECTED → PURSUE：操作員手動確認。停掉群組掃描，接管 4 機瞬時追蹤。"""
+        if self.phase != MissionPhase.DETECTED:
+            self.status_message.emit("目前不在待確認狀態")
+            return
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            self.status_message.emit("敵機定位遺失，無法重編隊")
+            return
+        merged = self._merged_ids_leader_first(gps, epos)
+        if self._positions_for(merged, gps) is None:
+            self.status_message.emit("有無人機尚未定位，無法重編隊")
+            return
+
+        # 先停掉操作員的 A/B 群組任務，避免與合併 executor 雙重送點塞爆 adapter
+        if self._stop_groups_fn:
+            try:
+                self._stop_groups_fn()
+            except Exception as e:
+                _log("WARN", f"[orchestrator] 停止群組任務失敗: {e}")
+
+        self._merged_ids = merged
+        self._last_heading = (0.0, 1.0)
+        self._enemy_hist = []
+        self._enemy_vel = (0.0, 0.0)
+        self._update_enemy_velocity(epos)
+        slots = self._pursue_slots(gps, epos)
+        self._merged_ex = self._make_executor()
+        self._merged_ex.start(self._slots_to_plan(slots))
+        self._mark_replanned(epos)
+        self._set_phase(MissionPhase.PURSUE)
+        self.status_message.emit("重編隊確認：4 機前進中（B 追上會合）")
+
+    def stop(self):
+        """全體停止，回 IDLE。"""
+        if self._merged_ex is not None:
+            try:
+                self._merged_ex.stop()
+            except Exception:
+                pass
+            self._merged_ex = None
+        self._merged_ids = []
+        self._detect_since = 0.0
+        self._enemy_hist = []
+        self._enemy_vel = (0.0, 0.0)
+        self._set_phase(MissionPhase.IDLE)
+        self.status_message.emit("任務已停止")
+
+    # ------------------------------------------------------------- tick
+    def tick(self):
+        """GUI 週期呼叫。"""
+        # 自動進出監看（偵察飛行由操作員群組任務負責，這裡只監看偵測）
+        if self.phase == MissionPhase.IDLE and self._configured():
+            self._set_phase(MissionPhase.WATCH)
+        elif self.phase == MissionPhase.WATCH and not self._configured():
+            self._detect_since = 0.0
+            self._set_phase(MissionPhase.IDLE)
+
+        if self.phase == MissionPhase.WATCH:
+            self._tick_watch()
+        elif self.phase == MissionPhase.PURSUE:
+            self._tick_pursue()
+        elif self.phase == MissionPhase.ENCIRCLE:
+            self._tick_encircle()
+
+    def _tick_watch(self):
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            self._detect_since = 0.0
+            return
+        nearest_id, nearest_d = None, None
+        for did in self.a_drone_ids:
+            pos = gps.get(did)
+            if not pos:
+                continue
+            d = _haversine(epos['lat'], epos['lon'], pos['lat'], pos['lon'])
+            if nearest_d is None or d < nearest_d:
+                nearest_id, nearest_d = did, d
+
+        now = time.monotonic()
+        if nearest_d is not None and nearest_d <= self.r_detect:
+            if self._detect_since == 0.0:
+                self._detect_since = now
+            elif now - self._detect_since >= self.detect_dwell_sec:
+                self._set_phase(MissionPhase.DETECTED)  # latch，等操作員手動確認
+                self.detection_prompt.emit(
+                    f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認重編隊")
+        else:
+            self._detect_since = 0.0
+
+    def _tick_pursue(self):
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            return  # 敵機定位遺失（S4 加 stale-timeout hold）
+        self._update_enemy_velocity(epos)
+        d = self._centroid_dist(gps, epos)
+        if d is None:
+            return
+        if d <= self.r_engage:
+            self._apply_slots(self._circle_slots(gps, epos), epos)
+            self._set_phase(MissionPhase.ENCIRCLE)
+            self.status_message.emit("進入包圍")
+            return
+        if self._should_replan(time.monotonic(), epos):
+            self._apply_slots(self._pursue_slots(gps, epos), epos)
+
+    def _tick_encircle(self):
+        gps = self._gps_provider()
+        epos = gps.get(self.enemy_id)
+        if not epos:
+            return
+        self._update_enemy_velocity(epos)
+        d = self._centroid_dist(gps, epos)
+        if d is None:
+            return
+        if d > self.r_engage_exit:
+            self._apply_slots(self._pursue_slots(gps, epos), epos)
+            self._set_phase(MissionPhase.PURSUE)
+            self.status_message.emit("敵機逃離，退回前進")
+            return
+        if self._should_replan(time.monotonic(), epos):
+            self._apply_slots(self._circle_slots(gps, epos), epos)
+
+    # ------------------------------------------------------------- 高度分層
+    def _alt_for(self, did):
+        """S3 高度分層：A 組（含領隊）走上層、B 組走下層（不低於地面下限）。
+        同高度繞圈/追擊會交叉撞機，分層後 A/B 垂直錯開自然避開。"""
+        if did in set(self.b_drone_ids):
+            return max(self.min_alt_floor,
+                       self.formation_alt - self.alt_layer_gap / 2.0)
+        return self.formation_alt + self.alt_layer_gap / 2.0
+
+    # ------------------------------------------------------------- 前置攔截（估敵速）
+    def _update_enemy_velocity(self, epos):
+        """用敵機 GPS 差分估水平速度 (east, north) m/s，視窗 enemy_vel_window_sec。"""
+        now = time.monotonic()
+        self._enemy_hist.append((now, epos['lat'], epos['lon']))
+        while (len(self._enemy_hist) >= 2
+               and now - self._enemy_hist[0][0] > self.enemy_vel_window_sec):
+            self._enemy_hist.pop(0)
+        if len(self._enemy_hist) >= 2:
+            t0, la0, lo0 = self._enemy_hist[0]
+            t1, la1, lo1 = self._enemy_hist[-1]
+            dt = t1 - t0
+            if dt > 1e-3:
+                dx, dy = _ll_to_m(la1, lo1, la0, lo0)  # sample0→sample1 位移 (m)
+                self._enemy_vel = (dx / dt, dy / dt)
+
+    def enemy_speed(self):
+        """估得的敵機水平速率 (m/s)。"""
+        return math.hypot(*self._enemy_vel)
+
+    def _aim_point(self, gps, epos):
+        """前置攔截點：敵位 + 敵速×lead_time。lead_time = gap/friendly_speed（封頂
+        lead_time_cap_sec）。敵速尚未估到時退化為瞄當下位置（等同原純追尾）。"""
+        evx, evy = self._enemy_vel
+        lp = gps.get(self._merged_ids[0]) if self._merged_ids else None
+        gap = (_haversine(lp['lat'], lp['lon'], epos['lat'], epos['lon'])
+               if lp else 0.0)
+        lead_t = min(self.lead_time_cap_sec, gap / max(self.friendly_speed, 0.1))
+        return _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon'])
+
+    # ------------------------------------------------------------- 瞬時隊形
+    def _pursue_slots(self, gps, epos):
+        """朝『前置攔截點』的瞬時 leader-follow：領隊朝攔截點，僚機沿隊軸後方 rank×間距、左右錯開。"""
+        ids = self._merged_ids
+        tlat, tlon = self._aim_point(gps, epos)  # 前置攔截點取代敵機當下位置
+        lp = gps.get(ids[0])
+        if lp is None:
+            return None
+        lx, ly = _ll_to_m(lp['lat'], lp['lon'], tlat, tlon)  # 領隊相對攔截點
+        hx, hy = -lx, -ly                                    # 領隊 → 攔截點
+        n = math.hypot(hx, hy)
+        if n < 1.0:
+            h = self._last_heading  # 太近，沿用上次朝向避免抖動
+        else:
+            h = (hx / n, hy / n)
+        self._last_heading = h
+        px, py = -h[1], h[0]  # 左垂直
+        slots = {}
+        for rank, d in enumerate(ids):
+            lat_sign = -1 if rank % 2 == 0 else 1
+            # 領隊(rank0)目標≈攔截點；僚機沿 -h 後退 rank×間距，並左右錯開
+            sx = -h[0] * (rank * self.lon_spacing) + px * (lat_sign * self.lat_offset)
+            sy = -h[1] * (rank * self.lon_spacing) + py * (lat_sign * self.lat_offset)
+            lat, lon = _m_to_ll(sx, sy, tlat, tlon)
+            slots[d] = (lat, lon, self._alt_for(d))
+        return slots
+
+    def _circle_slots(self, gps, epos):
+        """繞敵機的瞬時包圍：N 機均分在半徑 circle_radius 的圓周上。"""
+        ids = self._merged_ids
+        elat, elon = epos['lat'], epos['lon']
+        n = len(ids)
+        slots = {}
+        for i, d in enumerate(ids):
+            theta = 2.0 * math.pi * i / max(1, n)
+            x = self.circle_radius * math.cos(theta)
+            y = self.circle_radius * math.sin(theta)
+            lat, lon = _m_to_ll(x, y, elat, elon)
+            slots[d] = (lat, lon, self._alt_for(d))
+        return slots
+
+    def _apply_slots(self, slots, epos):
+        """把瞬時 slot 熱替換給合併 executor 並更新重算基準。"""
+        if slots is None or self._merged_ex is None:
+            return
+        self._merged_ex.update_plan(self._slots_to_plan(slots))
+        self._mark_replanned(epos)
+
+    def _slots_to_plan(self, slots):
+        return {
+            'drone_ids': list(self._merged_ids),
+            'waypoints': [[slots[d]] for d in self._merged_ids],
+            'rendezvous_indices': [],
+        }
+
+    # ------------------------------------------------------------- 內部
+    def _centroid_dist(self, gps, epos):
+        m_pos = self._positions_for(self._merged_ids, gps)
+        if m_pos is None:
+            return None
+        cen_lat = sum(p[0] for p in m_pos) / len(m_pos)
+        cen_lon = sum(p[1] for p in m_pos) / len(m_pos)
+        return _haversine(cen_lat, cen_lon, epos['lat'], epos['lon'])
+
+    def _positions_for(self, drone_ids, gps):
+        out = []
+        for did in drone_ids:
+            p = gps.get(did)
+            if not p:
+                return None
+            out.append((p['lat'], p['lon'], p.get('alt', 0.0)))
+        return out
+
+    def _merged_ids_leader_first(self, gps, epos):
+        """A+B 合併；A 中離敵最近者當領隊(rank0)，其餘 A 維持原序，再接 B。"""
+        def dist(did):
+            p = gps.get(did)
+            return float('inf') if not p else _haversine(
+                epos['lat'], epos['lon'], p['lat'], p['lon'])
+        if not self.a_drone_ids:
+            return list(self.b_drone_ids)
+        leader = min(self.a_drone_ids, key=dist)
+        rest_a = [d for d in self.a_drone_ids if d != leader]
+        return [leader] + rest_a + list(self.b_drone_ids)
+
+    def _mark_replanned(self, epos):
+        self._last_replan_at = time.monotonic()
+        self._last_plan_enemy = (epos['lat'], epos['lon'])
+
+    def _should_replan(self, now, epos):
+        if self._last_plan_enemy is None:
+            return True
+        moved = _haversine(self._last_plan_enemy[0], self._last_plan_enemy[1],
+                           epos['lat'], epos['lon'])
+        return (moved >= self.replan_move_m
+                or (now - self._last_replan_at) >= self.replan_cap_sec)
+
+    def _set_phase(self, phase):
+        if phase == self.phase:
+            return
+        self.phase = phase
+        _log("INFO", f"[orchestrator] phase → {phase.value}")
+        self.phase_changed.emit(phase.value)
+
+
+class EngagementPanel(QWidget):
+    """
+    交戰控制面板。
+
+    - 敵機下拉 + 即時「敵機↔最近友機」距離
+    - A/B 組指派下拉
+    - 階段顯示、重編隊 / 停止 按鈕
+
+    偵察飛行用既有的群組執行按鈕控制，這裡不含「開始」。面板只發訊號，邏輯在 GUI/orchestrator。
+    """
+
+    enemy_changed = pyqtSignal(str)        # drone_id（未選擇=空字串）
+    confirm_requested = pyqtSignal()       # 重編隊
+    stop_requested = pyqtSignal()
+    groups_changed = pyqtSignal(str, str)  # (a_group_id, b_group_id)
+    apply_speed_requested = pyqtSignal(float, float)  # (friendly_speed, enemy_ratio)
+
+    def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT,
+                 friendly_speed=DEFAULT_FRIENDLY_SPEED):
+        super().__init__(parent)
+        self.r_detect = float(r_detect)
+        self._init_friendly_speed = float(friendly_speed)
+        self._enemy_id = ""
+        self._suppress_enemy = False
+        self._suppress_group = False
+        self._group_sig = None  # 目前下拉內容的簽章，變了才重建（避免 5Hz 狂刷）
+        self._build_ui()
+
+    # ------------------------------------------------------------------ UI
+    def _build_ui(self):
+        root = QVBoxLayout(self)
+        root.setContentsMargins(12, 12, 12, 12)
+        root.setSpacing(10)
+        root.setAlignment(Qt.AlignmentFlag.AlignTop)
+
+        title = QLabel("交戰設定")
+        title.setStyleSheet(
+            "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
+        root.addWidget(title)
+
+        combo_css = (
+            "QComboBox { background-color: #333; color: #EEE; border: 1px solid #555;"
+            " border-radius: 3px; padding: 3px 6px; font-size: 12px; min-width: 120px; }")
+
+        enemy_row = QHBoxLayout()
+        enemy_row.setSpacing(8)
+        el = QLabel("敵機 sysid：")
+        el.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.enemy_combo = QComboBox()
+        self.enemy_combo.setStyleSheet(combo_css)
+        self.enemy_combo.addItem(_NONE_LABEL)
+        self.enemy_combo.currentTextChanged.connect(self._on_enemy_changed)
+        enemy_row.addWidget(el)
+        enemy_row.addWidget(self.enemy_combo)
+        enemy_row.addStretch()
+        root.addLayout(enemy_row)
+
+        a_row = QHBoxLayout()
+        a_row.setSpacing(8)
+        al = QLabel("偵察組 A：")
+        al.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.a_group_combo = QComboBox()
+        self.a_group_combo.setStyleSheet(combo_css)
+        self.a_group_combo.addItem(_NONE_LABEL, None)
+        self.a_group_combo.currentTextChanged.connect(self._on_group_changed)
+        a_row.addWidget(al)
+        a_row.addWidget(self.a_group_combo)
+        a_row.addStretch()
+        root.addLayout(a_row)
+
+        b_row = QHBoxLayout()
+        b_row.setSpacing(8)
+        bl = QLabel("待機組 B：")
+        bl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.b_group_combo = QComboBox()
+        self.b_group_combo.setStyleSheet(combo_css)
+        self.b_group_combo.addItem(_NONE_LABEL, None)
+        self.b_group_combo.currentTextChanged.connect(self._on_group_changed)
+        b_row.addWidget(bl)
+        b_row.addWidget(self.b_group_combo)
+        b_row.addStretch()
+        root.addLayout(b_row)
+
+        # 速度比例限速：友方 V、敵方 r×V，按鈕一次套用（DO_CHANGE_SPEED）
+        spin_css = (
+            "QDoubleSpinBox { background-color: #333; color: #EEE;"
+            " border: 1px solid #555; border-radius: 3px; padding: 2px 4px;"
+            " font-size: 12px; min-width: 70px; }")
+        speed_row = QHBoxLayout()
+        speed_row.setSpacing(8)
+        sl = QLabel("友機速度：")
+        sl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.friendly_speed_spin = QDoubleSpinBox()
+        self.friendly_speed_spin.setStyleSheet(spin_css)
+        self.friendly_speed_spin.setRange(0.5, 20.0)
+        self.friendly_speed_spin.setSingleStep(0.5)
+        self.friendly_speed_spin.setDecimals(1)
+        self.friendly_speed_spin.setSuffix(" m/s")
+        self.friendly_speed_spin.setValue(self._init_friendly_speed)
+        rl = QLabel("敵/友比例：")
+        rl.setStyleSheet("color: #CCC; font-size: 12px;")
+        self.enemy_ratio_spin = QDoubleSpinBox()
+        self.enemy_ratio_spin.setStyleSheet(spin_css)
+        self.enemy_ratio_spin.setRange(0.1, 1.0)
+        self.enemy_ratio_spin.setSingleStep(0.05)
+        self.enemy_ratio_spin.setDecimals(2)
+        self.enemy_ratio_spin.setValue(0.6)
+        speed_row.addWidget(sl)
+        speed_row.addWidget(self.friendly_speed_spin)
+        speed_row.addWidget(rl)
+        speed_row.addWidget(self.enemy_ratio_spin)
+        speed_row.addStretch()
+        root.addLayout(speed_row)
+
+        apply_speed_row = QHBoxLayout()
+        self.apply_speed_btn = QPushButton("套用速度")
+        self.apply_speed_btn.setStyleSheet(
+            "QPushButton { background-color: #00796B; color: white; border: none;"
+            " padding: 6px 14px; border-radius: 4px; font-weight: bold; }"
+            " QPushButton:hover { background-color: #00897B; }")
+        self.apply_speed_btn.clicked.connect(self._on_apply_speed)
+        self.enemy_speed_hint = QLabel("")
+        self.enemy_speed_hint.setStyleSheet("color: #777; font-size: 11px;")
+        self._refresh_enemy_speed_hint()
+        self.friendly_speed_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
+        self.enemy_ratio_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
+        apply_speed_row.addWidget(self.apply_speed_btn)
+        apply_speed_row.addWidget(self.enemy_speed_hint)
+        apply_speed_row.addStretch()
+        root.addLayout(apply_speed_row)
+
+        sep = QFrame()
+        sep.setFrameShape(QFrame.Shape.HLine)
+        sep.setStyleSheet("color: #444;")
+        root.addWidget(sep)
+
+        self.distance_label = QLabel("敵機 ↔ 最近友機：—")
+        self.distance_label.setStyleSheet(
+            "color: #AAA; font-size: 13px; padding: 4px 2px;")
+        root.addWidget(self.distance_label)
+
+        hint = QLabel(f"（偵測範圍 R_detect = {self.r_detect:.0f} m）")
+        hint.setStyleSheet("color: #777; font-size: 11px;")
+        root.addWidget(hint)
+
+        self.phase_label = QLabel("階段：待機")
+        self.phase_label.setStyleSheet(
+            "color: #64B5F6; font-size: 13px; font-weight: bold; padding: 4px 2px;")
+        root.addWidget(self.phase_label)
+
+        btn_css = ("QPushButton {{ background-color: {bg}; color: {fg}; border: none;"
+                   " padding: 8px 16px; border-radius: 4px; font-weight: bold; }}"
+                   " QPushButton:hover {{ background-color: {hover}; }}"
+                   " QPushButton:disabled {{ background-color: #444; color: #888; }}")
+        btn_row = QHBoxLayout()
+        btn_row.setSpacing(8)
+        self.confirm_btn = QPushButton("重編隊")
+        self.confirm_btn.setStyleSheet(btn_css.format(bg='#F57C00', fg='white', hover='#FB8C00'))
+        self.confirm_btn.clicked.connect(lambda: self.confirm_requested.emit())
+        self.confirm_btn.setEnabled(False)
+        self.stop_btn = QPushButton("停止")
+        self.stop_btn.setStyleSheet(btn_css.format(bg='#555', fg='#DDD', hover='#666'))
+        self.stop_btn.clicked.connect(lambda: self.stop_requested.emit())
+        btn_row.addWidget(self.confirm_btn)
+        btn_row.addWidget(self.stop_btn)
+        btn_row.addStretch()
+        root.addLayout(btn_row)
+
+    # -------------------------------------------------------------- 對外 API
+    def current_enemy(self):
+        return self._enemy_id or None
+
+    def refresh_drone_list(self, drone_ids):
+        items = [_NONE_LABEL] + sorted(drone_ids)
+        current = self._enemy_id if self._enemy_id in drone_ids else ""
+        self._suppress_enemy = True
+        self.enemy_combo.clear()
+        self.enemy_combo.addItems(items)
+        idx = self.enemy_combo.findText(current) if current else 0
+        self.enemy_combo.setCurrentIndex(idx if idx >= 0 else 0)
+        self._suppress_enemy = False
+        if current != self._enemy_id:
+            self._enemy_id = current
+            self.enemy_changed.emit(self._enemy_id)
+
+    def refresh_group_list(self, group_labels):
+        """group_labels: list[(group_id, display_text)]。只有內容變動才重建，避免 5Hz 狂刷。"""
+        sig = tuple(group_labels)
+        if sig == self._group_sig:
+            return
+        self._group_sig = sig
+        self._suppress_group = True
+        for combo in (self.a_group_combo, self.b_group_combo):
+            prev = combo.currentData()
+            combo.clear()
+            combo.addItem(_NONE_LABEL, None)
+            for gid, text in group_labels:
+                combo.addItem(text, gid)
+            if prev is not None:
+                i = combo.findData(prev)
+                combo.setCurrentIndex(i if i >= 0 else 0)
+        self._suppress_group = False
+
+    def selected_group_ids(self):
+        return self.a_group_combo.currentData(), self.b_group_combo.currentData()
+
+    def set_phase_text(self, text):
+        self.phase_label.setText(f"階段：{text}")
+
+    def set_confirm_enabled(self, enabled):
+        self.confirm_btn.setEnabled(enabled)
+
+    def tick(self, drone_positions):
+        """{drone_id: (lat, lon)} 更新距離讀數。"""
+        enemy = self._enemy_id
+        base = "color: #AAA; font-size: 13px; padding: 4px 2px;"
+        if not enemy:
+            self.distance_label.setText("敵機 ↔ 最近友機：（尚未選擇敵機）")
+            self.distance_label.setStyleSheet(base)
+            return
+        epos = drone_positions.get(enemy)
+        if not epos:
+            self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（等待敵機定位）")
+            self.distance_label.setStyleSheet(base)
+            return
+        nearest_id, nearest_d = None, None
+        for did, pos in drone_positions.items():
+            if did == enemy or not pos:
+                continue
+            d = _haversine(epos[0], epos[1], pos[0], pos[1])
+            if nearest_d is None or d < nearest_d:
+                nearest_id, nearest_d = did, d
+        if nearest_id is None:
+            self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（無友機定位）")
+            self.distance_label.setStyleSheet(base)
+            return
+        color = "#E57373" if nearest_d <= self.r_detect else "#81C784"
+        self.distance_label.setText(
+            f"敵機 {enemy} ↔ 最近友機 {nearest_id}：{nearest_d:.1f} m")
+        self.distance_label.setStyleSheet(
+            f"color: {color}; font-size: 13px; font-weight: bold; padding: 4px 2px;")
+
+    # ----------------------------------------------------------------- 內部
+    def _on_enemy_changed(self, text):
+        if self._suppress_enemy:
+            return
+        self._enemy_id = "" if text == _NONE_LABEL else text
+        _log("INFO", f"[交戰] 敵機選擇：{self._enemy_id or '（未選擇）'}")
+        self.enemy_changed.emit(self._enemy_id)
+
+    def _on_group_changed(self, _text):
+        if self._suppress_group:
+            return
+        a_id, b_id = self.selected_group_ids()
+        self.groups_changed.emit(a_id or "", b_id or "")
+
+    def _refresh_enemy_speed_hint(self, *_):
+        v = self.friendly_speed_spin.value()
+        r = self.enemy_ratio_spin.value()
+        self.enemy_speed_hint.setText(f"→ 友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s")
+
+    def _on_apply_speed(self):
+        v = self.friendly_speed_spin.value()
+        r = self.enemy_ratio_spin.value()
+        _log("INFO", f"[交戰] 套用速度：友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s (比例 {r:.2f})")
+        self.apply_speed_requested.emit(v, r)

src/GUI/mission_planner.py

@@ -1,323 +1,550 @@
 #!/usr/bin/env python3
 """
 飛行任務規劃模組
-負責將 GPS 點擊座標轉換為隊形飛行任務
+支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
 """
 import math
-from typing import List, Tuple, Optional
+from typing import List, Tuple, Optional, Dict, Any
+from enum import Enum
+
+
+class MissionType(Enum):
+    """任務類型"""
+    M_FORMATION = "m_formation"
+    CIRCLE_FORMATION = "circle_formation"
+    LEADER_FOLLOWER = "leader_follower"
+    GRID_SWEEP = "grid_sweep"
 
 
 class CoordinateConverter:
     """GPS 座標與 Local 座標的轉換器"""
-    
+
     def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
-        """
-        初始化座標轉換器
-        
-        Args:
-            origin_lat: 參考原點緯度
-            origin_lon: 參考原點經度
-            origin_alt: 參考原點高度（公尺，相對於海平面）
-        """
         self.origin_lat = origin_lat
         self.origin_lon = origin_lon
         self.origin_alt = origin_alt
-        self.R = 6371000.0  # 地球半徑（公尺）
+        self.R = 6371000.0
-    
+
     def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
-        """
-        GPS 座標轉換為 Local 座標（ENU 系統：East-North-Up）
-        
-        Args:
-            lat: 緯度
-            lon: 經度
-            alt: 高度（公尺，相對於海平面）
-        
-        Returns:
-            (x, y, z): Local 座標（公尺）
-                x: East（東方）
-                y: North（北方）
-                z: Up（向上）
-        """
         lat_rad = math.radians(lat)
         lon_rad = math.radians(lon)
         origin_lat_rad = math.radians(self.origin_lat)
         origin_lon_rad = math.radians(self.origin_lon)
-        
+
         dlat = lat_rad - origin_lat_rad
         dlon = lon_rad - origin_lon_rad
-        
+
-        # 使用 Equirectangular projection（適用於小範圍 < 10 km）
+        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
-        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)  # East
+        y = self.R * dlat
-        y = self.R * dlat  # North
+        z = alt - self.origin_alt
-        z = alt - self.origin_alt  # Up
+
-        
         return x, y, z
-    
+
     def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
-        """
-        Local 座標轉換為 GPS 座標
-        
-        Args:
-            x: East（東方，公尺）
-            y: North（北方，公尺）
-            z: Up（向上，公尺）
-        
-        Returns:
-            (lat, lon, alt): GPS 座標
-        """
         origin_lat_rad = math.radians(self.origin_lat)
         origin_lon_rad = math.radians(self.origin_lon)
-        
+
         lat_rad = origin_lat_rad + (y / self.R)
         lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
-        
+
         lat = math.degrees(lat_rad)
         lon = math.degrees(lon_rad)
         alt = self.origin_alt + z
-        
+
         return lat, lon, alt
 
 
 class FormationPlanner:
     """隊形規劃器"""
-    
+
-    def __init__(self, spacing: float = 5.0, 
+    def __init__(self, spacing: float = 5.0,
-                 base_altitude: float = 10.0, 
+                 base_altitude: float = 30.0,
                  altitude_diff: float = 2.0):
-        """
-        初始化隊形規劃器
-        
-        Args:
-            spacing: 無人機間距（公尺）
-            base_altitude: 基準飛行高度（公尺，相對於參考原點）
-            altitude_diff: M 字形的高低差（公尺）
-        """
         self.spacing = spacing
         self.base_altitude = base_altitude
         self.altitude_diff = altitude_diff
         self.current_origin = None
         self.converter = None
-    
+
-    def plan_formation_mission(self, 
+    def plan_formation_mission(self,
-                              drone_gps_positions: List[Tuple[float, float, float]], 
+                               drone_gps_positions: List[Tuple[float, float, float]],
-                              target_gps: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], 
+                               target_gps: Tuple[float, float, float],
-                                                                                List[Tuple[float, float, float]]]:
+                               mission_type: MissionType = MissionType.M_FORMATION,
+                               params: Optional[Dict[str, Any]] = None
+                               ) -> Tuple[List[List[Tuple[float, float, float]]],
+                                          Tuple[float, float, float],
+                                          List[int]]:
         """
-        規劃兩階段隊形任務
+        回傳 (waypoints_gps, origin, rendezvous_indices)。
-        
+        rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
-        Args:
-            drone_gps_positions: 當前無人機 GPS 位置列表 [(lat, lon, alt), ...]
-            target_gps: 目標點 GPS 座標 (lat, lon, alt)
-        
-        Returns:
-            stage1_gps: 階段 1（集合點）的 GPS 航點列表
-            stage2_gps: 階段 2（目標點）的 GPS 航點列表
-            origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt)
-            origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt)
         """
         if len(drone_gps_positions) == 0:
             raise ValueError("無人機位置列表不能為空")
-        
+
-        # ===== 步驟 1: 計算中央點並設為參考原點 =====
         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)
+
-        
+        # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
-        self.current_origin = (center_lat, center_lon, center_alt)
+        # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
-        self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
+        self.current_origin = (center_lat, center_lon, 0.0)
-        
+        self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
-        print(f"📍 參考原點: ({center_lat:.6f}, {center_lon:.6f}, {center_alt:.1f}m)")
+
-        
+        drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
-        # ===== 步驟 2: 轉換到 Local 座標系 =====
+        target_local = self.converter.gps_to_local(*target_gps)
-        drone_local_positions = []
+
-        for lat, lon, alt in drone_gps_positions:
+        if mission_type == MissionType.M_FORMATION:
-            x, y, z = self.converter.gps_to_local(lat, lon, alt)
+            s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
-            drone_local_positions.append((x, y, z))
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
-        
+            # 起點集合後一起出發到 stage2
-        target_local = self.converter.gps_to_local(
+            rendezvous_indices = [0]
-            target_gps[0], target_gps[1], target_gps[2]
+
-        )
+        elif mission_type == MissionType.CIRCLE_FORMATION:
-        
+            s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
-        # ===== 步驟 3: 在 Local 座標系中計算隊形 =====
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
-        stage1_local, stage2_local = self._calculate_formation_local(
+            # 半程集合後一起進最終圓環位置
-            drone_local_positions,
+            rendezvous_indices = [0]
-            target_local
+
-        )
+        elif mission_type == MissionType.LEADER_FOLLOWER:
-        
+            params = params or {}
-        # ===== 步驟 4: 轉回 GPS 座標 =====
+            route_wps_gps = params.get('route_waypoints')
-        stage1_gps = []
+            if route_wps_gps is None or len(route_wps_gps) < 2:
-        for x, y, z in stage1_local:
+                raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
-            lat, lon, alt = self.converter.local_to_gps(x, y, z)
+            route_wps_local = [
-            stage1_gps.append((lat, lon, alt))
+                self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
-        
+                for wp in route_wps_gps
-        stage2_gps = []
+            ]
-        for x, y, z in stage2_local:
+            waypoints_local, rendezvous_indices = self._calculate_leader_follower(
-            lat, lon, alt = self.converter.local_to_gps(x, y, z)
+                drone_local, route_wps_local, params
-            stage2_gps.append((lat, lon, alt))
+            )
-        
+
-        print(f"✅ 任務規劃完成: {len(stage1_gps)} 台無人機，2 階段飛行")
+        elif mission_type == MissionType.GRID_SWEEP:
-        
+            params = params or {}
-        # ================================================================================
+            rect_corners_gps = params.get('rect_corners')
-        # 【修改】回傳中心點座標供地圖視覺化使用
+            if rect_corners_gps is None or len(rect_corners_gps) != 4:
-        # ================================================================================
+                raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
-        return stage1_gps, stage2_gps, self.current_origin
+            rect_corners_local = [
-        # ================================================================================
+                self.converter.gps_to_local(c[0], c[1], 0)[:2]
-    
+                for c in rect_corners_gps
-    def _calculate_formation_local(self, 
+            ]
-                                   drone_positions: List[Tuple[float, float, float]], 
+            waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
-                                   target_point: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], 
+                drone_local, rect_corners_local, params
-                                                                                       List[Tuple[float, float, float]]]:
+            )
-        """
+        else:
-        在 Local 座標系中計算隊形
+            raise ValueError(f"不支援的任務類型: {mission_type}")
-        
+
-        Args:
+        waypoints_gps = []
-            drone_positions: Local 座標的無人機位置 [(x, y, z), ...]
+        for drone_wps in waypoints_local:
-            target_point: Local 座標的目標點 (x, y, z)
+            gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
-        
+            waypoints_gps.append(gps_wps)
-        Returns:
+
-            stage1_positions: 階段 1（中央點分佈）的 Local 座標
+        return waypoints_gps, self.current_origin, rendezvous_indices
-            stage2_positions: 階段 2（目標點分佈）的 Local 座標
+
-        """
+    # ------------------------------------------------------------------ M-Formation
+
+    def _calculate_m_formation(self, drone_positions, target_point, params):
+        params = params or {}
         N = len(drone_positions)
-        
+        spacing = params.get('spacing', self.spacing)
-        # ===== 步驟 1: 計算中央點（在 Local 座標系中應該接近原點）=====
+        base_altitude = params.get('base_altitude', self.base_altitude)
+        altitude_diff = params.get('altitude_diff', self.altitude_diff)
+
         C_x = sum(pos[0] for pos in drone_positions) / N
         C_y = sum(pos[1] for pos in drone_positions) / N
-        C_z = sum(pos[2] for pos in drone_positions) / N
+
-        
-        print(f"   中央點 Local: ({C_x:.2f}, {C_y:.2f}, {C_z:.2f})")
-        
-        # ===== 步驟 2: 計算方向向量（中央點 → 目標點）=====
         T_x, T_y, T_z = target_point
-        V_x = T_x - C_x
+        V_x, V_y = T_x - C_x, T_y - C_y
-        V_y = T_y - C_y
+        P_x, P_y = -V_y, V_x
-        
+
-        print(f"   方向向量: ({V_x:.2f}, {V_y:.2f})")
+        length = math.sqrt(P_x ** 2 + P_y ** 2)
-        
+        P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
-        # ===== 步驟 3: 計算垂直向量（逆時針旋轉 90°）=====
+
-        P_x = -V_y
+        projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
-        P_y = V_x
+                       for i, pos in enumerate(drone_positions)]
-        
-        # 單位化垂直向量
-        length = math.sqrt(P_x**2 + P_y**2)
-        if length < 0.01:  # 避免除以零
-            # 如果目標點與中央點重合，使用默認方向（向東）
-            P_x_unit, P_y_unit = 1.0, 0.0
-        else:
-            P_x_unit = P_x / length
-            P_y_unit = P_y / length
-        
-        print(f"   垂直單位向量: ({P_x_unit:.3f}, {P_y_unit:.3f})")
-        
-        # ===== 步驟 4: 計算無人機在垂直方向的投影並排序 =====
-        projections = []
-        for i, pos in enumerate(drone_positions):
-            relative_x = pos[0] - C_x
-            relative_y = pos[1] - C_y
-            projection = relative_x * P_x_unit + relative_y * P_y_unit
-            projections.append((projection, i))
-        
-        # 按投影值排序（保持左右順序）
         projections.sort()
-        sorted_indices = [idx for _, idx in projections]
+
-        
+        stage1_positions = [None] * N
-        print(f"   排序後的無人機索引: {sorted_indices}")
-        
-        # ===== 步驟 5: 分佈無人機位置 =====
-        stage1_positions = [None] * N  # 預分配列表
         stage2_positions = [None] * N
-        
+
-        for rank, original_idx in enumerate(sorted_indices):
+        for rank, (_, original_idx) in enumerate(projections):
-            # 計算相對中心的水平偏移
+            offset = (rank - (N - 1) / 2) * spacing
-            offset = (rank - (N - 1) / 2) * self.spacing
+            altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
-            
+            stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
-            # 計算 M 字形高度（交替高低）
+            stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
-            if rank % 2 == 0:
+
-                altitude = self.base_altitude + self.altitude_diff  # 高
-            else:
-                altitude = self.base_altitude - self.altitude_diff  # 低
-            
-            # 階段 1：在中央點附近分佈
-            stage1_x = C_x + P_x_unit * offset
-            stage1_y = C_y + P_y_unit * offset
-            stage1_z = altitude
-            
-            # 階段 2：在目標點附近分佈（保持相同的相對位置）
-            stage2_x = T_x + P_x_unit * offset
-            stage2_y = T_y + P_y_unit * offset
-            stage2_z = altitude
-            
-            # 按照原始索引存儲（保持無人機 ID 順序）
-            stage1_positions[original_idx] = (stage1_x, stage1_y, stage1_z)
-            stage2_positions[original_idx] = (stage2_x, stage2_y, stage2_z)
-        
         return stage1_positions, stage2_positions
-    
+
-    def update_parameters(self, spacing: Optional[float] = None, 
+    # ------------------------------------------------------------------ Circle
-                         base_altitude: Optional[float] = None, 
+
-                         altitude_diff: Optional[float] = None):
+    def _calculate_circle_formation(self, drone_positions, target_point, params):
+        params = params or {}
+        N = len(drone_positions)
+        radius = params.get('radius', 10.0)
+        altitude = params.get('altitude', 30.0)
+        start_angle = params.get('start_angle', 0.0)
+
+        center_x, center_y, center_z = target_point
+        stage1_positions = []
+        stage2_positions = []
+        angle_step = 360.0 / N
+
+        for i in range(N):
+            angle_rad = math.radians(start_angle + angle_step * i)
+            final_x = center_x + radius * math.cos(angle_rad)
+            final_y = center_y + radius * math.sin(angle_rad)
+            final_z = altitude
+
+            current_x, current_y, _current_z = drone_positions[i]
+            # stage1 高度直接用 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,
+                final_z,
+            ))
+            stage2_positions.append((final_x, final_y, final_z))
+
+        return stage1_positions, stage2_positions
+
+    # ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
+
+    def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
         """
-        更新隊形參數
+        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化
-        
+
-        Args:
+        center_path = 使用者畫的路徑點（直線段連接，不插弧），
-            spacing: 無人機間距（公尺）
+        每架 drone 沿弧長 s 參數化做隊形偏移：
-            base_altitude: 基準飛行高度（公尺）
+            s_follower = s_leader - lon_k
-            altitude_diff: M 字形的高低差（公尺）
+            position = lookup(s_follower) + lat_k × perp(tangent)
+        s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
+
+        Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
+        隊形 (俯視):
+            D0 (lat=-L, lon=0)   ← leader
+                D1 (lat=+L, lon=5)
+            D2 (lat=-L, lon=10)
+                D3 (lat=+L, lon=15)
         """
-        if spacing is not None:
+        N = len(drone_positions)
-            self.spacing = spacing
+        lateral_offset = params.get('lateral_offset', 3.0)
-            print(f"   間距更新為: {spacing} m")
+        longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
-        
+        altitude = params.get('altitude', self.base_altitude)
-        if base_altitude is not None:
+
-            self.base_altitude = base_altitude
+        # Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
-            print(f"   基準高度更新為: {base_altitude} m")
+        center_path, barrier_indices = self._build_center_path(route_wps_local)
-        
+
-        if altitude_diff is not None:
+        s_list = [pt['s'] for pt in center_path]
-            self.altitude_diff = altitude_diff
+        s_max = s_list[-1]
-            print(f"   高低差更新為: {altitude_diff} m")
+        n_pts = len(center_path)
+
+        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']
+
+            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 in range(N):
+            lat_sign = -1 if rank % 2 == 0 else 1
+            lat = lat_sign * lateral_offset
+            lon = rank * longitudinal_spacing
+
+            waypoints = []
+            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
+                waypoints.append((
+                    x + lat * perp_x,
+                    y + lat * perp_y,
+                    altitude,
+                ))
+
+            all_waypoints[rank] = waypoints
+
+        return all_waypoints, barrier_indices
+
+    def _build_center_path(self, waypoints):
+        """
+        建立中心路徑 — 直接用路徑點連接，不插弧。
+
+        每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
+        飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。
+
+        Returns:
+            (path, barrier_indices)
+        """
+        num_wps = len(waypoints)
+
+        if num_wps < 2:
+            return [{
+                'seg': 'straight',
+                'x': waypoints[0][0], 'y': waypoints[0][1],
+                'dir': (0.0, 1.0),
+                's': 0.0,
+            }], []
+
+        path = []
+        barrier_indices = []
+
+        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]
+
+            length = math.hypot(dx, dy)
+            if length < 0.01:
+                direction = (0.0, 1.0)
+            else:
+                direction = (dx / length, dy / length)
+
+            path.append({
+                'seg': 'straight',
+                'x': waypoints[i][0], 'y': waypoints[i][1],
+                'dir': direction,
+            })
+
+            if 0 < i < num_wps - 1:
+                barrier_indices.append(len(path) - 1)
+
+        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']
+            )
+
+        return path, barrier_indices
+
+    # ------------------------------------------------------------------ 柵狀掃描
+
+    def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
+        """柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊"""
+        N = len(drone_positions)
+        line_spacing = params.get('line_spacing', 5.0)
+        altitude = params.get('altitude', self.base_altitude)
+
+        c0, c1, c2, c3 = rect_corners_local
+
+        edge_a = (c1[0] - c0[0], c1[1] - c0[1])
+        len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
+        edge_b = (c3[0] - c0[0], c3[1] - c0[1])
+        len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)
+
+        if len_a >= len_b:
+            sweep_vec = edge_a
+            sweep_len = len_a
+            cut_vec = edge_b
+            cut_len = len_b
+            sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
+            sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
+            cut_start_corner = c0
+        else:
+            sweep_vec = edge_b
+            sweep_len = len_b
+            cut_vec = edge_a
+            cut_len = len_a
+            sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
+            sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
+            cut_start_corner = c0
+
+        sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
+        cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)
+
+        C_x = sum(p[0] for p in drone_positions) / N
+        C_y = sum(p[1] for p in drone_positions) / N
+
+        dist_to_start = math.sqrt(
+            (C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
+        )
+        dist_to_end = math.sqrt(
+            (C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
+        )
+
+        if dist_to_start <= dist_to_end:
+            near_corner_a = cut_start_corner
+        else:
+            sweep_dir = (-sweep_dir[0], -sweep_dir[1])
+            near_corner_a = (cut_start_corner[0] + sweep_vec[0],
+                            cut_start_corner[1] + sweep_vec[1])
+
+        projections = [
+            ((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
+            for i, pos in enumerate(drone_positions)
+        ]
+        projections.sort()
+
+        strip_width = cut_len / N
+        drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
+        near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
+        gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2
+
+        all_waypoints = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            strip_center_offset = (rank + 0.5) * strip_width
+            base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
+            base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset
+
+            waypoints_list = []
+
+            gather_offset = gather_sweep_proj - near_sweep_proj
+            gx = base_x + sweep_dir[0] * gather_offset
+            gy = base_y + sweep_dir[1] * gather_offset
+            waypoints_list.append((gx, gy, altitude))
+
+            num_lines = max(1, int(strip_width / line_spacing))
+            actual_spacing = strip_width / num_lines
+            first_line_offset = (rank * strip_width) + actual_spacing / 2
+
+            entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
+            entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
+            waypoints_list.append((entry_x, entry_y, altitude))
+
+            current_cut_offset = first_line_offset
+
+            for line_idx in range(num_lines):
+                line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
+                line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
+                line_far_x = line_near_x + sweep_dir[0] * sweep_len
+                line_far_y = line_near_y + sweep_dir[1] * sweep_len
+
+                if line_idx % 2 == 0:
+                    waypoints_list.append((line_far_x, line_far_y, altitude))
+                else:
+                    waypoints_list.append((line_near_x, line_near_y, altitude))
+
+                if line_idx < num_lines - 1:
+                    next_cut_offset = current_cut_offset + actual_spacing
+                    next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
+                    next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
+                    next_far_x = next_near_x + sweep_dir[0] * sweep_len
+                    next_far_y = next_near_y + sweep_dir[1] * sweep_len
+
+                    if line_idx % 2 == 0:
+                        waypoints_list.append((next_far_x, next_far_y, altitude))
+                    else:
+                        waypoints_list.append((next_near_x, next_near_y, altitude))
+                    current_cut_offset = next_cut_offset
+
+            all_waypoints[original_idx] = waypoints_list
+
+        # 每一條掃描線的「起點」都做一次同步：
+        #   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
 
 
-# ===== 測試程式碼 =====
+# ================================================================================
+# 測試
+# ================================================================================
 if __name__ == "__main__":
-    print("=" * 60)
+    planner = FormationPlanner()
-    print("隊形任務規劃器測試")
+
-    print("=" * 60)
+    drones = [
-    
+        (24.123450, 120.567800, 0.0),
-    # 模擬 5 台無人機的 GPS 位置
+        (24.123470, 120.567820, 0.0),
-    drone_gps = [
+        (24.123440, 120.567810, 0.0),
-        (24.123450, 120.567800, 100.0),  # 無人機 0
+        (24.123460, 120.567830, 0.0),
-        (24.123470, 120.567820, 102.0),  # 無人機 1
-        (24.123440, 120.567810, 98.0),   # 無人機 2
-        (24.123460, 120.567830, 101.0),  # 無人機 3
-        (24.123445, 120.567795, 99.0),   # 無人機 4
     ]
-    
+    target = (24.12400, 120.56795, 10.0)
-    # 目標點
+
-    target_gps = (24.123600, 120.568000, 105.0)
+    # M-Formation
-    
+    wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
-    # 建立規劃器
+    print("M-Formation:")
-    planner = FormationPlanner(
+    for i, wp_list in enumerate(wps):
-        spacing=5.0,           # 5 公尺間距
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
-        base_altitude=10.0,    # 基準高度 10 公尺
+
-        altitude_diff=2.0      # 高低差 2 公尺
+    # Leader-Follower (虛擬領隊 / 弧長參數化)
+    route = [
+        (24.12345, 120.56780),
+        (24.12370, 120.56800),
+        (24.12390, 120.56820),
+        (24.12400, 120.56800),
+        (24.12420, 120.56790),
+    ]
+    wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
+        drones, target, MissionType.LEADER_FOLLOWER,
+        params={
+            'route_waypoints': route,
+            'lateral_offset': 3.0,
+            'longitudinal_spacing': 5.0,
+            'min_inner_radius': 3.0,
+            'arc_resolution': 8,
+            'altitude': 10.0
+        }
     )
-    
+    print(f"\nLeader-Follower (arc-length virtual leader):")
-    # 規劃任務
+    for i, wp_list in enumerate(wps_lf):
-    print("\n開始規劃任務...")
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
-    stage1, stage2 = planner.plan_formation_mission(drone_gps, target_gps)
+        for j, wp in enumerate(wp_list):
-    
+            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
-    # 顯示結果
+
-    print("\n" + "=" * 60)
+    # Grid Sweep
-    print("階段 1：集合點位置（GPS）")
+    rect = [
-    print("=" * 60)
+        (24.1237, 120.5679),
-    for i, (lat, lon, alt) in enumerate(stage1):
+        (24.1237, 120.5683),
-        print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)")
+        (24.1240, 120.5683),
-    
+        (24.1240, 120.5679)
-    print("\n" + "=" * 60)
+    ]
-    print("階段 2：目標點位置（GPS）")
+    wps2, origin2, rv2 = planner.plan_formation_mission(
-    print("=" * 60)
+        drones, target, MissionType.GRID_SWEEP,
-    for i, (lat, lon, alt) in enumerate(stage2):
+        params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
-        print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)")
+    )
-    
+    print(f"\nGrid Sweep:")
-    print("\n✅ 測試完成！")
+    for i, wp_list in enumerate(wps2):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")

src/GUI/overview_table.py

@@ -6,29 +6,38 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向",
+    # 「任務狀態」放最上面：閉環跑點時一眼看每台的 WP 進度 + 通訊(送出)狀態。
-                          "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+    # 此列不來自遙測 QLabel，由 MissionExecutor 訊號經 update_table(field='mission') 餵入。
-    
+    DEFAULT_INFO_TYPES = ["任務狀態",
+                          "模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
+                          "高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
+                          "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+
     DEFAULT_INFO_TYPE_MAP = {
-        "mode": 0,
+        "mission": 0,
-        "armed": 1,
+        "mode": 1,
-        "battery": 2,
+        "armed": 2,
-        "longitude": 3,
+        "battery": 3,
-        "latitude": 4,
+        "longitude": 4,
-        "altitude": 5,
+        "latitude": 5,
-        "local": 6,
+        "fix_type": 6,
-        "velocity": 7,
+        "satellites_visible": 7,
-        "groundspeed": 8,
+        "eph": 8,
-        "heading": 9,
+        "epv": 9,
-        "airspeed": 10,
+        "altitude": 10,
-        "throttle": 11,
+        "local": 11,
-        "hud_alt": 12,
+        "velocity": 12,
-        "climb": 13,
+        "groundspeed": 13,
-        "roll": 14,
+        "heading": 14,
-        "pitch": 15,
+        "airspeed": 15,
-        "yaw": 16,
+        "throttle": 16,
-        "loss_rate": 17,
+        "hud_alt": 17,
-        "ping": 18
+        "climb": 18,
+        "roll": 19,
+        "pitch": 20,
+        "yaw": 21,
+        "loss_rate": 22,
+        "ping": 23
     }
     
     def __init__(self, info_types=None, info_type_map=None, parent=None):
@@ -38,6 +47,8 @@ class OverviewTable(QTableWidget):
         self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES
         self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP
         self.drones = {}  # 存儲無人機面板的引用
+        # 任務狀態不是遙測，不在 panel QLabel 內；獨立保存，避免 refresh_all 用遙測把它蓋掉
+        self.mission_status = {}  # drone_id -> 任務狀態文字
         
         # 初始化表格
         self.setColumnCount(1)
@@ -70,7 +81,11 @@ class OverviewTable(QTableWidget):
         if drone_id and field and value:
             if drone_id not in self.drones:
                 return
-                
+
+            # 任務狀態另存一份，讓 refresh_all（用遙測重繪）不會把它清掉
+            if field == 'mission':
+                self.mission_status[drone_id] = value
+
             col = 1 + list(self.drones.keys()).index(drone_id)
             row = self.info_type_map.get(field, -1)
 
@@ -99,8 +114,12 @@ class OverviewTable(QTableWidget):
         for col, did in enumerate(self.drones, start=1):
             panel = self.drones[did]
             for field, row in self.info_type_map.items():
-                lbl = panel.findChild(QLabel, f"{did}_{field}")
+                if field == 'mission':
-                val = lbl.text() if lbl else "--"
+                    # 任務狀態非遙測，取自獨立保存，避免被 "-" 蓋掉
+                    val = self.mission_status.get(did, "-")
+                else:
+                    lbl = panel.findChild(QLabel, f"{did}_{field}")
+                    val = lbl.text() if lbl else "-"
                 val_item = QTableWidgetItem(val)
                 val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
                 self.setItem(row, col, val_item)

src/GUI/validation/test_mission.py

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

src/GUI/validation/verify_waypoints.py

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

src/GUI/websocket_client.py

@@ -0,0 +1,155 @@
+import asyncio
+import websockets
+import json
+import sys
+
+WS_URL = "ws://127.0.0.1:8765"
+SYSTEM_ID = 2
+GOTO_POSITION = [24.1234567, 120.1234567]
+
+
+class WebSocketClient:
+    def __init__(self, ws_url=WS_URL, system_id=SYSTEM_ID):
+        self.ws_url = ws_url
+        if isinstance(system_id, bool) or not isinstance(system_id, int):
+            raise ValueError("system_id 必須為整數")
+        self.system_id = system_id
+        
+    def process_websocket_message(self, data):
+        """解析 server 的 Essential／Full UAV 狀態或命令 ACK。"""
+        if isinstance(data, str):
+            try:
+                data = json.loads(data)
+            except json.JSONDecodeError as e:
+                print(f"JSON 格式錯誤: {e}")
+                return
+
+        if not isinstance(data, dict):
+            print(f"未知訊息格式: {data}")
+            return
+
+        if all(key in data for key in ("result", "command", "message")):
+            print(
+                f"ACK | Command:{data.get('command', '-')} | "
+                f"Result:{data.get('result', '-')} | "
+                f"Message:{data.get('message', '-')}"
+            )
+            return
+
+        if "s" not in data:
+            print(f"未知 JSON 訊息: {data}")
+            return
+
+        position = data.get("p")
+        if isinstance(position, (list, tuple)) and len(position) >= 2:
+            position_text = f"{position[0]}, {position[1]}"
+        else:
+            position_text = "-"
+
+        ypr = data.get("ypr")
+        if isinstance(ypr, (list, tuple)) and len(ypr) >= 3:
+            yaw, pitch, roll = ypr[0], ypr[1], ypr[2]
+        else:
+            yaw, pitch, roll = data.get("y"), None, None
+
+        dop = data.get("d")
+        if isinstance(dop, (list, tuple)) and len(dop) >= 2:
+            dop_text = f"{dop[0]}/{dop[1]}"
+        else:
+            dop_text = "-"
+
+        armed = data.get("a")
+        if armed is None:
+            armed_text = "-"
+        else:
+            armed_text = "ARMED" if bool(armed) else "DISARMED"
+
+        def value_or_dash(value, suffix=""):
+            return f"{value}{suffix}" if value is not None else "-"
+
+        print(
+            f"UAV | ID:{data.get('s', '-')} | "
+            f"Mode:{value_or_dash(data.get('m'))} | "
+            f"Arm:{armed_text} | "
+            f"Battery:{value_or_dash(data.get('b'), '%')} | "
+            f"Position:{position_text} | "
+            f"Height:{value_or_dash(data.get('h'), 'm')} | "
+            f"Velocity:{value_or_dash(data.get('v'), 'm/s')} | "
+            f"Yaw:{value_or_dash(yaw, 'deg')} | "
+            f"Pitch:{value_or_dash(pitch, 'deg')} | "
+            f"Roll:{value_or_dash(roll, 'deg')} | "
+            f"GPS:{value_or_dash(data.get('g'))} | DOP(H/V):{dop_text}"
+        )
+
+    async def send_commands_loop(self, websocket):
+        """從 stdin 讀取按鍵 (輸入後按 Enter) 並發送對應的 JSON 指令"""
+        loop = asyncio.get_event_loop()
+        print("輸入 1 = START, 2 = STOP, 3 = GOTO (然後按 Enter)")
+        while True:
+            # readline 會包含換行
+            try:
+                line = await loop.run_in_executor(None, sys.stdin.readline)
+            except Exception:
+                return
+            if not line:
+                # EOF
+                return
+            cmd = line.strip()
+            if cmd == '1':
+                msg = {"s": self.system_id, "c": "START"}
+            elif cmd == '2':
+                msg = {"s": self.system_id, "c": "STOP"}
+            elif cmd == '3':
+                msg = {
+                    "s": self.system_id,
+                    "c": "GOTO",
+                    "p": list(GOTO_POSITION),
+                }
+            else:
+                print(f"未知指令: {cmd}")
+                continue
+
+            try:
+                await websocket.send(json.dumps(msg))
+                print(f"已發送: {msg}")
+            except Exception as e:
+                print(f"發送失敗: {e}")
+                return
+
+    async def ws_client_loop(self):
+        retry_count = 0
+        max_retries = 5
+        base_delay = 1.0
+        while retry_count < max_retries:
+            try:
+                async with websockets.connect(self.ws_url) as websocket:
+                    print(f"WebSocket connected: {self.ws_url}")
+                    retry_count = 0
+                    # 啟動後台任務來接收 stdin 指令並發送
+                    send_task = asyncio.create_task(self.send_commands_loop(websocket))
+                    try:
+                        async for message in websocket:
+                            # message 已經是 string
+                            self.process_websocket_message(message)
+                    finally:
+                        send_task.cancel()
+                        await asyncio.gather(send_task, return_exceptions=True)
+                            
+            except websockets.exceptions.ConnectionClosedError:
+                print("WebSocket connection closed")
+                break
+            except Exception as e:
+                retry_count += 1
+                delay = base_delay * (2 ** min(retry_count, 4))
+                print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
+                await asyncio.sleep(delay)
+        
+        print("WebSocket client stopped after maximum retries")
+
+    async def start(self):
+        """啟動 WebSocket 客戶端"""
+        await self.ws_client_loop()
+
+if __name__ == "__main__":
+    client = WebSocketClient()
+    asyncio.run(client.start())

src/GUI/websocket_server.py

@@ -0,0 +1,179 @@
+import asyncio
+import websockets
+import json
+from urllib.parse import urlparse
+
+# 可切換為 "essential" 或 "full"；預設廣播完整狀態。
+STATUS_FORMAT = "full"
+SYSTEM_ID = 2
+
+# WebSocket 伺服器位址；修改此參數即可更換監聽 IP/port。
+WS_URL = "ws://0.0.0.0:8765"
+
+
+def parse_ws_url(ws_url):
+    """將 ws://host:port 解析為 websockets.serve 所需參數。"""
+    parsed = urlparse(ws_url)
+    if parsed.scheme != "ws":
+        raise ValueError("WS_URL 必須使用 ws:// 格式")
+    if not parsed.hostname:
+        raise ValueError("WS_URL 缺少主機位址")
+    try:
+        port = parsed.port
+    except ValueError as e:
+        raise ValueError(f"WS_URL port 格式錯誤: {e}") from e
+    if port is None:
+        raise ValueError("WS_URL 缺少 port")
+    return parsed.hostname, port
+
+# 啟動伺服器
+async def main(ws_url=WS_URL):
+    host, port = parse_ws_url(ws_url)
+    async with websockets.serve(client_handler, host, port):
+        print(f"WebSocket server started at {ws_url}")
+        await asyncio.Future()  # run forever
+
+# 模擬資料
+def get_uav_status(status_format=None):
+    status_format = status_format or STATUS_FORMAT
+    essential_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "b": 78,                        # battery (%)
+        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
+        "y": 178.5,                     # yaw (deg)
+    }
+
+    full_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "a": 1,                         # 1=armed, 0=disarmed
+        "b": 78,                        # battery (%)
+        "h": 12.3,                      # height (m)
+        "v": 4.2,                       # velocity (m/s)
+        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
+        "ypr": [178.5, -0.1, 0.0],      # yaw, pitch, roll (deg)
+        "g": 3,                         # GPS fix type
+        "d": [0.8, 1.2],                # HDOP, VDOP
+    }
+
+    if status_format == "essential":
+        return essential_status
+    if status_format == "full":
+        return full_status
+    raise ValueError(
+        f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
+
+# 處理 client 傳來的訊息
+async def handle_client_messages(websocket):
+    try:
+        async for message in websocket:
+            try:
+                data = json.loads(message)
+                print(f"Received from client: {data}")
+                system_id = data.get("s")
+                cmd = data.get("c")
+                # XBee 命令格式:
+                # {"s":2,"c":"START"}
+                # {"s":2,"c":"STOP"}
+                # {"s":2,"c":"GOTO","p":[lat,lon]}
+                if system_id is None:
+                    ack = {
+                        "result": "ERROR",
+                        "command": cmd,
+                        "message": "缺少 system_id 欄位 s",
+                    }
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+                elif (isinstance(system_id, bool)
+                      or not isinstance(system_id, int)
+                      or system_id != SYSTEM_ID):
+                    ack = {
+                        "result": "ERROR",
+                        "command": cmd,
+                        "message": (
+                            f"指令目標 system_id={system_id} 不符，"
+                            f"本 UAV system_id={SYSTEM_ID}"
+                        ),
+                    }
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+                elif isinstance(cmd, str) and cmd in {"START", "STOP", "GOTO"}:
+                    if cmd == "START":
+                        print(f"收到 START 指令: system_id={system_id}")
+                        ack = {
+                            "result": "SUCCESS",
+                            "command": cmd,
+                            "message": "已接收 START 指令",
+                        }
+                    elif cmd == "STOP":
+                        print(f"收到 STOP 指令: system_id={system_id}")
+                        ack = {
+                            "result": "SUCCESS",
+                            "command": cmd,
+                            "message": "已接收 STOP 指令",
+                        }
+                    elif cmd == "GOTO":
+                        position = data.get("p")
+                        if isinstance(position, (list, tuple)) and len(position) >= 2:
+                            lat, lon = position[0], position[1]
+                            print(
+                                f"收到 GOTO 指令: system_id={system_id}, "
+                                f"lat={lat}, lon={lon}"
+                            )
+                            ack = {
+                                "result": "SUCCESS",
+                                "command": cmd,
+                                "message": "已接收 GOTO 指令",
+                            }
+                        else:
+                            print("GOTO 指令缺少位置 p=[lat,lon]")
+                            ack = {
+                                "result": "ERROR",
+                                "command": cmd,
+                                "message": "GOTO 指令缺少位置 p=[lat,lon]",
+                            }
+
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+                else:
+                    ack = {
+                        "result": "ERROR",
+                        "command": cmd,
+                        "message": "指令 c 必須為大寫 START、STOP 或 GOTO",
+                    }
+                    await websocket.send(json.dumps(ack, ensure_ascii=False))
+                    print(f"Sent ack: {ack}")
+            except json.JSONDecodeError:
+                print("❌ JSON 格式錯誤")
+    except websockets.ConnectionClosed:
+        print("❌ Client connection closed while receiving")
+
+# 持續廣播 UAV 資訊
+async def broadcast_uav_status(websocket):
+    try:
+        while True:
+            msg = get_uav_status()
+            await websocket.send(json.dumps(msg))
+            await asyncio.sleep(1)  # 每秒發送一次
+    except websockets.ConnectionClosed:
+        print("❌ Client connection closed while sending")
+
+# 每個 client 的處理流程
+async def client_handler(websocket, path=None):
+    print(f"🔗 Client connected: {websocket.remote_address}")
+    receive_task = asyncio.create_task(handle_client_messages(websocket))
+    send_task = asyncio.create_task(broadcast_uav_status(websocket))
+
+    done, pending = await asyncio.wait(
+        [receive_task, send_task],
+        return_when=asyncio.FIRST_COMPLETED,
+    )
+
+    for task in pending:
+        task.cancel()
+
+    print(f"🔌 Client disconnected: {websocket.remote_address}")
+
+if __name__ == "__main__":
+    asyncio.run(main(WS_URL))

src/GUI/xbee_gcs.py

@@ -0,0 +1,127 @@
+import serial
+import json
+import threading
+import time
+from pynput import keyboard
+
+SYSTEM_ID = 2
+
+try:
+    ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1) 
+    print("XBee 地面站啟動")
+    print("[1] START  [2] STOP  [3] GOTO  [Esc] 退出程式")
+except Exception as e:
+    print(f"無法開啟序列埠: {e}")
+    exit()
+
+GPS_TYPE_LABELS = {
+    0: "no GPS",
+    1: "no fix",
+    2: "2D",
+    3: "3D",
+    4: "DGPS",
+    5: "RTK float",
+    6: "RTK fixed",
+}
+
+def format_uav_data(uav_data):
+    system_id = uav_data.get("s", "?")
+    mode = uav_data.get("m", "?")
+    battery = uav_data.get("b", "?")
+    position = uav_data.get("p", "?")
+
+    parts = [
+        f"[UAV] ID:{system_id}",
+        f"Mode:{mode}",
+        f"Bat:{battery}%",
+        f"Pos:{position}",
+    ]
+
+    if "a" in uav_data:
+        armed_text = "ARMED" if int(uav_data.get("a", 0)) == 1 else "DISARMED"
+        parts.append(f"Arm:{armed_text}")
+
+    ypr = uav_data.get("ypr")
+    if isinstance(ypr, list) and len(ypr) >= 3:
+        parts.append(f"Height:{uav_data.get('h', '?')}m")
+        parts.append(f"Yaw:{ypr[0]}deg")
+        parts.append(f"Pitch:{ypr[1]}deg")
+        parts.append(f"Roll:{ypr[2]}deg")
+    elif "y" in uav_data:
+        parts.append(f"Height:{uav_data.get('h', '?')}m")
+        parts.append(f"Yaw:{uav_data.get('y')}deg")
+    elif "h" in uav_data:
+        parts.append(f"Heading:{uav_data.get('h')}")
+
+    if "v" in uav_data:
+        parts.append(f"Vel:{uav_data.get('v')}m/s")
+
+    if "g" in uav_data:
+        gps_type = uav_data.get("g")
+        parts.append(f"GPS:{gps_type}({GPS_TYPE_LABELS.get(gps_type, 'unknown')})")
+
+    dop = uav_data.get("d")
+    if isinstance(dop, list) and len(dop) >= 2:
+        parts.append(f"DOP(H/V):{dop[0]}/{dop[1]}")
+
+    return " | ".join(parts)
+
+def receive_thread():
+    while True:
+        try:
+            if ser.in_waiting > 0:
+                line = ser.readline().decode('utf-8', errors='ignore').strip()
+                if line:
+                    try:
+                        data = json.loads(line)
+                        # 判斷是確認訊息（ack）還是 UAV 狀態
+                        if "result" in data and "command" in data and "message" in data:
+                            # 這是確認訊息
+                            result = data.get("result")
+                            cmd = data.get("command")
+                            msg = data.get("message")
+                            print(f"\n[確認] 命令:{cmd} 狀態:{result} 訊息:{msg}", flush=True)
+                        else:
+                            # 這是 UAV 狀態訊息
+                            print(format_uav_data(data), flush=True)
+                    except json.JSONDecodeError:
+                        pass 
+        except Exception as e:
+            print(f"\n[接收錯誤] {e}")
+        time.sleep(0.01)
+
+t = threading.Thread(target=receive_thread, daemon=True)
+t.start()
+
+def send_command(command, position=None):
+    try:
+        if command not in {"START", "STOP", "GOTO"}:
+            raise ValueError("指令必須為大寫 START、STOP 或 GOTO")
+        cmd = {
+            "s": SYSTEM_ID,
+            "c": command
+        }
+        if position is not None:
+            cmd["p"] = position
+
+        payload = json.dumps(cmd, separators=(',', ':')) + "\n"
+        ser.write(payload.encode('utf-8'))
+        print(f"\n>> [指令已發送] {payload.strip()}")
+    except Exception as e:
+        print(f"\n發送失敗: {e}")
+
+def on_press(key):
+    try:
+        if key.char == "1":
+            send_command("START")
+        elif key.char == "2":
+            send_command("STOP")
+        elif key.char == "3":
+            send_command("GOTO", [24.1199660, 120.677230])
+    except AttributeError:
+        if key == keyboard.Key.esc:
+            print("\n程式結束")
+            return False
+
+with keyboard.Listener(on_press=on_press) as listener:
+    listener.join()

src/GUI/xbee_uav.py

@@ -0,0 +1,141 @@
+import serial
+import json
+import math
+import time
+
+PORT = '/dev/ttyUSB0'  # 替換為實際的序列埠名稱
+BAUD_RATE = 115200
+SYSTEM_ID = 2
+
+# 可切換為 "essential" 或 "full"；預設發送完整狀態。
+STATUS_FORMAT = "full"
+
+try:
+    ser = serial.Serial(PORT, BAUD_RATE, timeout=0.1)
+    print("XBee 無人機啟動...")
+except Exception as e:
+    print(f"無法開啟序列埠: {e}")
+    exit()
+
+def get_uav_status(status_format=None):
+    status_format = status_format or STATUS_FORMAT
+    essential_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "b": 78,                        # battery (%)
+        "p": [24.1199660, 120.677230],  # [lat, lon] (deg)
+        "y": 178.5,                     # yaw (deg)
+    }
+    full_status = {
+        "s": SYSTEM_ID,                 # system ID
+        "m": "GUIDED",                  # mode
+        "a": 1,                         # 1=armed, 0=disarmed
+        "b": 78,                        # battery (%)
+        "h": 12.3,                      # height (m)
+        "v": 4.2,                       # velocity (m/s)
+        "p": [24.1199660, 120.677230],  # position: [lat, lon] (deg)
+        "ypr": [178.5, -0.1, 0.0],      # yaw, pitch, roll (deg)
+        "g": 3,                         # GPS type {0: no GPS, 1: no fix, 2: 2D, 3: 3D, 4: DGPS, 5: RTK float, 6: RTK fixed}
+        "d": [0.8, 1.2],                # DOP: [hdop, vdop]
+    }
+    if status_format == "essential":
+        return essential_status
+    if status_format == "full":
+        return full_status
+    raise ValueError(
+        f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
+
+def handle_command(cmd_obj):
+    # 統一指令格式: {"s": <int>, "c": "START|STOP|GOTO"}
+    target_s = cmd_obj.get("s")
+    command = cmd_obj.get("c")
+    ack = None
+
+    if (isinstance(target_s, bool)
+            or not isinstance(target_s, int)
+            or target_s != SYSTEM_ID):
+        ack = {
+            "result": "ERROR",
+            "command": command,
+            "message": (
+                f"指令目標 system_id={target_s} 不符，"
+                f"本 UAV system_id={SYSTEM_ID}"
+            ),
+        }
+    elif not isinstance(command, str) or command not in {"START", "STOP", "GOTO"}:
+        print(f"\n[接收指令] 未知或格式錯誤的指令: {command}")
+        ack = {
+            "result": "ERROR",
+            "command": command,
+            "message": "指令 c 必須為大寫 START、STOP 或 GOTO",
+        }
+    elif command == "START":
+        print("\n[接收指令] 開始任務")
+        ack = {"result": "SUCCESS", "command": "START", "message": "成功"}
+    elif command == "STOP":
+        print("\n[接收指令] 停止任務")
+        ack = {"result": "SUCCESS", "command": "STOP", "message": "成功"}
+    elif command == "GOTO":
+        # 優先使用 XBee 格式 p=[lat, lon]，並兼容舊版 lat/lon 欄位。
+        position = cmd_obj.get("p")
+        if isinstance(position, (list, tuple)) and len(position) >= 2:
+            lat, lon = position[0], position[1]
+        else:
+            lat, lon = cmd_obj.get("lat"), cmd_obj.get("lon")
+
+        try:
+            if isinstance(lat, bool) or isinstance(lon, bool):
+                raise ValueError
+            lat = float(lat)
+            lon = float(lon)
+            if not math.isfinite(lat) or not math.isfinite(lon):
+                raise ValueError
+            if not -90.0 <= lat <= 90.0 or not -180.0 <= lon <= 180.0:
+                raise ValueError
+        except (TypeError, ValueError):
+            print("\n[接收指令] GOTO 目標無效，需要有效的 p=[lat,lon]")
+            ack = {
+                "result": "ERROR",
+                "command": "GOTO",
+                "message": "GOTO 指令缺少或包含無效的 p=[lat,lon]",
+            }
+        else:
+            target = [lat, lon]
+            print(f"\n[接收指令] 導航任務 目標位置: {target}")
+            ack = {
+                "result": "SUCCESS",
+                "command": "GOTO",
+                "p": target,
+                "message": "成功",
+            }
+    # 發送回覆（ack）到 GCS via serial
+    if ack is not None:
+        try:
+            payload = json.dumps(ack, ensure_ascii=False) + "\n"
+            ser.write(payload.encode('utf-8'))
+            print(f"已發送回覆: {payload.strip()}")
+        except Exception as e:
+            print(f"發送回覆失敗: {e}")
+
+try:
+    while True:
+        data = get_uav_status()
+        json_payload = json.dumps(data, separators=(',', ':')) + "\n"
+        ser.write(json_payload.encode('utf-8'))
+        print(f"發送狀態: {json_payload.strip()}")
+        for _ in range(10):
+            if ser.in_waiting > 0:
+                try:
+                    cmd_line = ser.readline().decode('utf-8').strip()
+                    if cmd_line:
+                        cmd_obj = json.loads(cmd_line)
+                        handle_command(cmd_obj)
+
+                except json.JSONDecodeError:
+                    pass 
+            time.sleep(0.1)
+
+except KeyboardInterrupt:
+    print("發送停止")
+finally:
+    ser.close()

src/external/mavros_msgs/CMakeLists.txt

@@ -0,0 +1,208 @@
+cmake_minimum_required(VERSION 3.5)
+project(mavros_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  # we dont use add_compile_options with pedantic in message packages
+  # because the Python C extensions dont comply with it
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(geographic_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# include_directories(include)
+
+# [[[cog:
+# import mavros_cog
+# ]]]
+# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e)
+
+set(msg_files
+  # [[[cog:
+  # mavros_cog.outl_glob_files('msg', '*.msg')
+  # ]]]
+  msg/ADSBVehicle.msg
+  msg/ActuatorControl.msg
+  msg/Altitude.msg
+  msg/AttitudeTarget.msg
+  msg/CamIMUStamp.msg
+  msg/CameraImageCaptured.msg
+  msg/CellularStatus.msg
+  msg/CommandCode.msg
+  msg/CompanionProcessStatus.msg
+  msg/DebugValue.msg
+  msg/ESCInfo.msg
+  msg/ESCInfoItem.msg
+  msg/ESCStatus.msg
+  msg/ESCStatusItem.msg
+  msg/ESCTelemetry.msg
+  msg/ESCTelemetryItem.msg
+  msg/EstimatorStatus.msg
+  msg/ExtendedState.msg
+  msg/FileEntry.msg
+  msg/GPSINPUT.msg
+  msg/GPSRAW.msg
+  msg/GPSRTK.msg
+  msg/GimbalDeviceAttitudeStatus.msg
+  msg/GimbalDeviceInformation.msg
+  msg/GimbalDeviceSetAttitude.msg
+  msg/GimbalManagerInformation.msg
+  msg/GimbalManagerSetAttitude.msg
+  msg/GimbalManagerSetPitchyaw.msg
+  msg/GimbalManagerStatus.msg
+  msg/GlobalPositionTarget.msg
+  msg/HilActuatorControls.msg
+  msg/HilControls.msg
+  msg/HilGPS.msg
+  msg/HilSensor.msg
+  msg/HilStateQuaternion.msg
+  msg/HomePosition.msg
+  msg/LandingTarget.msg
+  msg/LogData.msg
+  msg/LogEntry.msg
+  msg/MagnetometerReporter.msg
+  msg/ManualControl.msg
+  msg/Mavlink.msg
+  msg/MountControl.msg
+  msg/NavControllerOutput.msg
+  msg/OnboardComputerStatus.msg
+  msg/OpticalFlow.msg
+  msg/OpticalFlowRad.msg
+  msg/OverrideRCIn.msg
+  msg/Param.msg
+  msg/ParamEvent.msg
+  msg/ParamValue.msg
+  msg/PlayTuneV2.msg
+  msg/PositionTarget.msg
+  msg/RCIn.msg
+  msg/RCOut.msg
+  msg/RTCM.msg
+  msg/RTKBaseline.msg
+  msg/RadioStatus.msg
+  msg/State.msg
+  msg/StatusEvent.msg
+  msg/StatusText.msg
+  msg/SysStatus.msg
+  msg/TerrainReport.msg
+  msg/Thrust.msg
+  msg/TimesyncStatus.msg
+  msg/Trajectory.msg
+  msg/Tunnel.msg
+  msg/VehicleInfo.msg
+  msg/VfrHud.msg
+  msg/Vibration.msg
+  msg/Waypoint.msg
+  msg/WaypointList.msg
+  msg/WaypointReached.msg
+  msg/WheelOdomStamped.msg
+  # [[[end]]] (checksum: a8e24eb0a6da5cea6cc049fdc6b2612e)
+)
+
+set(srv_files
+  # [[[cog:
+  # mavros_cog.outl_glob_files('srv', '*.srv')
+  # ]]]
+  srv/CommandAck.srv
+  srv/CommandBool.srv
+  srv/CommandHome.srv
+  srv/CommandInt.srv
+  srv/CommandLong.srv
+  srv/CommandTOL.srv
+  srv/CommandTOLLocal.srv
+  srv/CommandTriggerControl.srv
+  srv/CommandTriggerInterval.srv
+  srv/CommandVtolTransition.srv
+  srv/EndpointAdd.srv
+  srv/EndpointDel.srv
+  srv/FileChecksum.srv
+  srv/FileClose.srv
+  srv/FileList.srv
+  srv/FileMakeDir.srv
+  srv/FileOpen.srv
+  srv/FileRead.srv
+  srv/FileRemove.srv
+  srv/FileRemoveDir.srv
+  srv/FileRename.srv
+  srv/FileTruncate.srv
+  srv/FileWrite.srv
+  srv/GimbalGetInformation.srv
+  srv/GimbalManagerCameraTrack.srv
+  srv/GimbalManagerConfigure.srv
+  srv/GimbalManagerPitchyaw.srv
+  srv/GimbalManagerSetRoi.srv
+  srv/LogRequestData.srv
+  srv/LogRequestEnd.srv
+  srv/LogRequestList.srv
+  srv/MessageInterval.srv
+  srv/MountConfigure.srv
+  srv/ParamGet.srv
+  srv/ParamPull.srv
+  srv/ParamPush.srv
+  srv/ParamSet.srv
+  srv/ParamSetV2.srv
+  srv/SetMavFrame.srv
+  srv/SetMode.srv
+  srv/StreamRate.srv
+  srv/VehicleInfoGet.srv
+  srv/WaypointClear.srv
+  srv/WaypointPull.srv
+  srv/WaypointPush.srv
+  srv/WaypointSetCurrent.srv
+  # [[[end]]] (checksum: cd7701b28a3176d96ef65cb1f2157917)
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${msg_files}
+  ${srv_files}
+  DEPENDENCIES
+  builtin_interfaces
+  rcl_interfaces
+  geographic_msgs
+  geometry_msgs
+  sensor_msgs
+  std_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+install(
+  FILES mavros_msgs_mapping_rule.yaml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(rcl_interfaces_VERSION VERSION_LESS "1.2.0")
+  install(
+    DIRECTORY include/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.hpp"
+  )
+else()
+  # NOTE(vooon): Humble
+  install(
+    DIRECTORY include/
+    DESTINATION include/mavros_msgs
+    FILES_MATCHING PATTERN "*.hpp"
+  )
+endif()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+
+  # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work.
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright)
+
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+# vim: ts=2 sw=2 et:

src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp

@@ -0,0 +1,145 @@
+//
+// Copyright 2015,2016,2021 Vladimir Ermakov.
+//
+// This file is part of the mavros package and subject to the license terms
+// in the top-level LICENSE file of the mavros repository.
+// https://github.com/mavlink/mavros/tree/master/LICENSE.md
+//
+/**
+ * @brief Mavlink convert utils
+ * @file
+ * @author Vladimir Ermakov <vooon341@gmail.com>
+ */
+
+#pragma once
+#ifndef MAVROS_MSGS__MAVLINK_CONVERT_HPP_
+#define MAVROS_MSGS__MAVLINK_CONVERT_HPP_
+
+#include <mavconn/mavlink_dialect.hpp>
+#include <mavros_msgs/msg/mavlink.hpp>
+
+#include <algorithm>
+
+namespace mavros_msgs
+{
+namespace mavlink
+{
+
+using ::mavlink::mavlink_message_t;
+using mavros_msgs::msg::Mavlink;
+
+// [[[cog:
+// FIELD_NAMES = [
+//   "magic",
+//   "len",
+//   "incompat_flags",
+//   "compat_flags",
+//   "seq",
+//   "sysid",
+//   "compid",
+//   "msgid",
+//   "checksum",
+// ]
+// ]]]
+// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e)
+
+// NOTE(vooon): Ignore impossible warning as
+// memcpy() should work with unaligned pointers without any trouble.
+//
+// warning: taking address of packed member of ‘mavlink::__mavlink_message’
+// may result in an unaligned pointer value
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+
+/**
+ * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t
+ *
+ * @note signature vector should be empty for unsigned OR
+ *       MAVLINK_SIGNATURE_BLOCK size for signed messages
+ *
+ * @param[in]  rmsg	mavros_msgs/Mavlink message
+ * @param[out] mmsg	mavlink_message_t struct
+ * @return true if success
+ */
+inline bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg)
+{
+  if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) {
+    return false;
+  }
+
+  if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) {
+    return false;
+  }
+
+  // [[[cog:
+  // for f in FIELD_NAMES:
+  //     cog.outl(f"mmsg.{f} = rmsg.{f};")
+  // ]]]
+  mmsg.magic = rmsg.magic;
+  mmsg.len = rmsg.len;
+  mmsg.incompat_flags = rmsg.incompat_flags;
+  mmsg.compat_flags = rmsg.compat_flags;
+  mmsg.seq = rmsg.seq;
+  mmsg.sysid = rmsg.sysid;
+  mmsg.compid = rmsg.compid;
+  mmsg.msgid = rmsg.msgid;
+  mmsg.checksum = rmsg.checksum;
+  // [[[end]]] (checksum: 0b66f0fc1cd46db0f18a2429c56a6b8c)
+  std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64);
+  std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature);
+
+  return true;
+}
+
+/**
+ * @brief Convert mavlink_message_t to mavros/Mavlink
+ *
+ * @param[in]  mmsg	mavlink_message_t struct
+ * @param[out] rmsg	mavros_msgs/Mavlink message
+ * @param[in]  framing_status  framing parse result (OK, BAD_CRC or BAD_SIGNATURE)
+ * @return true, this convertion can't fail
+ */
+inline bool convert(
+  const mavlink_message_t & mmsg, Mavlink & rmsg,
+  uint8_t framing_status = Mavlink::FRAMING_OK)
+{
+  const size_t payload64_len = (mmsg.len + 7) / 8;
+
+  rmsg.framing_status = framing_status;
+
+  // [[[cog:
+  // for f in FIELD_NAMES:
+  //     cog.outl(f"rmsg.{f} = mmsg.{f};")
+  // ]]]
+  rmsg.magic = mmsg.magic;
+  rmsg.len = mmsg.len;
+  rmsg.incompat_flags = mmsg.incompat_flags;
+  rmsg.compat_flags = mmsg.compat_flags;
+  rmsg.seq = mmsg.seq;
+  rmsg.sysid = mmsg.sysid;
+  rmsg.compid = mmsg.compid;
+  rmsg.msgid = mmsg.msgid;
+  rmsg.checksum = mmsg.checksum;
+  // [[[end]]] (checksum: 64ef6c1af60c622ed427e005d8ca4f2a)
+  rmsg.payload64.assign(
+    mmsg.payload64,
+    mmsg.payload64 + payload64_len);
+
+  // copy signature block only if message is signed
+  if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) {
+    rmsg.signature.assign(
+      mmsg.signature,
+      mmsg.signature + sizeof(mmsg.signature));
+  } else {
+    rmsg.signature.clear();
+  }
+
+  return true;
+}
+
+#pragma GCC diagnostic pop
+
+}  // namespace mavlink
+}  // namespace mavros_msgs
+
+#endif  // MAVROS_MSGS__MAVLINK_CONVERT_HPP_

src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml

@@ -0,0 +1,8 @@
+# This file defines mappings between ROS 1 and ROS 2 interfaces.
+# It is used with the ros1_bridge to allow for communcation between ROS 1 and ROS 2.
+
+-
+  ros1_package_name: 'mavros_msgs'
+  ros1_message_name: 'VFR_HUD'
+  ros2_package_name: 'mavros_msgs'
+  ros2_message_name: 'VfrHud'

src/external/mavros_msgs/msg/ADSBVehicle.msg

@@ -0,0 +1,66 @@
+# The location and information of an ADSB vehicle
+#
+# https://mavlink.io/en/messages/common.html#ADSB_VEHICLE
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum('ADSB_ALTITUDE_TYPE', 'ALT_')
+# mavros_cog.idl_decl_enum('ADSB_EMITTER_TYPE', 'EMITTER_')
+# mavros_cog.idl_decl_enum('ADSB_FLAGS', 'FLAG_', 16)
+# ]]]
+# ADSB_ALTITUDE_TYPE
+uint8 ALT_PRESSURE_QNH = 0               # Altitude reported from a Baro source using QNH reference
+uint8 ALT_GEOMETRIC = 1                  # Altitude reported from a GNSS source
+# ADSB_EMITTER_TYPE
+uint8 EMITTER_NO_INFO = 0
+uint8 EMITTER_LIGHT = 1
+uint8 EMITTER_SMALL = 2
+uint8 EMITTER_LARGE = 3
+uint8 EMITTER_HIGH_VORTEX_LARGE = 4
+uint8 EMITTER_HEAVY = 5
+uint8 EMITTER_HIGHLY_MANUV = 6
+uint8 EMITTER_ROTOCRAFT = 7
+uint8 EMITTER_UNASSIGNED = 8
+uint8 EMITTER_GLIDER = 9
+uint8 EMITTER_LIGHTER_AIR = 10
+uint8 EMITTER_PARACHUTE = 11
+uint8 EMITTER_ULTRA_LIGHT = 12
+uint8 EMITTER_UNASSIGNED2 = 13
+uint8 EMITTER_UAV = 14
+uint8 EMITTER_SPACE = 15
+uint8 EMITTER_UNASSGINED3 = 16
+uint8 EMITTER_EMERGENCY_SURFACE = 17
+uint8 EMITTER_SERVICE_SURFACE = 18
+uint8 EMITTER_POINT_OBSTACLE = 19
+# ADSB_FLAGS
+uint16 FLAG_VALID_COORDS = 1
+uint16 FLAG_VALID_ALTITUDE = 2
+uint16 FLAG_VALID_HEADING = 4
+uint16 FLAG_VALID_VELOCITY = 8
+uint16 FLAG_VALID_CALLSIGN = 16
+uint16 FLAG_VALID_SQUAWK = 32
+uint16 FLAG_SIMULATED = 64
+uint16 FLAG_VERTICAL_VELOCITY_VALID = 128
+uint16 FLAG_BARO_VALID = 256
+uint16 FLAG_SOURCE_UAT = 32768
+# [[[end]]] (checksum: a34f2a081739921b6e3e443ed0516d8d)
+
+std_msgs/Header header
+
+uint32 icao_address
+string callsign
+
+float64 latitude
+float64 longitude
+float32 altitude 	# AMSL
+
+float32 heading		# deg [0..360)
+float32 hor_velocity	# m/s
+float32 ver_velocity	# m/s
+
+uint8 altitude_type	# Type from ADSB_ALTITUDE_TYPE enum
+uint8 emitter_type	# Type from ADSB_EMITTER_TYPE enum
+
+builtin_interfaces/Duration tslc		# Duration from last communication, seconds [0..255]
+uint16 flags		# ADSB_FLAGS bit field
+uint16 squawk		# Squawk code

src/external/mavros_msgs/msg/ActuatorControl.msg

@@ -0,0 +1,16 @@
+# raw servo values for direct actuator controls
+#
+# about groups, mixing and channels:
+# https://pixhawk.org/dev/mixing
+
+# constant for mixer group
+uint8 PX4_MIX_FLIGHT_CONTROL = 0
+uint8 PX4_MIX_FLIGHT_CONTROL_VTOL_ALT = 1
+uint8 PX4_MIX_PAYLOAD = 2
+uint8 PX4_MIX_MANUAL_PASSTHROUGH = 3
+#uint8 PX4_MIX_FC_MC_VIRT = 4
+#uint8 PX4_MIX_FC_FW_VIRT = 5
+
+std_msgs/Header header
+uint8 group_mix
+float32[8] controls

src/external/mavros_msgs/msg/Altitude.msg

@@ -0,0 +1,12 @@
+# Altitude information
+#
+# https://mavlink.io/en/messages/common.html#ALTITUDE
+
+std_msgs/Header header
+
+float32 monotonic
+float32 amsl
+float32 local
+float32 relative
+float32 terrain
+float32 bottom_clearance

src/external/mavros_msgs/msg/AttitudeTarget.msg

@@ -0,0 +1,17 @@
+# Message for SET_ATTITUDE_TARGET
+#
+# Some complex system requires all feautures that mavlink
+# message provide. See issue #402, #418.
+
+std_msgs/Header header
+
+uint8 type_mask
+uint8 IGNORE_ROLL_RATE = 1	# body_rate.x
+uint8 IGNORE_PITCH_RATE = 2	# body_rate.y
+uint8 IGNORE_YAW_RATE = 4	# body_rate.z
+uint8 IGNORE_THRUST = 64
+uint8 IGNORE_ATTITUDE = 128	# orientation field
+
+geometry_msgs/Quaternion orientation
+geometry_msgs/Vector3 body_rate
+float32 thrust

src/external/mavros_msgs/msg/CamIMUStamp.msg

@@ -0,0 +1,4 @@
+# IMU-Camera synchronisation data
+
+builtin_interfaces/Time frame_stamp		# Timestamp when the camera was triggered
+int32 frame_seq_id		# Sequence number of the image frame

src/external/mavros_msgs/msg/CameraImageCaptured.msg

@@ -0,0 +1,11 @@
+# MAVLink message: CAMERA_IMAGE_CAPTURED
+# https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED
+
+std_msgs/Header header
+
+geometry_msgs/Quaternion orientation	# Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)
+geographic_msgs/GeoPoint geo
+float32 relative_alt	# mm	Altitude above ground
+int32 image_index # Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1)
+int8 capture_result # Boolean indicating success (1) or failure (0) while capturing this image.
+string file_url #URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface.

src/external/mavros_msgs/msg/CellularStatus.msg

@@ -0,0 +1,9 @@
+#Follows https://mavlink.io/en/messages/common.html#CELLULAR_STATUS specification
+
+uint8 status
+uint8 failure_reason
+uint8 type
+uint8 quality
+uint16 mcc
+uint16 mnc
+uint16 lac

src/external/mavros_msgs/msg/CommandCode.msg

@@ -0,0 +1,200 @@
+# MAV_CMD command codes.
+# Actual meaning and params you may find in MAVLink documentation
+# https://mavlink.io/en/messages/common.html#MAV_CMD
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum_mav_cmd()
+# ]]]
+# MAV_CMD_AIRFRAME
+uint16 AIRFRAME_CONFIGURATION = 2520
+
+# MAV_CMD_ARM
+uint16 ARM_AUTHORIZATION_REQUEST = 3001            # Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON.
+
+# MAV_CMD_CAMERA
+uint16 CAMERA_TRACK_POINT = 2004                   # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking.
+uint16 CAMERA_TRACK_RECTANGLE = 2005               # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking.
+uint16 CAMERA_STOP_TRACKING = 2010                 # Stops ongoing tracking.
+
+# MAV_CMD_CAN
+uint16 CAN_FORWARD = 32000                         # Request forwarding of CAN packets from the given CAN bus to this interface. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages
+
+# MAV_CMD_COMPONENT
+uint16 COMPONENT_ARM_DISARM = 400                  # Arms / Disarms a component
+
+# MAV_CMD_CONDITION
+uint16 CONDITION_DELAY = 112                       # Delay mission state machine.
+uint16 CONDITION_CHANGE_ALT = 113                  # Ascend/descend to target altitude at specified rate. Delay mission state machine until desired altitude reached.
+uint16 CONDITION_DISTANCE = 114                    # Delay mission state machine until within desired distance of next NAV point.
+uint16 CONDITION_YAW = 115                         # Reach a certain target angle.
+uint16 CONDITION_LAST = 159                        # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration
+
+# MAV_CMD_CONTROL
+uint16 CONTROL_HIGH_LATENCY = 2600                 # Request to start/stop transmitting over the high latency telemetry
+
+# MAV_CMD_DO
+uint16 DO_FOLLOW = 32                              # Begin following a target
+uint16 DO_FOLLOW_REPOSITION = 33                   # Reposition the MAV after a follow target command has been sent
+uint16 DO_SET_MODE = 176                           # Set system mode.
+uint16 DO_JUMP = 177                               # Jump to the desired command in the mission list.  Repeat this action only the specified number of times
+uint16 DO_CHANGE_SPEED = 178                       # Change speed and/or throttle set points
+uint16 DO_SET_HOME = 179                           # Changes the home location either to the current location or a specified location.
+uint16 DO_SET_PARAMETER = 180                      # Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter.
+uint16 DO_SET_RELAY = 181                          # Set a relay to a condition.
+uint16 DO_REPEAT_RELAY = 182                       # Cycle a relay on and off for a desired number of cycles with a desired period.
+uint16 DO_SET_SERVO = 183                          # Set a servo to a desired PWM value.
+uint16 DO_REPEAT_SERVO = 184                       # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.
+uint16 DO_FLIGHTTERMINATION = 185                  # Terminate flight immediately
+uint16 DO_CHANGE_ALTITUDE = 186                    # Change altitude set point.
+uint16 DO_LAND_START = 189                         # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude/Altitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence.
+uint16 DO_RALLY_LAND = 190                         # Mission command to perform a landing from a rally point.
+uint16 DO_GO_AROUND = 191                          # Mission command to safely abort an autonomous landing.
+uint16 DO_REPOSITION = 192                         # Reposition the vehicle to a specific WGS84 global position.
+uint16 DO_PAUSE_CONTINUE = 193                     # If in a GPS controlled position mode, hold the current position or continue.
+uint16 DO_SET_REVERSE = 194                        # Set moving direction to forward or reverse.
+uint16 DO_SET_ROI_LOCATION = 195                   # Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_SET_ROI_WPNEXT_OFFSET = 196              # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_SET_ROI_NONE = 197                       # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_SET_ROI_SYSID = 198                      # Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means.
+uint16 DO_CONTROL_VIDEO = 200                      # Control onboard camera system.
+uint16 DO_SET_ROI = 201                            # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_DIGICAM_CONFIGURE = 202                  # Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ).
+uint16 DO_DIGICAM_CONTROL = 203                    # Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ).
+uint16 DO_MOUNT_CONFIGURE = 204                    # Mission command to configure a camera or antenna mount
+uint16 DO_MOUNT_CONTROL = 205                      # Mission command to control a camera or antenna mount
+uint16 DO_SET_CAM_TRIGG_DIST = 206                 # Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera.
+uint16 DO_FENCE_ENABLE = 207                       # Mission command to enable the geofence
+uint16 DO_PARACHUTE = 208                          # Mission item/command to release a parachute or enable/disable auto release.
+uint16 DO_MOTOR_TEST = 209                         # Mission command to perform motor test.
+uint16 DO_INVERTED_FLIGHT = 210                    # Change to/from inverted flight.
+uint16 DO_GRIPPER = 211                            # Mission command to operate a gripper.
+uint16 DO_AUTOTUNE_ENABLE = 212                    # Enable/disable autotune.
+uint16 DO_SET_CAM_TRIGG_INTERVAL = 214             # Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera.
+uint16 DO_MOUNT_CONTROL_QUAT = 220                 # Mission command to control a camera or antenna mount, using a quaternion as reference.
+uint16 DO_GUIDED_MASTER = 221                      # set id of master controller
+uint16 DO_GUIDED_LIMITS = 222                      # Set limits for external control
+uint16 DO_ENGINE_CONTROL = 223                     # Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines
+uint16 DO_SET_MISSION_CURRENT = 224                # Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between).
+uint16 DO_LAST = 240                               # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration
+uint16 DO_JUMP_TAG = 601                           # Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number.
+uint16 DO_GIMBAL_MANAGER_PITCHYAW = 1000           # Set gimbal manager pitch/yaw setpoints (low rate command). It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: only the gimbal manager will react to this command - it will be ignored by a gimbal device. Use GIMBAL_MANAGER_SET_PITCHYAW if you need to stream pitch/yaw setpoints at higher rate.
+uint16 DO_GIMBAL_MANAGER_CONFIGURE = 1001          # Gimbal configuration to set which sysid/compid is in primary and secondary control.
+uint16 DO_TRIGGER_CONTROL = 2003                   # Enable or disable on-board camera triggering system.
+uint16 DO_VTOL_TRANSITION = 3000                   # Request VTOL transition
+uint16 DO_ADSB_OUT_IDENT = 10001                   # Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec.
+uint16 DO_WINCH = 42600                            # Command to operate winch.
+
+# MAV_CMD_FIXED
+uint16 FIXED_MAG_CAL_YAW = 42006                   # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
+
+# MAV_CMD_GET
+uint16 GET_HOME_POSITION = 410                     # Request the home position from the vehicle.
+uint16 GET_MESSAGE_INTERVAL = 510                  # Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message.
+
+# MAV_CMD_IMAGE
+uint16 IMAGE_START_CAPTURE = 2000                  # Start image capture sequence. CAMERA_IMAGE_CAPTURED must be emitted after each capture. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param 1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels.
+uint16 IMAGE_STOP_CAPTURE = 2001                   # Stop image capture sequence. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels.
+
+# MAV_CMD_JUMP
+uint16 JUMP_TAG = 600                              # Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG.
+
+# MAV_CMD_LOGGING
+uint16 LOGGING_START = 2510                        # Request to start streaming logging data over MAVLink (see also LOGGING_DATA message)
+uint16 LOGGING_STOP = 2511                         # Request to stop streaming log data over MAVLink
+
+# MAV_CMD_MISSION
+uint16 MISSION_START = 300                         # start running a mission
+
+# MAV_CMD_NAV
+uint16 NAV_WAYPOINT = 16                           # Navigate to waypoint.
+uint16 NAV_LOITER_UNLIM = 17                       # Loiter around this waypoint an unlimited amount of time
+uint16 NAV_LOITER_TURNS = 18                       # Loiter around this waypoint for X turns
+uint16 NAV_LOITER_TIME = 19                        # Loiter around this waypoint for X seconds
+uint16 NAV_RETURN_TO_LAUNCH = 20                   # Return to launch location
+uint16 NAV_LAND = 21                               # Land at location.
+uint16 NAV_TAKEOFF = 22                            # Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode.
+uint16 NAV_LAND_LOCAL = 23                         # Land at local position (local frame only)
+uint16 NAV_TAKEOFF_LOCAL = 24                      # Takeoff from local position (local frame only)
+uint16 NAV_FOLLOW = 25                             # Vehicle following, i.e. this waypoint represents the position of a moving vehicle
+uint16 NAV_CONTINUE_AND_CHANGE_ALT = 30            # Continue on the current course and climb/descend to specified altitude.  When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached.
+uint16 NAV_LOITER_TO_ALT = 31                      # Begin loiter at the specified Latitude and Longitude.  If Lat=Lon=0, then loiter at the current position.  Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint.
+uint16 NAV_ROI = 80                                # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 NAV_PATHPLANNING = 81                       # Control autonomous path planning on the MAV.
+uint16 NAV_SPLINE_WAYPOINT = 82                    # Navigate to waypoint using a spline path.
+uint16 NAV_VTOL_TAKEOFF = 84                       # Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.).
+uint16 NAV_VTOL_LAND = 85                          # Land using VTOL mode
+uint16 NAV_GUIDED_ENABLE = 92                      # hand control over to an external controller
+uint16 NAV_DELAY = 93                              # Delay the next navigation command a number of seconds or until a specified time
+uint16 NAV_PAYLOAD_PLACE = 94                      # Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload.
+uint16 NAV_LAST = 95                               # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration
+uint16 NAV_SET_YAW_SPEED = 213                     # Sets a desired vehicle turn angle and speed change.
+uint16 NAV_FENCE_RETURN_POINT = 5000               # Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead.
+uint16 NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001   # Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required.
+uint16 NAV_FENCE_POLYGON_VERTEX_EXCLUSION = 5002   # Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required.
+uint16 NAV_FENCE_CIRCLE_INCLUSION = 5003           # Circular fence area. The vehicle must stay inside this area.
+uint16 NAV_FENCE_CIRCLE_EXCLUSION = 5004           # Circular fence area. The vehicle must stay outside this area.
+uint16 NAV_RALLY_POINT = 5100                      # Rally point. You can have multiple rally points defined.
+
+# MAV_CMD_OBLIQUE
+uint16 OBLIQUE_SURVEY = 260                        # Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera.
+
+# MAV_CMD_OVERRIDE
+uint16 OVERRIDE_GOTO = 252                         # Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position.
+
+# MAV_CMD_PANORAMA
+uint16 PANORAMA_CREATE = 2800                      # Create a panorama at the current position
+
+# MAV_CMD_PAYLOAD
+uint16 PAYLOAD_PREPARE_DEPLOY = 30001              # Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity.
+uint16 PAYLOAD_CONTROL_DEPLOY = 30002              # Control the payload deployment.
+
+# MAV_CMD_PREFLIGHT
+uint16 PREFLIGHT_CALIBRATION = 241                 # Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero.
+uint16 PREFLIGHT_SET_SENSOR_OFFSETS = 242          # Set sensor offsets. This command will be only accepted if in pre-flight mode.
+uint16 PREFLIGHT_UAVCAN = 243                      # Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named).
+uint16 PREFLIGHT_STORAGE = 245                     # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode.
+uint16 PREFLIGHT_REBOOT_SHUTDOWN = 246             # Request the reboot or shutdown of system components.
+
+# MAV_CMD_REQUEST
+uint16 REQUEST_MESSAGE = 512                       # Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL).
+uint16 REQUEST_PROTOCOL_VERSION = 519              # Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message
+uint16 REQUEST_AUTOPILOT_CAPABILITIES = 520        # Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message
+uint16 REQUEST_CAMERA_INFORMATION = 521            # Request camera information (CAMERA_INFORMATION).
+uint16 REQUEST_CAMERA_SETTINGS = 522               # Request camera settings (CAMERA_SETTINGS).
+uint16 REQUEST_STORAGE_INFORMATION = 525           # Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage.
+uint16 REQUEST_CAMERA_CAPTURE_STATUS = 527         # Request camera capture status (CAMERA_CAPTURE_STATUS)
+uint16 REQUEST_FLIGHT_INFORMATION = 528            # Request flight information (FLIGHT_INFORMATION)
+uint16 REQUEST_VIDEO_STREAM_INFORMATION = 2504     # Request video stream information (VIDEO_STREAM_INFORMATION)
+uint16 REQUEST_VIDEO_STREAM_STATUS = 2505          # Request video stream status (VIDEO_STREAM_STATUS)
+
+# MAV_CMD_RESET
+uint16 RESET_CAMERA_SETTINGS = 529                 # Reset all camera settings to Factory Default
+
+# MAV_CMD_RUN
+uint16 RUN_PREARM_CHECKS = 401                     # Instructs system to run pre-arm checks.  This command should return MAV_RESULT_TEMPORARILY_REJECTED in the case the system is armed, otherwse MAV_RESULT_ACCEPTED.  Note that the return value from executing this command does not indicate whether the vehicle is armable or not, just whether the system has successfully run/is currently running the checks.  The result of the checks is reflected in the SYS_STATUS message.
+
+# MAV_CMD_SET
+uint16 SET_MESSAGE_INTERVAL = 511                  # Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM.
+uint16 SET_CAMERA_MODE = 530                       # Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming.
+uint16 SET_CAMERA_ZOOM = 531                       # Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success).
+uint16 SET_CAMERA_FOCUS = 532                      # Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success).
+uint16 SET_GUIDED_SUBMODE_STANDARD = 4000          # This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes.
+uint16 SET_GUIDED_SUBMODE_CIRCLE = 4001            # This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position.
+
+# MAV_CMD_START
+uint16 START_RX_PAIR = 500                         # Starts receiver pairing.
+
+# MAV_CMD_STORAGE
+uint16 STORAGE_FORMAT = 526                        # Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage.
+
+# MAV_CMD_UAVCAN
+uint16 UAVCAN_GET_NODE_INFO = 5200                 # Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages.
+
+# MAV_CMD_VIDEO
+uint16 VIDEO_START_CAPTURE = 2500                  # Starts video capture (recording).
+uint16 VIDEO_STOP_CAPTURE = 2501                   # Stop the current video capture (recording).
+uint16 VIDEO_START_STREAMING = 2502                # Start video streaming
+uint16 VIDEO_STOP_STREAMING = 2503                 # Stop the given video stream
+
+# [[[end]]] (checksum: 73ee94ac661c9fcb61528a6668f71d94)

src/external/mavros_msgs/msg/CompanionProcessStatus.msg

@@ -0,0 +1,19 @@
+# Mavros message: COMPANIONPROCESSSTATUS
+
+std_msgs/Header header
+
+uint8 state			# See enum COMPANION_PROCESS_STATE
+uint8 component		# See enum MAV_COMPONENT
+
+uint8 MAV_STATE_UNINIT = 0
+uint8 MAV_STATE_BOOT = 1
+uint8 MAV_STATE_CALIBRATING = 2
+uint8 MAV_STATE_STANDBY = 3
+uint8 MAV_STATE_ACTIVE = 4
+uint8 MAV_STATE_CRITICAL = 5
+uint8 MAV_STATE_EMERGENCY = 6
+uint8 MAV_STATE_POWEROFF = 7
+uint8 MAV_STATE_FLIGHT_TERMINATION = 8
+
+uint8 MAV_COMP_ID_OBSTACLE_AVOIDANCE = 196
+uint8 MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197

src/external/mavros_msgs/msg/DebugValue.msg

@@ -0,0 +1,26 @@
+# Msg for Debug MAVLink API
+#
+# Supported types:
+# DEBUG			https://mavlink.io/en/messages/common.html#DEBUG
+# DEBUG_VECTOR			https://mavlink.io/en/messages/common.html#DEBUG_VECT
+# DEBUG_FLOAT_ARRAY      https://mavlink.io/en/messages/common.html#DEBUG_FLOAT_ARRAY
+# NAMED_VALUE_FLOAT		https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT
+# NAMED_VALUE_INT		https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT
+
+std_msgs/Header header
+
+int32 index			# index value of DEBUG value (-1 if not indexed)
+int32 array_id      # Unique ID used to discriminate between DEBUG_FLOAT_ARRAYS (-1 if not used)
+
+string name			# value name/key
+
+float32 value_float		# float value for NAMED_VALUE_FLOAT and DEBUG
+int32 value_int		# int value for NAMED_VALUE_INT
+float32[] data			# DEBUG vector or array
+
+uint8 type
+uint8 TYPE_DEBUG		= 0
+uint8 TYPE_DEBUG_VECT		= 1
+uint8 TYPE_DEBUG_FLOAT_ARRAY	= 2
+uint8 TYPE_NAMED_VALUE_FLOAT	= 3
+uint8 TYPE_NAMED_VALUE_INT	= 4

src/external/mavros_msgs/msg/ESCInfo.msg

@@ -0,0 +1,14 @@
+# ESCInfo.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_INFO
+
+std_msgs/Header header
+
+uint16 counter
+uint8 count
+uint8 connection_type
+uint8 info
+mavros_msgs/ESCInfoItem[] esc_info
+

src/external/mavros_msgs/msg/ESCInfoItem.msg

@@ -0,0 +1,12 @@
+# ESCInfoItem.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_INFO
+
+std_msgs/Header header
+
+uint16 failure_flags
+uint32 error_count
+int32 temperature
+

src/external/mavros_msgs/msg/ESCStatus.msg

@@ -0,0 +1,9 @@
+# ESCStatus.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_STATUS
+
+std_msgs/Header header
+
+mavros_msgs/ESCStatusItem[] esc_status

src/external/mavros_msgs/msg/ESCStatusItem.msg

@@ -0,0 +1,11 @@
+# ESCStatusItem.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_STATUS
+
+std_msgs/Header header
+
+int32 rpm
+float32 voltage
+float32 current

src/external/mavros_msgs/msg/ESCTelemetry.msg

@@ -0,0 +1,10 @@
+# APM ESC Telemetry as returned by BLHeli
+#
+# See:
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12
+
+std_msgs/Header header
+
+mavros_msgs/ESCTelemetryItem[] esc_telemetry

src/external/mavros_msgs/msg/ESCTelemetryItem.msg

@@ -0,0 +1,15 @@
+# APM ESC Telemetry as returned by BLHeli
+#
+# See:
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12
+
+std_msgs/Header header
+
+float32 temperature     # deg C
+float32 voltage         # V
+float32 current         # A
+float32 totalcurrent    # Ah
+int32 rpm               # 1/min
+uint16 count            # count of telemetry packets

src/external/mavros_msgs/msg/EstimatorStatus.msg

@@ -0,0 +1,23 @@
+# Current autopilot estimator state
+#
+# https://mavlink.io/en/messages/common.html#ESTIMATOR_STATUS_FLAGS
+
+std_msgs/Header header
+bool attitude_status_flag
+
+bool velocity_horiz_status_flag
+bool velocity_vert_status_flag
+
+bool pos_horiz_rel_status_flag
+bool pos_horiz_abs_status_flag
+
+bool pos_vert_abs_status_flag
+bool pos_vert_agl_status_flag
+
+bool const_pos_mode_status_flag
+
+bool pred_pos_horiz_rel_status_flag
+bool pred_pos_horiz_abs_status_flag
+
+bool gps_glitch_status_flag
+bool accel_error_status_flag

src/external/mavros_msgs/msg/ExtendedState.msg

@@ -0,0 +1,19 @@
+# Extended autopilot state
+#
+# https://mavlink.io/en/messages/common.html#EXTENDED_SYS_STATE
+
+uint8 VTOL_STATE_UNDEFINED = 0
+uint8 VTOL_STATE_TRANSITION_TO_FW = 1
+uint8 VTOL_STATE_TRANSITION_TO_MC = 2
+uint8 VTOL_STATE_MC = 3
+uint8 VTOL_STATE_FW = 4
+
+uint8 LANDED_STATE_UNDEFINED = 0
+uint8 LANDED_STATE_ON_GROUND = 1
+uint8 LANDED_STATE_IN_AIR = 2
+uint8 LANDED_STATE_TAKEOFF = 3
+uint8 LANDED_STATE_LANDING = 4
+
+std_msgs/Header header
+uint8 vtol_state
+uint8 landed_state

src/external/mavros_msgs/msg/FileEntry.msg

@@ -0,0 +1,12 @@
+# File/Dir information
+
+uint8 TYPE_FILE = 0
+uint8 TYPE_DIRECTORY = 1
+
+string name
+uint8 type
+uint64 size
+
+# Not supported by MAVLink FTP
+#builtin_interfaces/Time atime
+#int32 access_flags

src/external/mavros_msgs/msg/GPSINPUT.msg

@@ -0,0 +1,37 @@
+# FCU GPS INPUT message for the gps_input plugin
+# <a href="https://mavlink.io/en/messages/common.html#GPS_INPUT">mavlink GPS_INPUT message</a>.
+
+std_msgs/Header header
+## GPS_FIX_TYPE enum
+uint8 GPS_FIX_TYPE_NO_GPS     = 0    # No GPS connected
+uint8 GPS_FIX_TYPE_NO_FIX     = 1    # No position information, GPS is connected
+uint8 GPS_FIX_TYPE_2D_FIX     = 2    # 2D position
+uint8 GPS_FIX_TYPE_3D_FIX     = 3    # 3D position
+uint8 GPS_FIX_TYPE_DGPS       = 4    # DGPS/SBAS aided 3D position
+uint8 GPS_FIX_TYPE_RTK_FLOATR = 5    # TK float, 3D position
+uint8 GPS_FIX_TYPE_RTK_FIXEDR = 6    # TK Fixed, 3D position
+uint8 GPS_FIX_TYPE_STATIC     = 7    # Static fixed, typically used for base stations
+uint8 GPS_FIX_TYPE_PPP        = 8    # PPP, 3D position
+uint8 fix_type      # [GPS_FIX_TYPE] GPS fix type
+
+uint8 gps_id        # ID of the GPS for multiple GPS inputs
+uint16 ignore_flags # Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided.
+
+uint32 time_week_ms # [ms] GPS time (from start of GPS week)
+uint16 time_week    # GPS week number
+int32 lat           # [degE7] Latitude (WGS84, EGM96 ellipsoid)
+int32 lon           # [degE7] Longitude (WGS84, EGM96 ellipsoid)
+float32 alt         # [m] Altitude (MSL). Positive for up.
+
+float32 hdop        # [m] GPS HDOP horizontal dilution of position.
+float32 vdop        # [m] GPS VDOP vertical dilution of position
+float32 vn          # [m/s] GPS velocity in NORTH direction in earth-fixed NED frame
+float32 ve          # [m/s] GPS velocity in EAST direction in earth-fixed NED frame
+float32 vd          # [m/s] GPS velocity in DOWN direction in earth-fixed NED frame
+
+float32 speed_accuracy # [m/s] GPS speed accuracy
+float32 horiz_accuracy # [m] GPS horizontal accuracy
+float32 vert_accuracy  # [m] GPS vertical accuracy
+
+uint8 satellites_visible # Number of satellites visible. If unknown, set to 255
+uint16 yaw          # [cdeg] Yaw in earth frame from north.

src/external/mavros_msgs/msg/GPSRAW.msg

@@ -0,0 +1,37 @@
+# FCU GPS RAW message for the gps_status plugin
+# A merge of <a href="https://mavlink.io/en/messages/common.html#GPS_RAW_INT">mavlink GPS_RAW_INT</a> and
+# <a href="https://mavlink.io/en/messages/common.html#GPS2_RAW">mavlink GPS2_RAW</a> messages.
+
+std_msgs/Header header
+## GPS_FIX_TYPE enum
+uint8 GPS_FIX_TYPE_NO_GPS     = 0    # No GPS connected
+uint8 GPS_FIX_TYPE_NO_FIX     = 1    # No position information, GPS is connected
+uint8 GPS_FIX_TYPE_2D_FIX     = 2    # 2D position
+uint8 GPS_FIX_TYPE_3D_FIX     = 3    # 3D position
+uint8 GPS_FIX_TYPE_DGPS       = 4    # DGPS/SBAS aided 3D position
+uint8 GPS_FIX_TYPE_RTK_FLOAT  = 5    # RTK float, 3D position
+uint8 GPS_FIX_TYPE_RTK_FIXED  = 6    # RTK Fixed, 3D position
+uint8 GPS_FIX_TYPE_STATIC     = 7    # Static fixed, typically used for base stations
+uint8 GPS_FIX_TYPE_PPP        = 8    # PPP, 3D position
+uint8 fix_type      # [GPS_FIX_TYPE] GPS fix type
+
+int32 lat           # [degE7] Latitude (WGS84, EGM96 ellipsoid)
+int32 lon           # [degE7] Longitude (WGS84, EGM96 ellipsoid)
+int32 alt           # [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.
+uint16 eph          # GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX
+uint16 epv          # GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX
+uint16 vel          # [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX
+uint16 cog          # [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
+uint8 satellites_visible # Number of satellites visible. If unknown, set to 255
+
+# -*- only available with MAVLink v2.0 and GPS_RAW_INT messages -*-
+int32 alt_ellipsoid # [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up.
+uint32 h_acc        # [mm] Position uncertainty. Positive for up.
+uint32 v_acc        # [mm] Altitude uncertainty. Positive for up.
+uint32 vel_acc      # [mm] Speed uncertainty. Positive for up.
+int32  hdg_acc      # [degE5] Heading / track uncertainty
+uint16 yaw          # [cdeg] Yaw in earth frame from north.
+
+# -*- only available with MAVLink v2.0 and GPS2_RAW messages -*-
+uint8 dgps_numch    # Number of DGPS satellites
+uint32 dgps_age     # [ms] Age of DGPS info

src/external/mavros_msgs/msg/GPSRTK.msg

@@ -0,0 +1,18 @@
+# FCU GPS RTK message for the gps_status plugin
+# A copy of <a href="https://mavlink.io/en/messages/common.html#GPS_RTK">mavlink GPS_RTK message</a>
+
+std_msgs/Header header
+
+uint8 rtk_receiver_id      # Identification of connected RTK receiver.
+int16 wn                   # GPS Week Number of last baseline.
+uint32 tow                 # [ms] GPS Time of Week of last baseline.
+uint8 rtk_health           # GPS-specific health report for RTK data.
+uint8 rtk_rate             # [Hz] Rate of baseline messages being received by GPS.
+uint8 nsats                # Current number of sats used for RTK calculation.
+int32 baseline_a           # [mm] Current baseline in ECEF x or NED north component, depends on header.frame_id.
+int32 baseline_b           # [mm] Current baseline in ECEF y or NED east component, depends on header.frame_id.
+int32 baseline_c           # [mm] Current baseline in ECEF z or NED down component, depends on header.frame_id.
+uint32 accuracy            # Current estimate of baseline accuracy.
+int32 iar_num_hypotheses   # Current number of integer ambiguity hypotheses.
+
+

src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg

@@ -0,0 +1,32 @@
+# MAVLink message: GIMBAL_DEVICE_ATTITUDE_STATUS
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS
+
+std_msgs/Header header
+
+uint8 target_system             # System ID
+uint8 target_component          # Component ID
+
+uint16 flags                    # Current gimbal flags set (bitwise) - See GIMBAL_DEVICE_FLAGS
+#GIMBAL_DEVICE_FLAGS
+uint16 FLAGS_RETRACT = 1      # Set to retracted safe position (no stabilization), takes presedence over all other flags.
+uint16 FLAGS_NEUTRAL = 2      # Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (pitch=yaw=0) but may be any orientation.
+uint16 FLAGS_ROLL_LOCK = 4    # Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal.
+uint16 FLAGS_PITCH_LOCK = 8   # Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default.
+uint16 FLAGS_YAW_LOCK = 16    # Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle).
+
+geometry_msgs/Quaternion q      # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
+float32 angular_velocity_x      # X component of angular velocity (NaN if unknown)
+float32 angular_velocity_y      # Y component of angular velocity (NaN if unknown)
+float32 angular_velocity_z      # Z component of angular velocity (NaN if unknown)
+
+uint32 failure_flags            # Failure flags (0 for no failure) (bitwise) - See GIMBAL_DEVICE_ERROR_FLAGS
+#GIMBAL_DEVICE_ERROR_FLAGS
+uint32 ERROR_FLAGS_AT_ROLL_LIMIT = 1          # Gimbal device is limited by hardware roll limit.
+uint32 ERROR_FLAGS_AT_PITCH_LIMIT = 2         # Gimbal device is limited by hardware pitch limit.
+uint32 ERROR_FLAGS_AT_YAW_LIMIT = 4           # Gimbal device is limited by hardware yaw limit.
+uint32 ERROR_FLAGS_ENCODER_ERROR = 8          # There is an error with the gimbal encoders.
+uint32 ERROR_FLAGS_POWER_ERROR = 16           # There is an error with the gimbal power source.
+uint32 ERROR_FLAGS_MOTOR_ERROR = 32           # There is an error with the gimbal motor's.
+uint32 ERROR_FLAGS_SOFTWARE_ERROR = 64        # There is an error with the gimbal's software.
+uint32 ERROR_FLAGS_COMMS_ERROR = 128          # There is an error with the gimbal's communication.
+uint32 ERROR_FLAGS_CALIBRATION_RUNNING = 256  # Gimbal is currently calibrating.

src/external/mavros_msgs/msg/GimbalDeviceInformation.msg

@@ -0,0 +1,34 @@
+# MAVLink message: GIMBAL_DEVICE_INFORMATION
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION
+
+std_msgs/Header header
+
+string vendor_name # Name of the gimbal vendor.
+string model_name # Name of the gimbal model.
+string custom_name # Custom name of the gimbal given to it by the user.
+uint32 firmware_version # Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).
+uint32 hardware_version # Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).
+uint64 uid # UID of gimbal hardware (0 if unknown).
+
+uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_DEVICE_CAP_FLAGS
+#GIMBAL_DEVICE_CAP_FLAGS
+uint32 CAP_FLAGS_HAS_RETRACT = 1 # Gimbal device supports a retracted position
+uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Gimbal device supports a horizontal, forward looking position, stabilized
+uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Gimbal device supports rotating around roll axis.
+uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Gimbal device supports to follow a roll angle relative to the vehicle
+uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized)
+uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Gimbal device supports rotating around pitch axis.
+uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Gimbal device supports to follow a pitch angle relative to the vehicle
+uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized)
+uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Gimbal device supports rotating around yaw axis.
+uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default)
+uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Gimbal device supports locking to an absolute heading (often this is an option available)
+uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Gimbal device supports yawing/panning infinetely (e.g. using slip disk).
+
+uint16 custom_cap_flags # Bitmap for use for gimbal-specific capability flags.
+float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 pitch_min # Minimum pitch angle (positive: up, negative: down)
+float32 pitch_max # Maximum pitch angle (positive: up, negative: down)
+float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left)
+float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left)

src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg

@@ -0,0 +1,18 @@
+# MAVLink message: GIMBAL_DEVICE_SET_ATTITUDE
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE
+
+uint8 target_system         # System ID
+uint8 target_component      # Component ID
+
+uint16 flags                # Low level gimbal flags (bitwise) - See GIMBAL_DEVICE_FLAGS
+#GIMBAL_DEVICE_FLAGS
+uint16 FLAGS_RETRACT = 1     # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint16 FLAGS_NEUTRAL = 2     # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint16 FLAGS_ROLL_LOCK = 4   # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint16 FLAGS_PITCH_LOCK = 8  # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint16 FLAGS_YAW_LOCK = 16   # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
+float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored.
+float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored.
+float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored.

src/external/mavros_msgs/msg/GimbalManagerInformation.msg

@@ -0,0 +1,29 @@
+# MAVLink message: GIMBAL_MANAGER_INFORMATION
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION
+
+std_msgs/Header header
+
+uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_MANAGER_CAP_FLAGS
+#GIMBAL_MANAGER_CAP_FLAGS
+uint32 CAP_FLAGS_HAS_RETRACT = 1 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT.
+uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL.
+uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS.
+uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW.
+uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK.
+uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS.
+uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW.
+uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK.
+uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS.
+uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW.
+uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK.
+uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW.
+uint32 CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 # Gimbal manager supports to point to a local position.
+uint32 CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 # Gimbal manager supports to point to a global latitude, longitude, altitude position.
+
+uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for.
+float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 pitch_min # Minimum pitch angle (positive: up, negative: down)
+float32 pitch_max # Maximum pitch angle (positive: up, negative: down)
+float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left)
+float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left)

src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg

@@ -0,0 +1,24 @@
+# MAVLink message: GIMBAL_MANAGER_SET_ATTITUDE
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE
+
+uint8 target_system         # System ID
+uint8 target_component      # Component ID
+
+uint32 flags                # High level gimbal manager flags to use (bitwise) - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1     # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2     # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4   # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8  # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16   # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id  # Component ID of gimbal device to address 
+                        # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+                        # components. Send command multiple times for more than
+                        # one gimbal (but not all gimbals).  Default Mavlink gimbal 
+                        # device ids: 154, 171-175
+
+geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
+float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored.
+float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored.
+float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored.

src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg

@@ -0,0 +1,27 @@
+# MAVLink message: GIMBAL_MANAGER_SET_PITCHYAW
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_PITCHYAW
+# Note that this message structure is identical also to GIMBAL_MANAGER_SET_MANUAL_CONTROL and is 
+# reused as such by the plugin
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_MANUAL_CONTROL
+
+uint8 target_system         # System ID
+uint8 target_component      # Component ID
+
+uint32 flags                # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1     # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2     # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4   # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8  # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16   # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id  # Component ID of gimbal device to address 
+                        # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+                        # components. Send command multiple times for more than
+                        # one gimbal (but not all gimbals).  Default Mavlink gimbal 
+                        # device ids: 154, 171-175
+
+float32 pitch # Pitch angle (positive: up, negative: down, NaN to be ignored).
+float32 yaw # Yaw angle (positive: to the right, negative: to the left, NaN to be ignored).
+float32 pitch_rate # Pitch angular rate (positive: up, negative: down, NaN to be ignored).
+float32 yaw_rate # Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored).

src/external/mavros_msgs/msg/GimbalManagerStatus.msg

@@ -0,0 +1,19 @@
+# MAVLink message: GIMBAL_MANAGER_STATUS
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS
+
+std_msgs/Header header
+
+uint32 flags                # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1     # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2     # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4   # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8  # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16   # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for.
+
+uint8 sysid_primary     # System ID of MAVLink component with primary control, 0 for none.
+uint8 compid_primary    # Component ID of MAVLink component with primary control, 0 for none.
+uint8 sysid_secondary   # System ID of MAVLink component with secondary control, 0 for none.
+uint8 compid_secondary  # Component ID of MAVLink component with secondary control, 0 for none.

src/external/mavros_msgs/msg/GlobalPositionTarget.msg

@@ -0,0 +1,34 @@
+# Message for SET_POSITION_TARGET_GLOBAL_INT
+#
+# https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT
+# Some complex system requires all feautures that mavlink
+# message provide. See issue #402, #415.
+
+std_msgs/Header header
+
+uint8 coordinate_frame
+uint8 FRAME_GLOBAL_INT = 5
+uint8 FRAME_GLOBAL_REL_ALT = 6
+uint8 FRAME_GLOBAL_TERRAIN_ALT = 11
+
+uint16 type_mask
+uint16 IGNORE_LATITUDE = 1	# Position ignore flags
+uint16 IGNORE_LONGITUDE = 2
+uint16 IGNORE_ALTITUDE = 4
+uint16 IGNORE_VX = 8	# Velocity vector ignore flags
+uint16 IGNORE_VY = 16
+uint16 IGNORE_VZ = 32
+uint16 IGNORE_AFX = 64	# Acceleration/Force vector ignore flags
+uint16 IGNORE_AFY = 128
+uint16 IGNORE_AFZ = 256
+uint16 FORCE = 512	# Force in af vector flag
+uint16 IGNORE_YAW = 1024
+uint16 IGNORE_YAW_RATE = 2048
+
+float64 latitude
+float64 longitude
+float32 altitude	# in meters, AMSL or above terrain
+geometry_msgs/Vector3 velocity
+geometry_msgs/Vector3 acceleration_or_force
+float32 yaw
+float32 yaw_rate

src/external/mavros_msgs/msg/HilActuatorControls.msg

@@ -0,0 +1,10 @@
+# HilActuatorControls.msg
+#
+# ROS representation of MAVLink HIL_ACTUATOR_CONTROLS
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS
+
+std_msgs/Header header
+float32[16] controls
+uint8 mode
+uint64 flags

src/external/mavros_msgs/msg/HilControls.msg

@@ -0,0 +1,18 @@
+# HilControls.msg
+#
+# ROS representation of MAVLink HIL_CONTROLS
+# (deprecated, use HIL_ACTUATOR_CONTROLS instead)
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_CONTROLS
+
+std_msgs/Header header
+float32 roll_ailerons
+float32 pitch_elevator
+float32 yaw_rudder
+float32 throttle
+float32 aux1
+float32 aux2
+float32 aux3
+float32 aux4
+uint8 mode
+uint8 nav_mode

src/external/mavros_msgs/msg/HilGPS.msg

@@ -0,0 +1,17 @@
+# HilControls.msg
+#
+# ROS representation of MAVLink HIL_GPS
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_GPS
+
+std_msgs/Header header
+uint8 fix_type
+geographic_msgs/GeoPoint geo
+uint16 eph
+uint16 epv
+uint16 vel
+int16 vn
+int16 ve
+int16 vd
+uint16 cog
+uint8 satellites_visible

src/external/mavros_msgs/msg/HilSensor.msg

@@ -0,0 +1,16 @@
+# HilSensor.msg
+#
+# ROS representation of MAVLink HIL_SENSOR
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_SENSOR
+
+std_msgs/Header header
+
+geometry_msgs/Vector3 acc
+geometry_msgs/Vector3 gyro
+geometry_msgs/Vector3 mag
+float32 abs_pressure
+float32 diff_pressure
+float32 pressure_alt
+float32 temperature
+uint32 fields_updated

src/external/mavros_msgs/msg/HilStateQuaternion.msg

@@ -0,0 +1,15 @@
+# HilStateQuaternion.msg
+#
+# ROS representation of MAVLink HIL_STATE_QUATERNION
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION
+
+std_msgs/Header header
+
+geometry_msgs/Quaternion orientation
+geometry_msgs/Vector3 angular_velocity
+geometry_msgs/Vector3 linear_acceleration
+geometry_msgs/Vector3 linear_velocity
+geographic_msgs/GeoPoint geo
+float32 ind_airspeed
+float32 true_airspeed

src/external/mavros_msgs/msg/HomePosition.msg

@@ -0,0 +1,10 @@
+# MAVLink message: HOME_POSITION
+# https://mavlink.io/en/messages/common.html#HOME_POSITION
+
+std_msgs/Header header
+
+geographic_msgs/GeoPoint geo # geodetic coordinates in WGS-84 datum
+
+geometry_msgs/Point position	# local position
+geometry_msgs/Quaternion orientation	# XXX: verify field name (q[4])
+geometry_msgs/Vector3 approach	# position of the end of approach vector

src/external/mavros_msgs/msg/LandingTarget.msg

@@ -0,0 +1,32 @@
+# MAVLink message: LANDING_TARGET
+# https://mavlink.io/en/messages/common.html
+
+std_msgs/Header header
+
+## MAV_FRAME enum
+uint8 GLOBAL = 0                   # Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL)
+uint8 LOCAL_NED = 2                # Local coordinate frame, Z-up (x: north, y: east, z: down).
+uint8 MISSION = 3                  # NOT a coordinate frame, indicates a mission command.
+uint8 GLOBAL_RELATIVE_ALT = 4      # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location.
+uint8 LOCAL_ENU = 5                # Local coordinate frame, Z-down (x: east, y: north, z: up)
+uint8 GLOBAL_INT = 6               # Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL)
+uint8 GLOBAL_RELATIVE_ALT_INT = 7  # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location.
+uint8 LOCAL_OFFSET_NED = 8         # Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position.
+uint8 BODY_NED = 9                 # Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right.
+uint8 BODY_OFFSET_NED = 10         # Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east.
+uint8 GLOBAL_TERRAIN_ALT = 11      # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model.
+uint8 GLOBAL_TERRAIN_ALT_INT = 12  # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model.
+
+## LANDING_TARGET_TYPE enum
+uint8 LIGHT_BEACON = 0             # Landing target signaled by light beacon (ex: IR-LOCK)
+uint8 RADIO_BEACON = 1             # Landing target signaled by radio beacon (ex: ILS, NDB)
+uint8 VISION_FIDUCIAL = 2          # Landing target represented by a fiducial marker (ex: ARTag)
+uint8 VISION_OTHER = 3             # Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square)
+
+uint8 target_num
+uint8 frame
+float32[2] angle
+float32 distance
+float32[2] size
+geometry_msgs/Pose pose
+uint8 type

src/external/mavros_msgs/msg/LogData.msg

@@ -0,0 +1,11 @@
+# Reply to LogRequestData, - a chunk of a log
+#
+#  :id: - log id
+#  :offset: - offset into the log
+#  :data: - chunk of data (if zero-sized, then there are no more chunks)
+
+std_msgs/Header header
+
+uint16 id
+uint32 offset
+uint8[] data

src/external/mavros_msgs/msg/LogEntry.msg

@@ -0,0 +1,15 @@
+# Information about a single log
+#
+#  :id: - log id
+#  :num_logs: - total number of logs
+#  :last_log_num: - id of last log
+#  :time_utc: - UTC timestamp of log (ros::Time(0) if not available)
+#  :size: - size of log in bytes (may be approximate)
+
+std_msgs/Header header
+
+uint16 id
+uint16 num_logs
+uint16 last_log_num
+builtin_interfaces/Time time_utc
+uint32 size

src/external/mavros_msgs/msg/MagnetometerReporter.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+uint8 report
+float32 confidence

src/external/mavros_msgs/msg/ManualControl.msg

@@ -0,0 +1,7 @@
+# Manual Control state
+std_msgs/Header header
+float32 x
+float32 y
+float32 z
+float32 r
+uint16 buttons

src/external/mavros_msgs/msg/Mavlink.msg

@@ -0,0 +1,38 @@
+# Mavlink message transport type.
+#
+# Used to transport mavlink_message_t via ROS topic
+#
+# :framing_status:
+#       Frame decoding status: OK, CRC error, bad Signature (mavlink v2.0)
+#       You may simply drop all non valid messages.
+#       Used for GCS Bridge to transport unknown messages.
+#
+# :magic:
+#       STX byte, used to determine protocol version v1.0 or v2.0.
+#
+# Please use mavros_msgs::mavlink::convert() from <mavros_msgs/mavlink_convert.hpp>
+# to convert between ROS and MAVLink message type
+
+# mavlink_framing_t enum
+uint8 FRAMING_OK = 1
+uint8 FRAMING_BAD_CRC = 2
+uint8 FRAMING_BAD_SIGNATURE = 3
+
+# stx values
+uint8 MAVLINK_V10 = 254
+uint8 MAVLINK_V20 = 253
+
+std_msgs/Header header
+uint8 framing_status
+
+uint8 magic		# STX byte
+uint8 len
+uint8 incompat_flags
+uint8 compat_flags
+uint8 seq
+uint8 sysid
+uint8 compid
+uint32 msgid		# 24-bit message id
+uint16 checksum
+uint64[<=33] payload64	# max size: (255+2+7)/8
+uint8[<=13] signature	# optional signature, max size: 13

src/external/mavros_msgs/msg/MountControl.msg

@@ -0,0 +1,18 @@
+# MAVLink message: DO_MOUNT_CONTROL
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL
+
+std_msgs/Header header
+
+uint8 mode # See enum MAV_MOUNT_MODE.
+uint8 MAV_MOUNT_MODE_RETRACT = 0
+uint8 MAV_MOUNT_MODE_NEUTRAL = 1
+uint8 MAV_MOUNT_MODE_MAVLINK_TARGETING = 2
+uint8 MAV_MOUNT_MODE_RC_TARGETING = 3
+uint8 MAV_MOUNT_MODE_GPS_POINT = 4
+
+float32 pitch # pitch degrees or degrees/second depending on mount mode.
+float32 roll # roll degrees or degrees/second depending on mount mode.
+float32 yaw # yaw degrees or degrees/second depending on mount mode.
+float32 altitude  # altitude depending on mount mode.
+float32 latitude # latitude in degrees * 1E7, set if appropriate mount mode.
+float32 longitude # longitude in degrees * 1E7, set if appropriate mount mode.

src/external/mavros_msgs/msg/NavControllerOutput.msg

@@ -0,0 +1,12 @@
+# https://mavlink.io/en/messages/common.html#NAV_CONTROLLER_OUTPUT
+
+std_msgs/Header header
+
+float32 nav_roll            # Current desired roll
+float32 nav_pitch           # Current desired pitch
+int16 nav_bearing           # Current desired heading
+int16 target_bearing        # Bearing to current waypoint/target
+uint16 wp_dist              # Distance to active waypoint
+float32 alt_error           # Current altitude error
+float32 aspd_error          # Current airspeed error
+float32 xtrack_error        # Current crosstrack error on x-y plane

src/external/mavros_msgs/msg/OnboardComputerStatus.msg

@@ -0,0 +1,25 @@
+# Mavros message: ONBOARDCOMPUTERSTATUS
+
+std_msgs/Header header
+
+uint8 component               # See enum MAV_COMPONENT
+
+uint32 uptime               # [ms] Time since system boot
+uint8 type                  # Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers.
+uint8[8] cpu_cores          # CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused.
+uint8[10] cpu_combined      # Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused
+uint8[4] gpu_cores          # GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused
+uint8[10] gpu_combined      # Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused.
+int8 temperature_board      # [degC] Temperature of the board. A value of INT8_MAX implies the field is unused.
+int8[8] temperature_core    # [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused.
+int16[4] fan_speed          # [rpm] Fan speeds. A value of INT16_MAX implies the field is unused.
+uint32 ram_usage            # [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused.
+uint32 ram_total            # [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused.
+uint32[4] storage_type      # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused.
+uint32[4] storage_usage     # [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused.
+uint32[4] storage_total     # [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_type         # Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary.
+uint32[6] link_tx_rate      # [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_rx_rate      # [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_tx_max       # [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_rx_max       # [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused.

src/external/mavros_msgs/msg/OpticalFlow.msg

@@ -0,0 +1,9 @@
+# OPTICAL_FLOW message data
+
+std_msgs/Header header
+
+geometry_msgs/Vector3 flow
+geometry_msgs/Vector3 flow_comp_m
+uint8 quality
+float32 ground_distance
+geometry_msgs/Vector3 flow_rate

src/external/mavros_msgs/msg/OpticalFlowRad.msg

@@ -0,0 +1,14 @@
+# OPTICAL_FLOW_RAD message data
+
+std_msgs/Header header
+
+uint32 integration_time_us
+float32 integrated_x
+float32 integrated_y
+float32 integrated_xgyro
+float32 integrated_ygyro
+float32 integrated_zgyro
+int16 temperature
+uint8 quality
+uint32 time_delta_distance_us
+float32 distance

src/external/mavros_msgs/msg/OverrideRCIn.msg

@@ -0,0 +1,9 @@
+# Override RC Input
+# Currently MAVLink defines override for 18 channels
+
+# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE
+
+uint16 CHAN_RELEASE=0
+uint16 CHAN_NOCHANGE=65535
+
+uint16[18] channels

src/external/mavros_msgs/msg/Param.msg

@@ -0,0 +1,11 @@
+# Parameter msg.
+
+# XXX DEPRECATED: replaced by ParamEvent
+
+std_msgs/Header header
+
+string param_id
+mavros_msgs/ParamValue value
+
+uint16 param_index
+uint16 param_count

src/external/mavros_msgs/msg/ParamEvent.msg

@@ -0,0 +1,14 @@
+# Parameter Event
+#
+# That messages replaces mavros_msgs/Param from mavros v1.
+# Reason for that: ROS2 have native message for parameters
+#
+# ROS2 also have it's own ParameterEvent stream, which could be used
+# to get FCU updates too. But that message is simpler to use.
+
+std_msgs/Header header
+
+string param_id
+rcl_interfaces/ParameterValue value
+uint16 param_index
+uint16 param_count

src/external/mavros_msgs/msg/ParamValue.msg

@@ -0,0 +1,12 @@
+# Parameter value storage type.
+#
+# Integer and float fields:
+#
+# if integer != 0: it is integer value
+# else if real != 0.0: it is float value
+# else: it is zero.
+
+# XXX DEPRECATED: replaced by rmw_interfaces/ParameterValue
+
+int64 integer
+float64 real

src/external/mavros_msgs/msg/PlayTuneV2.msg

@@ -0,0 +1,10 @@
+# Play tune V2
+#
+# https://mavlink.io/en/messages/common.html#PLAY_TUNE_V2
+
+## TUNE_FORMAT enum
+uint8 QBASIC1_1 = 1
+uint8 MML_MODERN = 2
+
+uint8 format
+string tune

src/external/mavros_msgs/msg/PositionTarget.msg

@@ -0,0 +1,32 @@
+# Message for SET_POSITION_TARGET_LOCAL_NED
+#
+# Some complex system requires all feautures that mavlink
+# message provide. See issue #402.
+
+std_msgs/Header header
+
+uint8 coordinate_frame
+uint8 FRAME_LOCAL_NED = 1
+uint8 FRAME_LOCAL_OFFSET_NED = 7
+uint8 FRAME_BODY_NED = 8
+uint8 FRAME_BODY_OFFSET_NED = 9
+
+uint16 type_mask
+uint16 IGNORE_PX = 1	# Position ignore flags
+uint16 IGNORE_PY = 2
+uint16 IGNORE_PZ = 4
+uint16 IGNORE_VX = 8	# Velocity vector ignore flags
+uint16 IGNORE_VY = 16
+uint16 IGNORE_VZ = 32
+uint16 IGNORE_AFX = 64	# Acceleration/Force vector ignore flags
+uint16 IGNORE_AFY = 128
+uint16 IGNORE_AFZ = 256
+uint16 FORCE = 512	# Force in af vector flag
+uint16 IGNORE_YAW = 1024
+uint16 IGNORE_YAW_RATE = 2048
+
+geometry_msgs/Point position
+geometry_msgs/Vector3 velocity
+geometry_msgs/Vector3 acceleration_or_force
+float32 yaw
+float32 yaw_rate

src/external/mavros_msgs/msg/RCIn.msg

@@ -0,0 +1,5 @@
+# RAW RC input state
+
+std_msgs/Header header
+uint8 rssi
+uint16[] channels

src/external/mavros_msgs/msg/RCOut.msg

@@ -0,0 +1,4 @@
+# RAW Servo out state
+
+std_msgs/Header header
+uint16[] channels

src/external/mavros_msgs/msg/RTCM.msg

@@ -0,0 +1,6 @@
+# RTCM message for the gps_rtk plugin
+# The gps_rtk plugin will fragment the data if necessary and
+# forward it to the FCU via Mavlink through the available link.
+# data should be <= 4*180, higher will be discarded.
+std_msgs/Header header
+uint8[] data

src/external/mavros_msgs/msg/RTKBaseline.msg

@@ -0,0 +1,23 @@
+# RTKBaseline received from the FCU.
+# Full description: https://mavlink.io/en/messages/common.html#GPS_RTK
+# Mavlink Common, #127and #128
+
+std_msgs/Header header
+
+uint32 time_last_baseline_ms
+uint8 rtk_receiver_id
+uint16 wn
+uint32 tow
+uint8 rtk_health
+uint8 rtk_rate
+uint8 nsats
+
+uint8 baseline_coords_type 
+uint8 RTK_BASELINE_COORDINATE_SYSTEM_ECEF = 0   # Earth-centered, earth-fixed
+uint8 RTK_BASELINE_COORDINATE_SYSTEM_NED = 1    # RTK basestation centered, north, east, down
+
+int32 baseline_a_mm
+int32 baseline_b_mm
+int32 baseline_c_mm
+uint32 accuracy
+int32 iar_num_hypotheses

src/external/mavros_msgs/msg/RadioStatus.msg

@@ -0,0 +1,16 @@
+# RADIO_STATUS message
+
+std_msgs/Header header
+
+# message data
+uint8 rssi
+uint8 remrssi
+uint8 txbuf
+uint8 noise
+uint8 remnoise
+uint16 rxerrors
+uint16 fixed
+
+# calculated
+float32 rssi_dbm
+float32 remrssi_dbm

src/external/mavros_msgs/msg/State.msg

@@ -0,0 +1,82 @@
+# Current autopilot state
+#
+# Known modes listed here:
+# http://wiki.ros.org/mavros/CustomModes
+#
+# For system_status values
+# see https://mavlink.io/en/messages/common.html#MAV_STATE
+#
+
+std_msgs/Header header
+bool connected
+bool armed
+bool guided
+bool manual_input
+string mode
+uint8 system_status
+
+string MODE_APM_PLANE_MANUAL = MANUAL
+string MODE_APM_PLANE_CIRCLE = CIRCLE
+string MODE_APM_PLANE_STABILIZE = STABILIZE
+string MODE_APM_PLANE_TRAINING = TRAINING
+string MODE_APM_PLANE_ACRO = ACRO
+string MODE_APM_PLANE_FBWA = FBWA
+string MODE_APM_PLANE_FBWB = FBWB
+string MODE_APM_PLANE_CRUISE = CRUISE
+string MODE_APM_PLANE_AUTOTUNE = AUTOTUNE
+string MODE_APM_PLANE_AUTO = AUTO
+string MODE_APM_PLANE_RTL = RTL
+string MODE_APM_PLANE_LOITER = LOITER
+string MODE_APM_PLANE_LAND = LAND
+string MODE_APM_PLANE_GUIDED = GUIDED
+string MODE_APM_PLANE_INITIALISING = INITIALISING
+string MODE_APM_PLANE_QSTABILIZE = QSTABILIZE
+string MODE_APM_PLANE_QHOVER = QHOVER
+string MODE_APM_PLANE_QLOITER = QLOITER
+string MODE_APM_PLANE_QLAND = QLAND
+string MODE_APM_PLANE_QRTL = QRTL
+
+string MODE_APM_COPTER_STABILIZE = STABILIZE
+string MODE_APM_COPTER_ACRO = ACRO
+string MODE_APM_COPTER_ALT_HOLD = ALT_HOLD
+string MODE_APM_COPTER_AUTO = AUTO
+string MODE_APM_COPTER_GUIDED = GUIDED
+string MODE_APM_COPTER_LOITER = LOITER
+string MODE_APM_COPTER_RTL = RTL
+string MODE_APM_COPTER_CIRCLE = CIRCLE
+string MODE_APM_COPTER_POSITION = POSITION
+string MODE_APM_COPTER_LAND = LAND
+string MODE_APM_COPTER_OF_LOITER = OF_LOITER
+string MODE_APM_COPTER_DRIFT = DRIFT
+string MODE_APM_COPTER_SPORT = SPORT
+string MODE_APM_COPTER_FLIP = FLIP
+string MODE_APM_COPTER_AUTOTUNE = AUTOTUNE
+string MODE_APM_COPTER_POSHOLD = POSHOLD
+string MODE_APM_COPTER_BRAKE = BRAKE
+string MODE_APM_COPTER_THROW = THROW
+string MODE_APM_COPTER_AVOID_ADSB = AVOID_ADSB
+string MODE_APM_COPTER_GUIDED_NOGPS = GUIDED_NOGPS
+
+string MODE_APM_ROVER_MANUAL = MANUAL
+string MODE_APM_ROVER_LEARNING = LEARNING
+string MODE_APM_ROVER_STEERING = STEERING
+string MODE_APM_ROVER_HOLD = HOLD
+string MODE_APM_ROVER_AUTO = AUTO
+string MODE_APM_ROVER_RTL = RTL
+string MODE_APM_ROVER_GUIDED = GUIDED
+string MODE_APM_ROVER_INITIALISING = INITIALISING
+
+string MODE_PX4_MANUAL = MANUAL
+string MODE_PX4_ACRO = ACRO
+string MODE_PX4_ALTITUDE = ALTCTL
+string MODE_PX4_POSITION = POSCTL
+string MODE_PX4_OFFBOARD = OFFBOARD
+string MODE_PX4_STABILIZED = STABILIZED
+string MODE_PX4_RATTITUDE = RATTITUDE
+string MODE_PX4_MISSION = AUTO.MISSION
+string MODE_PX4_LOITER = AUTO.LOITER
+string MODE_PX4_RTL = AUTO.RTL
+string MODE_PX4_LAND = AUTO.LAND
+string MODE_PX4_RTGS = AUTO.RTGS
+string MODE_PX4_READY = AUTO.READY
+string MODE_PX4_TAKEOFF = AUTO.TAKEOFF

src/external/mavros_msgs/msg/StatusEvent.msg

@@ -0,0 +1,19 @@
+# EVENT message representation
+# https://mavlink.io/en/messages/common.html#EVENT
+
+# Severity levels
+uint8 EMERGENCY = 0
+uint8 ALERT = 1
+uint8 CRITICAL = 2
+uint8 ERROR = 3
+uint8 WARNING = 4
+uint8 NOTICE = 5
+uint8 INFO = 6
+uint8 DEBUG = 7
+
+# Fields
+std_msgs/Header header
+uint8 severity
+uint32 px4_id
+uint8[40] arguments
+uint16 sequence

src/external/mavros_msgs/msg/StatusText.msg

@@ -0,0 +1,17 @@
+# STATUSTEXT message representation
+# https://mavlink.io/en/messages/common.html#STATUSTEXT
+
+# Severity levels
+uint8 EMERGENCY = 0
+uint8 ALERT = 1
+uint8 CRITICAL = 2
+uint8 ERROR = 3
+uint8 WARNING = 4
+uint8 NOTICE = 5
+uint8 INFO = 6
+uint8 DEBUG = 7
+
+# Fields
+std_msgs/Header header
+uint8 severity
+string text

src/external/mavros_msgs/msg/SysStatus.msg

@@ -0,0 +1,15 @@
+std_msgs/Header header
+
+uint32 sensors_present
+uint32 sensors_enabled
+uint32 sensors_health
+uint16 load
+uint16 voltage_battery
+int16 current_battery
+int8 battery_remaining
+uint16 drop_rate_comm
+uint16 errors_comm
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4

src/external/mavros_msgs/msg/TerrainReport.msg

@@ -0,0 +1,12 @@
+# Message for TERRAIN_REPORT
+# https://mavlink.io/en/messages/common.html#TERRAIN_REPORT
+
+std_msgs/Header header
+
+float64 latitude
+float64 longitude
+uint16 spacing
+float32 terrain_height # in meters, terrain height
+float32 current_height # in meters, vehicle height above terrain
+uint16 pending
+uint16 loaded 

src/external/mavros_msgs/msg/Thrust.msg

@@ -0,0 +1,5 @@
+# Thrust to send to the FCU
+
+std_msgs/Header header
+
+float32 thrust

src/external/mavros_msgs/msg/TimesyncStatus.msg

@@ -0,0 +1,7 @@
+# Status of the MAVLink time synchroniser
+
+std_msgs/Header header
+uint64 remote_timestamp_ns		# remote system timestamp (nanoseconds)
+int64 observed_offset_ns		# raw time offset directly observed from this timesync packet (nanoseconds)
+int64 estimated_offset_ns		# smoothed time offset between companion system and Mavros (nanoseconds)
+float32 round_trip_time_ms		# round trip time of this timesync packet (milliseconds)

src/external/mavros_msgs/msg/Trajectory.msg

@@ -0,0 +1,19 @@
+# MAVLink message: TRAJECTORY
+# https://mavlink.io/en/messages/common.html#TRAJECTORY
+
+std_msgs/Header header
+
+uint8 type # See enum MAV_TRAJECTORY_REPRESENTATION.
+uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0
+uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1
+
+mavros_msgs/PositionTarget point_1
+mavros_msgs/PositionTarget point_2
+mavros_msgs/PositionTarget point_3
+mavros_msgs/PositionTarget point_4
+mavros_msgs/PositionTarget point_5
+
+uint8[5] point_valid # States if respective point is valid.
+uint16[5] command # MAV_CMD associated with each point. UINT16_MAX if unused.
+
+float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN

src/external/mavros_msgs/msg/Tunnel.msg

@@ -0,0 +1,27 @@
+# Tunnel
+#
+# https://mavlink.io/en/messages/common.html#TUNNEL
+
+uint8 target_system
+uint8 target_component
+uint16 payload_type
+uint8 payload_length
+uint8[128] payload
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum('MAV_TUNNEL_PAYLOAD_TYPE', 'PAYLOAD_TYPE_', 16)
+# ]]]
+# MAV_TUNNEL_PAYLOAD_TYPE
+uint16 PAYLOAD_TYPE_UNKNOWN = 0          # Encoding of payload unknown.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller.
+# [[[end]]] (checksum: 3327b212af02c2d47d940cd6de049624)

src/external/mavros_msgs/msg/VehicleInfo.msg

@@ -0,0 +1,31 @@
+# Vehicle Info msg
+
+std_msgs/Header header
+
+uint8 HAVE_INFO_HEARTBEAT = 1
+uint8 HAVE_INFO_AUTOPILOT_VERSION = 2
+uint8 available_info		# Bitmap shows what info is available
+
+# Vehicle address
+uint8 sysid                     # SYSTEM ID
+uint8 compid                    # COMPONENT ID
+
+# -*- Heartbeat info -*-
+uint8 autopilot                 # MAV_AUTOPILOT
+uint8 type                      # MAV_TYPE
+uint8 system_status             # MAV_STATE
+uint8 base_mode
+uint32 custom_mode
+string mode                     # MAV_MODE string
+uint32 mode_id                  # MAV_MODE number
+
+# -*- Autopilot version -*-
+uint64 capabilities             # MAV_PROTOCOL_CAPABILITY
+uint32 flight_sw_version        # Firmware version number
+uint32 middleware_sw_version    # Middleware version number
+uint32 os_sw_version            # Operating system version number
+uint32 board_version            # HW / board version (last 8 bytes should be silicon ID, if any)
+string flight_custom_version    # Custom version field, commonly from the first 8 bytes of the git hash
+uint16 vendor_id                # ID of the board vendor
+uint16 product_id               # ID of the product
+uint64 uid                      # UID if provided by hardware

src/external/mavros_msgs/msg/VfrHud.msg

@@ -0,0 +1,11 @@
+# Metrics typically displayed on a HUD for fixed wing aircraft
+#
+# VFR_HUD message
+
+std_msgs/Header header
+float32 airspeed	# m/s
+float32 groundspeed	# m/s
+int16 heading		# degrees 0..360
+float32 throttle		# normalized to 0.0..1.0
+float32 altitude		# MSL
+float32 climb		# current climb rate m/s

src/external/mavros_msgs/msg/Vibration.msg

@@ -0,0 +1,7 @@
+# VIBRATION message data
+# @description: Vibration levels and accelerometer clipping
+
+std_msgs/Header header
+
+geometry_msgs/Vector3 vibration		# 3-axis vibration levels
+float32[3] clipping				# Accelerometers clipping

src/external/mavros_msgs/msg/Waypoint.msg

@@ -0,0 +1,45 @@
+# Waypoint.msg
+#
+# ROS representation of MAVLink MISSION_ITEM
+# See mavlink documentation
+
+
+
+# see enum MAV_FRAME
+uint8 frame
+uint8 FRAME_GLOBAL = 0
+uint8 FRAME_LOCAL_NED = 1
+uint8 FRAME_MISSION = 2
+uint8 FRAME_GLOBAL_REL_ALT = 3
+uint8 FRAME_LOCAL_ENU = 4
+uint8 FRAME_GLOBAL_INT = 5
+uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6
+uint8 FRAME_LOCAL_OFFSET_NED = 7
+uint8 FRAME_BODY_NED = 8
+uint8 FRAME_BODY_OFFSET_NED = 9
+uint8 FRAME_GLOBAL_TERRAIN_ALT = 10
+uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11
+uint8 FRAME_BODY_FRD = 12
+uint8 FRAME_RESERVED_13 = 13
+uint8 FRAME_RESERVED_14 = 14
+uint8 FRAME_RESERVED_15 = 15
+uint8 FRAME_RESERVED_16 = 16
+uint8 FRAME_RESERVED_17 = 17
+uint8 FRAME_RESERVED_18 = 18
+uint8 FRAME_RESERVED_19 = 19
+uint8 FRAME_LOCAL_FRD = 20
+uint8 FRAME_LOCAL_FLU = 21
+
+# see enum MAV_CMD and CommandCode.msg
+uint16 command
+
+bool is_current
+bool autocontinue
+# meaning of this params described in enum MAV_CMD
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+float64 x_lat
+float64 y_long
+float64 z_alt

src/external/mavros_msgs/msg/WaypointList.msg

@@ -0,0 +1,9 @@
+# WaypointList.msg
+#
+#  :current_seq:   seq nr of currently active waypoint
+#                  waypoints[current_seq].is_current == True
+#
+#  :waypoints:     list of waypoints
+
+uint16 current_seq
+mavros_msgs/Waypoint[] waypoints