tmpCommit1

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

.gitignore

@@ -8,7 +8,8 @@
 # 編譯的資料夾
 **/build/
 **/install/
-log/
+**/log/
+**/logs/
 
 # CMAKE 的衍生檔
 CMakeCache.txt
@@ -24,3 +25,15 @@ Makefile
 **/*.class
 **/*.pyc
 **/*.pyo
+**/.cursor/
+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,20 +1,109 @@
 這是天雷系統的專案
 
 ===
-專案核心環境
+## 功能簡介
+1. mavlink 多對多串流與交換
+2. 支援 xbee 與 telemetry
+3. 支援 udp 管理
+4. ROS2 介面監控載具
+
+===
+使用限制
+1. 不允許進到 ros 系統有相同 sysid
+2. 假設一台載具上所有 component 共用同一 socket
+
+===
+## 運行環境
+
+### 專案核心框架
 1. ROS2 Humble
-2. Python
-3. Ardupilot
+2. Python 3.8.10
 
-===
-必要相依套件
+### 必要相依套件及版本
+- Python
+[Package] fc_network_adapter
+    1. pymavlink -> Version: 2.4.42
+    2. conda-forge 中的 pyserial-asyncio
+    3. importlib_metadata -> Version: 8.5.0
+    4. setuptools -> Version: 58.2.0 (版本太新不行)
+    5. pyserial-asyncio
+[Package] GUI
+    1. testresources
+    2. websockets
+    3. PyQt6
+    4. PyQt6-WebEngine
 
+- ROS2
+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 簡述
+===
+## 使用說明
+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 -> 倫渝
+8. logs 是執行時期的記錄檔
+
+===

src/GUI/comm_panel.py

@@ -0,0 +1,774 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
+                            QPushButton, QLineEdit, QComboBox, QApplication)
+from PyQt6.QtGui import QFont
+from PyQt6.QtCore import pyqtSignal
+import glob
+import os
+
+
+def _get_font_scale():
+    app = QApplication.instance()
+    if app is None:
+        return 1.0
+    scale = app.property("font_scale")
+    try:
+        return float(scale) if scale is not None else 1.0
+    except (TypeError, ValueError):
+        return 1.0
+
+
+def _scale_stylesheet_font_sizes(stylesheet, scale):
+    if not stylesheet or 'font-size' not in stylesheet:
+        return stylesheet
+
+    import re
+
+    def repl(match):
+        size = float(match.group(1))
+        unit = match.group(2)
+        scaled = max(1.0, size * scale)
+        text = f"{scaled:.2f}".rstrip('0').rstrip('.')
+        return f"font-size: {text}{unit}"
+
+    return re.sub(r'font-size\s*:\s*([0-9]+(?:\.[0-9]+)?)\s*(px|pt)', repl, stylesheet)
+
+
+def _set_scaled_stylesheet(widget, stylesheet):
+    widget._base_stylesheet = stylesheet
+    scaled = _scale_stylesheet_font_sizes(stylesheet, _get_font_scale())
+    widget._applied_stylesheet = scaled
+    widget.setStyleSheet(scaled)
+
+
+def _reapply_scaled_stylesheet(widget):
+    current_stylesheet = widget.styleSheet()
+    base_stylesheet = getattr(widget, '_base_stylesheet', None)
+    applied_stylesheet = getattr(widget, '_applied_stylesheet', None)
+
+    if current_stylesheet != applied_stylesheet:
+        base_stylesheet = current_stylesheet
+        widget._base_stylesheet = base_stylesheet
+
+    if base_stylesheet is not None:
+        scaled = _scale_stylesheet_font_sizes(base_stylesheet, _get_font_scale())
+        widget._applied_stylesheet = scaled
+        if current_stylesheet != scaled:
+            widget.setStyleSheet(scaled)
+
+
+def _apply_scaled_font(widget):
+    base_font = getattr(widget, '_base_font_for_scale', None)
+    if base_font is None:
+        app = QApplication.instance()
+        app_base_font = app.property("base_app_font") if app else None
+        base_font = QFont(app_base_font) if app_base_font is not None else QFont(widget.font())
+        widget._base_font_for_scale = QFont(base_font)
+
+    scaled_font = QFont(base_font)
+    scale = _get_font_scale()
+    if base_font.pointSizeF() > 0:
+        scaled_font.setPointSizeF(max(1.0, base_font.pointSizeF() * scale))
+    elif base_font.pointSize() > 0:
+        scaled_font.setPointSize(max(1, int(round(base_font.pointSize() * scale))))
+    widget.setFont(scaled_font)
+
+class CommPanel(QWidget):
+    """通讯设置面板类"""
+    
+    # 定义信号
+    udp_connection_added = pyqtSignal(str, int)  # ip, port
+    udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    udp_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    ws_connection_added = pyqtSignal(str)  # url
+    ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    ws_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    serial_connection_added = pyqtSignal(str, int)  # port, baudrate
+    serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel)  # conn, btn, status_label
+    serial_connection_removed = pyqtSignal(dict, QWidget)  # conn, panel
+    status_message = pyqtSignal(str, int)  # message, timeout
+    
+    def __init__(self, parent=None):
+        super().__init__(parent)
+        self.udp_connections = []
+        self.ws_connections = []
+        self.serial_connections = []
+        self._init_ui()
+        self.apply_font_scale()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(10)
+        
+        # ========== UDP MAVLink 區域 ==========
+        udp_title = QLabel("UDP")
+        _set_scaled_stylesheet(udp_title, """
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(udp_title)
+        
+        # UDP 連接列表容器
+        self.udp_list_widget = QWidget()
+        self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
+        self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.udp_list_layout.setSpacing(5)
+        layout.addWidget(self.udp_list_widget)
+        
+        # UDP 添加新連接區域
+        add_udp_widget = QWidget()
+        add_udp_layout = QHBoxLayout(add_udp_widget)
+        add_udp_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.udp_ip_input = QLineEdit()
+        self.udp_ip_input.setText("127.0.0.1")
+        self.udp_ip_input.setPlaceholderText("IP")
+        _set_scaled_stylesheet(self.udp_ip_input, """
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        self.udp_port_input = QLineEdit()
+        self.udp_port_input.setText("14550")
+        self.udp_port_input.setPlaceholderText("Port")
+        self.udp_port_input.setFixedWidth(80)
+        _set_scaled_stylesheet(self.udp_port_input, """
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        add_udp_btn = QPushButton("添加")
+        add_udp_btn.clicked.connect(self._handle_add_udp)
+        _set_scaled_stylesheet(add_udp_btn, """
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        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)
+        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)
+        
+        layout.addWidget(add_udp_widget)
+        
+        # 分隔線
+        separator = QWidget()
+        separator.setFixedHeight(20)
+        layout.addWidget(separator)
+        
+        # ========== Serial 區域 ==========
+        serial_title = QLabel("Serial")
+        _set_scaled_stylesheet(serial_title, """
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(serial_title)
+        
+        # Serial 連接列表容器
+        self.serial_list_widget = QWidget()
+        self.serial_list_layout = QVBoxLayout(self.serial_list_widget)
+        self.serial_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.serial_list_layout.setSpacing(5)
+        layout.addWidget(self.serial_list_widget)
+        
+        # Serial 添加新連接區域
+        add_serial_widget = QWidget()
+        add_serial_layout = QHBoxLayout(add_serial_widget)
+        add_serial_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.serial_port_combo = QComboBox()
+        _set_scaled_stylesheet(self.serial_port_combo, """
+            QComboBox {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+            QComboBox::drop-down {
+                border: none;
+            }
+            QComboBox::down-arrow {
+                image: none;
+                border-left: 5px solid transparent;
+                border-right: 5px solid transparent;
+                border-top: 5px solid #DDD;
+            }
+        """)
+        self._refresh_serial_ports()
+        
+        refresh_ports_btn = QPushButton("↻")
+        refresh_ports_btn.setFixedWidth(35)
+        refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
+        refresh_ports_btn.setToolTip("重新掃描串口")
+        _set_scaled_stylesheet(refresh_ports_btn, """
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 6px;
+                border-radius: 4px;
+                font-size: 16px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        self.serial_baudrate_combo = QComboBox()
+        self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
+        self.serial_baudrate_combo.setCurrentText('115200')
+        self.serial_baudrate_combo.setFixedWidth(100)
+        _set_scaled_stylesheet(self.serial_baudrate_combo, """
+            QComboBox {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+            QComboBox::drop-down {
+                border: none;
+            }
+            QComboBox::down-arrow {
+                image: none;
+                border-left: 5px solid transparent;
+                border-right: 5px solid transparent;
+                border-top: 5px solid #DDD;
+            }
+        """)
+        
+        add_serial_btn = QPushButton("添加")
+        add_serial_btn.clicked.connect(self._handle_add_serial)
+        _set_scaled_stylesheet(add_serial_btn, """
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        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)
+        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)
+        
+        layout.addWidget(add_serial_widget)
+        
+        # 分隔線
+        separator = QWidget()
+        separator.setFixedHeight(20)
+        layout.addWidget(separator)
+        
+        # ========== WebSocket 區域 ==========
+        ws_title = QLabel("WebSocket")
+        _set_scaled_stylesheet(ws_title, """
+            color: #DDD;
+            font-size: 14px;
+            font-weight: bold;
+            padding: 5px;
+            background-color: #333;
+            border-radius: 4px;
+        """)
+        layout.addWidget(ws_title)
+        
+        # WebSocket 連接列表容器
+        self.ws_list_widget = QWidget()
+        self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
+        self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
+        self.ws_list_layout.setSpacing(5)
+        layout.addWidget(self.ws_list_widget)
+        
+        # WebSocket 添加新連接區域
+        add_ws_widget = QWidget()
+        add_ws_layout = QHBoxLayout(add_ws_widget)
+        add_ws_layout.setContentsMargins(0, 0, 0, 0)
+        
+        self.ws_url_input = QLineEdit()
+        self.ws_url_input.setPlaceholderText("host")
+        _set_scaled_stylesheet(self.ws_url_input, """
+            QLineEdit {
+                background-color: #333;
+                color: #DDD;
+                border: 1px solid #555;
+                border-radius: 4px;
+                padding: 5px;
+            }
+        """)
+        
+        add_ws_btn = QPushButton("添加")
+        add_ws_btn.clicked.connect(self._handle_add_ws)
+        _set_scaled_stylesheet(add_ws_btn, """
+            QPushButton {
+                background-color: #4CAF50;
+                color: white;
+                border: none;
+                padding: 6px 12px;
+                border-radius: 4px;
+                min-width: 30px;
+            }
+            QPushButton:hover { background-color: #45a049; }
+        """)
+        
+        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)
+        
+        layout.addWidget(add_ws_widget)
+        layout.addStretch()
+    
+    def _handle_add_udp(self):
+        """處理添加 UDP 連接"""
+        ip = self.udp_ip_input.text().strip()
+        port_text = self.udp_port_input.text().strip()
+        
+        if not ip or not port_text:
+            self.status_message.emit("請輸入 IP 和 Port", 3000)
+            return
+        
+        try:
+            port = int(port_text)
+            if port < 1 or port > 65535:
+                raise ValueError("Port 超出範圍")
+        except ValueError:
+            self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.udp_connections:
+            if conn['ip'] == ip and conn['port'] == port:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.udp_connection_added.emit(ip, port)
+        
+        # 只清空 port 輸入框，保留 IP
+        self.udp_port_input.clear()
+    
+    def _handle_add_ws(self):
+        """處理添加 WebSocket 連接"""
+        input_url = self.ws_url_input.text().strip()
+        
+        if not input_url:
+            self.status_message.emit("請輸入 WebSocket URL", 3000)
+            return
+        
+        # 自動添加 ws:// 前綴
+        if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+            url = f'ws://{input_url}'
+        else:
+            url = input_url
+        
+        # 基本 URL 格式驗證
+        try:
+            if '://' in url:
+                parts = url.split('://', 1)
+                if len(parts) == 2 and ':' not in parts[1]:
+                    self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+                    return
+        except:
+            self.status_message.emit("URL 格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.ws_connections:
+            if conn['url'] == url:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.ws_connection_added.emit(url)
+        
+        # 清空輸入框
+        self.ws_url_input.clear()
+    
+    def _refresh_serial_ports(self):
+        """重新掃描可用的串口"""
+        self.serial_port_combo.clear()
+        
+        # 掃描 Linux 下的串口設備
+        ports = []
+        
+        # 掃描 USB 串口
+        usb_ports = glob.glob('/dev/ttyUSB*')
+        ports.extend(usb_ports)
+        
+        # 掃描 ACM 串口
+        acm_ports = glob.glob('/dev/ttyACM*')
+        ports.extend(acm_ports)
+        
+        # 排序
+        ports.sort()
+        
+        if ports:
+            self.serial_port_combo.addItems(ports)
+        else:
+            self.serial_port_combo.addItem("沒有找到串口")
+            self.serial_port_combo.setEnabled(False)
+            return
+        
+        self.serial_port_combo.setEnabled(True)
+    
+    def _handle_add_serial(self):
+        """處理添加 Serial 連接"""
+        port = self.serial_port_combo.currentText()
+        baudrate_text = self.serial_baudrate_combo.currentText()
+        
+        if not port or port == "沒有找到串口":
+            self.status_message.emit("請選擇有效的串口", 3000)
+            return
+        
+        try:
+            baudrate = int(baudrate_text)
+        except ValueError:
+            self.status_message.emit("波特率格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.serial_connections:
+            if conn['port'] == port:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.serial_connection_added.emit(port, baudrate)
+    
+    def _handle_add_ws(self):
+        """處理添加 WebSocket 連接"""
+        input_url = self.ws_url_input.text().strip()
+        
+        if not input_url:
+            self.status_message.emit("請輸入 WebSocket URL", 3000)
+            return
+        
+        # 自動添加 ws:// 前綴
+        if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+            url = f'ws://{input_url}'
+        else:
+            url = input_url
+        
+        # 基本 URL 格式驗證
+        try:
+            if '://' in url:
+                parts = url.split('://', 1)
+                if len(parts) == 2 and ':' not in parts[1]:
+                    self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+                    return
+        except:
+            self.status_message.emit("URL 格式錯誤", 3000)
+            return
+        
+        # 檢查是否已存在相同連接
+        for conn in self.ws_connections:
+            if conn['url'] == url:
+                self.status_message.emit("連接已存在", 3000)
+                return
+        
+        # 發送信號通知主窗口
+        self.ws_connection_added.emit(url)
+        
+        # 清空輸入框
+        self.ws_url_input.clear()
+    
+    def create_udp_connection_panel(self, conn):
+        """創建 UDP 連接面板"""
+        panel = QWidget()
+        _set_scaled_stylesheet(panel, """
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
+        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            _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))
+        _set_scaled_stylesheet(toggle_btn, """
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
+        _set_scaled_stylesheet(remove_btn, """
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def create_ws_connection_panel(self, conn):
+        """創建 WebSocket 連接面板"""
+        panel = QWidget()
+        _set_scaled_stylesheet(panel, """
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['url']}")
+        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            _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))
+        _set_scaled_stylesheet(toggle_btn, """
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
+        _set_scaled_stylesheet(remove_btn, """
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def add_udp_panel(self, conn):
+        """添加 UDP 連接面板到列表"""
+        panel = self.create_udp_connection_panel(conn)
+        self.udp_list_layout.addWidget(panel)
+        self.udp_connections.append(conn)
+        return panel
+    
+    def add_ws_panel(self, conn):
+        """添加 WebSocket 連接面板到列表"""
+        panel = self.create_ws_connection_panel(conn)
+        self.ws_list_layout.addWidget(panel)
+        self.ws_connections.append(conn)
+        return panel
+    
+    def remove_udp_connection_from_list(self, conn):
+        """從列表中移除 UDP 連接"""
+        if conn in self.udp_connections:
+            self.udp_connections.remove(conn)
+    
+    def remove_ws_connection_from_list(self, conn):
+        """從列表中移除 WebSocket 連接"""
+        if conn in self.ws_connections:
+            self.ws_connections.remove(conn)
+    
+    def create_serial_connection_panel(self, conn):
+        """創建 Serial 連接面板"""
+        panel = QWidget()
+        _set_scaled_stylesheet(panel, """
+            QWidget {
+                background-color: #2A2A2A;
+                border-radius: 6px;
+                padding: 8px;
+                border: 1px solid #444;
+            }
+        """)
+        
+        layout = QHBoxLayout(panel)
+        layout.setContentsMargins(8, 8, 8, 8)
+        
+        # 連接資訊
+        info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
+        _set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
+        
+        # 狀態指示器
+        status_label = QLabel("●")
+        if conn.get('enabled', False):
+            _set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
+            status_label.setToolTip("運行中")
+        else:
+            _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))
+        _set_scaled_stylesheet(toggle_btn, """
+            QPushButton {
+                background-color: #444;
+                color: #DDD;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #555; }
+        """)
+        
+        remove_btn = QPushButton("移除")
+        remove_btn.setFixedWidth(60)
+        remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
+        _set_scaled_stylesheet(remove_btn, """
+            QPushButton {
+                background-color: #d32f2f;
+                color: white;
+                border: none;
+                padding: 4px 8px;
+                border-radius: 3px;
+                font-size: 11px;
+            }
+            QPushButton:hover { background-color: #b71c1c; }
+        """)
+        
+        layout.addWidget(status_label)
+        layout.addWidget(info_label)
+        layout.addStretch()
+        layout.addWidget(toggle_btn)
+        layout.addWidget(remove_btn)
+        
+        # 儲存引用
+        panel.connection = conn
+        panel.toggle_btn = toggle_btn
+        panel.status_label = status_label
+        
+        return panel
+    
+    def add_serial_panel(self, conn):
+        """添加 Serial 連接面板到列表"""
+        panel = self.create_serial_connection_panel(conn)
+        self.serial_list_layout.addWidget(panel)
+        self.serial_connections.append(conn)
+        return panel
+    
+    def remove_serial_connection_from_list(self, conn):
+        """從列表中移除 Serial 連接"""
+        if conn in self.serial_connections:
+            self.serial_connections.remove(conn)
+
+    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,169 @@
+#!/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)
+
+    DEFAULT_TIMEOUT_SEC = 2.0
+
+    def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
+        super().__init__()
+        self._monitor = monitor
+        self._timeout_sec = timeout_sec
+        self._pending = {}  # drone_id -> asyncio.Task
+
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
+        """同步介面。立刻回，結果透過 send_result signal 回報。"""
+        # 取消該機上一個未完成的 task，避免舊指令覆蓋新指令
+        old = self._pending.get(drone_id)
+        if old is not None and not old.done():
+            old.cancel()
+
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+
+        task = loop.create_task(
+            self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
+        )
+        self._pending[drone_id] = task
+
+    async def _do_send(self, drone_id, sysid, lat, lon, alt):
+        try:
+            client = self._monitor.get_or_create_position_client(drone_id)
+            if client is None:
+                self.send_result.emit(
+                    drone_id, sysid, False,
+                    "PositionTargetGlobalIntClient 無法建立"
+                )
+                return
+
+            result = await client.goto_position_only_async(
+                target_sysid=sysid,
+                target_compid=0,
+                latitude_deg=lat,
+                longitude_deg=lon,
+                alt=alt,
+                timeout_sec=self._timeout_sec,
+            )
+            self.send_result.emit(
+                drone_id, sysid, bool(result.success), str(result.message or "")
+            )
+        except asyncio.CancelledError:
+            # 被新的目標取代，正常狀況，不用回報
+            pass
+        except Exception as e:
+            traceback.print_exc()
+            self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
+
+    def close(self):
+        """取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
+        for task in self._pending.values():
+            if not task.done():
+                task.cancel()
+        self._pending.clear()
+        _log("INFO", "Ros2CommandSender 已關閉")

src/GUI/drone_panel.py

@@ -0,0 +1,711 @@
+#!/usr/bin/env python3
+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):
+    """單個無人機面板類別"""
+    
+    # 定義信號
+    mode_change_requested = pyqtSignal(str)  # drone_id
+    arm_requested = pyqtSignal(str)  # drone_id
+    takeoff_requested = pyqtSignal(str)  # drone_id
+    setpoint_requested = pyqtSignal(str)  # drone_id
+    selection_changed = pyqtSignal(str, int)  # drone_id, state
+    
+    def __init__(self, drone_id, parent=None):
+        super().__init__(parent)
+        self.drone_id = drone_id
+        
+        # 提取資訊 (格式: s{socket_seq}_{system_id}, 如 s0_11, s1_12)
+        parts = drone_id.split('_')
+        if len(parts) >= 2:
+            self.socket_seq = parts[0][1:]  # socket 序號 (移除 's' 前綴)
+            self.system_id = parts[1]       # system ID
+            self.display_id = f"ID:{self.system_id}"  # 顯示為 ID:11, ID:12
+        else:
+            self.socket_seq = "?"
+            self.system_id = "?"
+            self.display_id = drone_id
+        
+        self.attitude_indicator = None
+        self._init_ui()
+        self.apply_font_scale()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        self.setObjectName(f"panel_{self.drone_id}")
+        self.setFixedHeight(160)
+        _set_scaled_stylesheet(self, """
+            background-color: #2A2A2A;
+            border-radius: 8px;
+        """)
+        
+        # 主佈局
+        main_layout = QHBoxLayout(self)
+        main_layout.setContentsMargins(8, 8, 8, 8)
+        main_layout.setSpacing(0)
+        
+        # 創建內容容器（包含 info 和 control）
+        content_widget = QWidget()
+        _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)
+        
+        # 左側資訊區域
+        info_widget = self._create_info_section()
+        
+        # 右側態度指示器
+        attitude_widget = self._create_attitude_indicator()
+        
+        # 將 info 和 attitude 加入內容容器
+        content_layout.addWidget(info_widget, 1)
+        content_layout.addWidget(attitude_widget, 0)
+        
+        # 將內容容器加入主佈局
+        main_layout.addWidget(content_widget)
+    
+    def _create_info_section(self):
+        """創建資訊顯示區域"""
+        info_widget = QWidget()
+        info_layout = QVBoxLayout(info_widget)
+        info_layout.setContentsMargins(0, 0, 0, 0)
+        info_layout.setSpacing(4)
+        
+        # 頂部標題欄
+        header = QWidget()
+        header_layout = QHBoxLayout(header)
+        header_layout.setContentsMargins(0, 0, 0, 0)
+        
+        # 勾選框
+        self.checkbox = QCheckBox()
+        self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
+        _set_scaled_stylesheet(self.checkbox, """
+            QCheckBox { 
+                color: #DDD;
+            }
+            QCheckBox::indicator {
+                width: 16px;
+                height: 16px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }
+            QCheckBox::indicator:checked {
+                background-color: #7FFFD4;
+                border: 2px solid #888888;
+            }
+        """)
+        self.checkbox.stateChanged.connect(
+            lambda state: self.selection_changed.emit(self.drone_id, state)
+        )
+        
+        # ID 顯示
+        self.id_label = QLabel(self.display_id)
+        _set_scaled_stylesheet(self.id_label, """
+            font-weight: bold; 
+            font-size: 14px; 
+            color: #7FFFD4;
+            min-width: 80px;
+        """)
+        
+        header_layout.addWidget(self.checkbox)
+        header_layout.addWidget(self.id_label)
+        header_layout.addStretch()
+        
+        info_layout.addWidget(header)
+        
+        # 第一行：狀態 (模式 + ARM狀態)
+        status_row = self._create_status_row()
+        info_layout.addWidget(status_row)
+        
+        # 第二行：電池（拆成百分比與電壓兩欄）
+        battery_row = self._create_battery_row()
+        info_layout.addWidget(battery_row)
+        
+        # 第三行：高度
+        altitude_row = self._create_altitude_row()
+        info_layout.addWidget(altitude_row)
+
+        # 第四行：GNSS 定位狀態
+        gnss_row = self._create_gnss_row()
+        info_layout.addWidget(gnss_row)
+        
+        # 第五行：航向 + 速度
+        nav_row = self._create_nav_row()
+        info_layout.addWidget(nav_row)
+        
+        return info_widget
+    
+    def _create_attitude_indicator(self):
+        """創建態度指示器（ADI 人工地平儀）"""
+        self.attitude_indicator = AttitudeIndicator(self.drone_id)
+        self.attitude_indicator.setFixedSize(90, 100)
+        return self.attitude_indicator
+    
+    def _create_status_row(self):
+        """創建狀態行"""
+        status_row = QWidget()
+        status_layout = QHBoxLayout(status_row)
+        status_layout.setContentsMargins(0, 0, 0, 0)
+        
+        status_title = QLabel("狀態：")
+        _set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;")
+        
+        self.mode_label = QLabel("--")
+        self.mode_label.setObjectName(f"{self.drone_id}_mode")
+        _set_scaled_stylesheet(self.mode_label, "color: #DDD;")
+        
+        self.armed_label = QLabel("--")
+        self.armed_label.setObjectName(f"{self.drone_id}_armed")
+        _set_scaled_stylesheet(self.armed_label, "color: #DDD;")
+        
+        status_layout.addWidget(status_title)
+        status_layout.addWidget(self.mode_label)
+        status_layout.addWidget(self.armed_label)
+        status_layout.addStretch()
+        
+        return status_row
+    
+    def _create_connection_row(self):
+        """創建連接資訊行 (Socket Seq + 連接方式)"""
+        connection_row = QWidget()
+        connection_layout = QHBoxLayout(connection_row)
+        connection_layout.setContentsMargins(0, 0, 0, 0)
+        
+        connection_title = QLabel("Socket：")
+        _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")
+        _set_scaled_stylesheet(self.socket_seq_label, "color: #DDD;")
+        
+        connection_sep = QLabel(" - ")
+        _set_scaled_stylesheet(connection_sep, "color: #DDD;")
+        
+        # 設定連接方式顯示
+        connection_type_map = {
+            'r': 'ROS2',
+            'u': 'UDP',
+            's': 'Serial',
+            'w': 'WS'
+        }
+        connection_type = connection_type_map.get(self.type_prefix, 'Unknown')
+        
+        self.connection_type_label = QLabel(connection_type)
+        self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
+        _set_scaled_stylesheet(self.connection_type_label, "color: #DDD;")
+        
+        connection_layout.addWidget(connection_title)
+        connection_layout.addWidget(self.socket_seq_label)
+        connection_layout.addWidget(connection_sep)
+        connection_layout.addWidget(self.connection_type_label)
+        connection_layout.addStretch()
+        
+        return connection_row
+    
+    def _create_battery_row(self):
+        """創建電池行"""
+        battery_row = QWidget()
+        battery_layout = QHBoxLayout(battery_row)
+        battery_layout.setContentsMargins(0, 0, 0, 0)
+        # 顯示百分比
+        battery_title = QLabel("電池：")
+        _set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;")
+
+        self.battery_pct_label = QLabel("--")
+        self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
+        _set_scaled_stylesheet(self.battery_pct_label, "color: #DDD;")
+
+        # 分隔符
+        separator1 = QLabel(" - ")
+        separator1.setObjectName(f"{self.drone_id}_battery_sep1")
+        _set_scaled_stylesheet(separator1, "color: #DDD;")
+
+        # 顯示電壓
+        self.battery_vol_label = QLabel("--")
+        self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
+        _set_scaled_stylesheet(self.battery_vol_label, "color: #DDD;")
+
+        # 分隔符
+        separator2 = QLabel(" - ")
+        separator2.setObjectName(f"{self.drone_id}_battery_sep2")
+        _set_scaled_stylesheet(separator2, "color: #DDD;")
+
+        # 顯示電池節數 (S count)
+        self.battery_cells_label = QLabel("--")
+        self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
+        _set_scaled_stylesheet(self.battery_cells_label, "color: #DDD;")
+
+        battery_layout.addWidget(battery_title)
+        battery_layout.addWidget(self.battery_pct_label)
+        battery_layout.addWidget(separator1)
+        battery_layout.addWidget(self.battery_vol_label)
+        battery_layout.addWidget(separator2)
+        battery_layout.addWidget(self.battery_cells_label)
+        battery_layout.addStretch()
+        
+        return battery_row
+    
+    def _create_altitude_row(self):
+        """創建高度和速度行"""
+        altitude_row = QWidget()
+        altitude_layout = QHBoxLayout(altitude_row)
+        altitude_layout.setContentsMargins(0, 0, 0, 0)
+        
+        altitude_title = QLabel("高度：")
+        _set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;")
+        
+        self.altitude_label = QLabel("--")
+        self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
+        _set_scaled_stylesheet(self.altitude_label, "color: #DDD;")
+        
+        speed_title = QLabel("速度：")
+        _set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;")
+        
+        self.speed_label = QLabel("--")
+        self.speed_label.setObjectName(f"{self.drone_id}_speed")
+        _set_scaled_stylesheet(self.speed_label, "color: #DDD;")
+        
+        altitude_layout.addWidget(altitude_title)
+        altitude_layout.addWidget(self.altitude_label)
+        altitude_layout.addWidget(speed_title)
+        altitude_layout.addWidget(self.speed_label)
+        altitude_layout.addStretch()
+        
+        return altitude_row
+
+    def _create_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):
+        """位置行已移除（位置座標不再顯示於面板）。"""
+        return QWidget()
+    
+    def _create_nav_row(self):
+        """創建導航行（已移除，不再顯示）"""
+        return QWidget()
+    
+    def update_field(self, field, text, color=None):
+        """更新指定欄位的值"""
+        label = self.findChild(QLabel, f"{self.drone_id}_{field}")
+        if label and label.text() != text:
+            label.setText(text)
+            if color:
+                label.setStyleSheet(f"color: {color};")
+    
+    def set_connection_info(self, socket_seq, connection_type):
+        """設定連接資訊（Socket Seq 和連接方式）
+        connection_type: 'UDP' | 'Serial' | 'WS'
+        """
+        self.socket_seq_label.setText(str(socket_seq))
+        
+        # 顯示友善的連接方式
+        type_display = {
+            'UDP': 'UDP',
+            'Serial': 'Serial',
+            'WS': 'WS'
+        }.get(connection_type, connection_type)
+        
+        self.connection_type_label.setText(type_display)
+
+    
+    def update_attitude(self, heading, roll, pitch):
+        """更新態度指示器"""
+        if self.attitude_indicator:
+            self.attitude_indicator.update_attitude(heading, roll, pitch)
+    
+    def get_checkbox(self):
+        """獲取勾選框"""
+        return self.checkbox
+    
+    def set_checked(self, checked):
+        """設置勾選狀態"""
+        self.checkbox.setChecked(checked)
+    
+    def is_checked(self):
+        """獲取勾選狀態"""
+        return self.checkbox.isChecked()
+
+    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):
+    # 定義信號
+    group_selection_changed = pyqtSignal(str, int)  # socket_id, state
+    
+    def __init__(self, socket_id, color='#AAAAAA', socket_type=None, parent=None):
+        super().__init__(parent)
+        self.socket_id = socket_id
+        self.color = color
+        self.socket_type = socket_type
+        self._init_ui()
+        self.apply_font_scale()
+    
+    def _init_ui(self):
+        """初始化UI"""
+        self.setObjectName(f"socket_group_{self.socket_id}")
+        _set_scaled_stylesheet(self, """
+            background-color: #1E1E1E;
+            border-radius: 12px;
+        """)
+        
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(10, 10, 10, 10)
+        layout.setSpacing(6)
+        
+        # Socket 分組標題行 - 包含勾選框
+        title_row = QWidget()
+        title_layout = QHBoxLayout(title_row)
+        title_layout.setContentsMargins(0, 0, 0, 0)
+        
+        # 分組勾選框
+        self.group_checkbox = QCheckBox()
+        self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
+        _set_scaled_stylesheet(self.group_checkbox, f"""
+            QCheckBox {{ color: #DDD; }}
+            QCheckBox::indicator {{
+                width: 14px;
+                height: 14px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }}
+            QCheckBox::indicator:checked {{
+                background-color: {self.color};
+                border: 2px solid #888888;
+            }}
+            QCheckBox::indicator:indeterminate {{
+                background-color: #666;
+                border: 2px solid #888888;
+            }}
+        """)
+        self.group_checkbox.stateChanged.connect(
+            lambda state: self.group_selection_changed.emit(self.socket_id, state)
+        )
+        
+        # Socket 分組標題
+        if self.socket_type:
+            title_text = f"{self.socket_type} {self.socket_id}"
+        else:
+            title_text = f"Socket {self.socket_id}"
+        self.title_label = QLabel(title_text)
+        _set_scaled_stylesheet(self.title_label, f"""
+            font-weight: bold; 
+            font-size: 16px; 
+            color: {self.color};
+            margin-bottom: 8px;
+            padding: 4px 8px;
+            border-radius: 6px;
+        """)
+        
+        title_layout.addWidget(self.group_checkbox)
+        title_layout.addWidget(self.title_label)
+        title_layout.addStretch()
+        
+        layout.addWidget(title_row)
+        
+        # 創建子容器用於放置該 socket 下的所有無人機面板
+        self.drones_container = QWidget()
+        self.drones_layout = QVBoxLayout(self.drones_container)
+        self.drones_layout.setContentsMargins(0, 0, 0, 0)
+        self.drones_layout.setSpacing(4)
+        
+        layout.addWidget(self.drones_container)
+    
+    def add_drone_panel(self, panel):
+        """添加無人機面板到分組"""
+        self.drones_layout.addWidget(panel)
+    
+    def clear_drones(self):
+        """清空所有無人機面板"""
+        while self.drones_layout.count():
+            item = self.drones_layout.takeAt(0)
+            if item.widget():
+                item.widget().setParent(None)
+    
+    def get_checkbox(self):
+        """獲取分組勾選框"""
+        return self.group_checkbox
+    
+    def set_checked(self, checked):
+        """設置分組勾選狀態"""
+        self.group_checkbox.setChecked(checked)
+    
+    def set_socket_type(self, conn_type):
+        """設置 socket 類型並更新標題"""
+        self.title_label.setText(f"{conn_type} {self.socket_id}")
+    
+    def set_check_state(self, state):
+        """設置分組勾選狀態（支持半選）"""
+        self.group_checkbox.setCheckState(state)
+
+    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):
+    """
+    人工地平儀 (ADI) — 仿 Mission Planner 風格
+    上半部顯示 roll/pitch 人工地平儀，下方細條顯示航向
+    """
+
+    def __init__(self, drone_id, parent=None):
+        super().__init__(parent)
+        self.drone_id = drone_id
+        self.heading = 0.0   # 航向 yaw (0–360)
+        self.roll    = 0.0   # 滾轉 (deg, left- negative)
+        self.pitch   = 0.0   # 俯仰 (deg, nose-up positive)
+        self.setStyleSheet("background-color: transparent;")
+
+    def update_attitude(self, heading, roll, pitch):
+        self.heading = heading % 360
+        self.roll    = roll
+        self.pitch   = pitch
+        self.update()
+
+    # ------------------------------------------------------------------ helpers
+    def _adi_rect(self):
+        """Returns the square rect used for the ADI ball."""
+        w, h = self.width(), self.height()
+        side  = min(w, h - 14)   # leave 14 px at bottom for heading strip
+        x = (w - side) / 2
+        y = 0
+        return x, y, side, side
+
+    # ------------------------------------------------------------------ paint
+    def paintEvent(self, event):
+        p = QPainter(self)
+        p.setRenderHint(QPainter.RenderHint.Antialiasing)
+        self._draw_adi(p)
+        self._draw_heading_strip(p)
+
+    # ---- artificial horizon ------------------------------------------------
+    def _draw_adi(self, p):
+        from PyQt6.QtGui import QPainterPath
+        x0, y0, side, _ = self._adi_rect()
+        cx = x0 + side / 2
+        cy = y0 + side / 2
+        r  = side / 2 - 1
+
+        # clip to circle
+        clip_path = QPainterPath()
+        clip_path.addEllipse(QPointF(cx, cy), r, r)
+        p.setClipPath(clip_path)
+
+        # pixels-per-degree for pitch (10 deg ≈ side/5)
+        ppd = side / 50.0
+
+        # ---- rotate + translate canvas for roll & pitch
+        p.save()
+        p.translate(cx, cy)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
+        pitch_offset = self.pitch * ppd
+
+        # sky (above horizon)
+        sky_color = QColor(30, 100, 180)
+        p.fillRect(int(-r*2), int(-r*2 + pitch_offset), int(r*4), int(r*4), sky_color)
+
+        # ground (below horizon)
+        ground_color = QColor(140, 90, 40)
+        p.fillRect(int(-r*2), int(pitch_offset), int(r*4), int(r*4), ground_color)
+
+        # horizon line
+        p.setPen(QPen(QColor("#FFFFFF"), 2))
+        p.drawLine(int(-r), int(pitch_offset), int(r), int(pitch_offset))
+
+        # pitch ladder (every 10°, ±30°)
+        p.setPen(QPen(QColor(255, 255, 255, 180), 1))
+        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
+            yy = int(pitch_offset - deg * ppd)
+            half = int(r * (0.35 if deg % 20 == 0 else 0.22))
+            p.drawLine(-half, yy, half, yy)
+
+        p.restore()
+        p.setClipping(False)
+
+        # ---- roll arc & tick marks (outside clip, fixed frame) ----
+        p.save()
+        p.translate(cx, cy)
+
+        arc_r = r - 2
+        p.setPen(QPen(QColor("#FFFFFF"), 1))
+        # draw arc from -60° to +60°  (Qt arc: 0=3o'clock, CCW, 16ths of deg)
+        p.drawArc(int(-arc_r), int(-arc_r), int(2*arc_r), int(2*arc_r),
+                  (90 - 60) * 16, 120 * 16)
+
+        # tick marks at 0, ±10, ±20, ±30, ±45, ±60
+        for deg in [0, 10, 20, 30, 45, 60, -10, -20, -30, -45, -60]:
+            rad = math.radians(deg - 90)
+            tick = 6 if deg % 30 == 0 else 4
+            x1 = arc_r * math.cos(rad)
+            y1 = arc_r * math.sin(rad)
+            x2 = (arc_r - tick) * math.cos(rad)
+            y2 = (arc_r - tick) * math.sin(rad)
+            p.drawLine(QPointF(x1, y1), QPointF(x2, y2))
+
+        # roll pointer triangle (rotates with roll)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
+        ptr_r = arc_r - 1
+        tri = QPolygonF([
+            QPointF(0, -ptr_r),
+            QPointF(-5, -ptr_r + 9),
+            QPointF(5,  -ptr_r + 9),
+        ])
+        p.setBrush(QColor("#FFFFFF"))
+        p.setPen(Qt.PenStyle.NoPen)
+        p.drawPolygon(tri)
+        p.restore()
+
+        # ---- fixed aircraft symbol ----
+        p.save()
+        p.translate(cx, cy)
+        p.setPen(QPen(QColor("#FFD700"), 2))
+        # left wing
+        p.drawLine(int(-r*0.5), 0, int(-r*0.15), 0)
+        p.drawLine(int(-r*0.15), 0, int(-r*0.15), int(r*0.12))
+        # right wing
+        p.drawLine(int(r*0.15), 0, int(r*0.5), 0)
+        p.drawLine(int(r*0.15), 0, int(r*0.15), int(r*0.12))
+        # centre dot
+        p.setBrush(QColor("#FFD700"))
+        p.drawEllipse(QPointF(0, 0), 2.5, 2.5)
+        p.restore()
+
+        # ---- outer ring ----
+        p.setPen(QPen(QColor("#888888"), 1))
+        p.setBrush(Qt.BrushStyle.NoBrush)
+        p.drawEllipse(QPointF(cx, cy), r, r)
+
+    # ---- heading strip at bottom ------------------------------------------
+    def _draw_heading_strip(self, p):
+        w         = self.width()
+        x0, y0, side, _ = self._adi_rect()
+        strip_y   = y0 + side
+        strip_h   = self.height() - strip_y
+        if strip_h < 4:
+            return
+
+        # background
+        p.fillRect(0, int(strip_y), w, strip_h, QColor(30, 30, 30))
+
+        # heading text centred (bigger)
+        p.setPen(QPen(QColor("#FFFFFF")))
+        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)

src/GUI/mission_executor.py

@@ -0,0 +1,431 @@
+#!/usr/bin/env python3
+"""
+任務執行模組
+管理多架無人機的 GUIDED 模式飛行控制迴圈
+
+設計:
+    - 每架無人機持有一個航點序列，逐點推進
+    - 各自到達就各自切換到下一個航點
+    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
+    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+    - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
+    - 用 QTimer 驅動到達判定，在 Qt 主線程執行
+    - 暫停 = 停止到達判定 + 停止新指令
+"""
+import asyncio
+import math
+import time
+from enum import Enum
+from PyQt6.QtCore import QObject, QTimer, pyqtSignal
+
+
+def _log(level, message):
+    print(f"[{level}] {message}")
+
+
+class MissionState(Enum):
+    """整體任務狀態"""
+    IDLE = "idle"
+    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',
+        'sent_current_wp', '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
+        self.sent_current_wp = False
+        self.fail_count = 0
+        self.status = TaskStatus.NORMAL
+        self.waiting_since = 0.0  # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
+        self.entered_radius_at = 0.0  # 進入 arrival_radius 的時間，0 = 尚未進入
+        self.last_log_at = 0.0  # 上次印 distance log 的時間
+
+    @property
+    def current_target(self):
+        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
+
+    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):
+        super().__init__()
+        self.sender = sender
+        self.drone_gps = drone_gps
+        self.monitor = monitor  # 用於失敗 fallback 到 LOITER
+        self.arrival_radius = arrival_radius
+        self.barrier_timeout_sec = barrier_timeout_sec
+        # hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
+        # 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
+        self.hover_stable_sec = hover_stable_sec
+        self.hysteresis_factor = 1.5
+        self.progress_log_interval_sec = progress_log_interval_sec
+        self.state = MissionState.IDLE
+        self.tasks = {}
+        self.rendezvous_indices = set()
+
+        self._interval_ms = int(1000 / 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"barrier timeout={self.barrier_timeout_sec}s, "
+            f"{rv_info}",
+        )
+
+    def pause(self):
+        if self.state == MissionState.RUNNING:
+            self._timer.stop()
+            self.state = MissionState.PAUSED
+            _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: 發送未送過的目標 ----
+        for task in self.tasks.values():
+            if task.done or task.status in (
+                TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
+            ):
+                continue
+            if task.sent_current_wp:
+                continue
+            target = task.current_target
+            if target is None:
+                continue
+            tgt_lat, tgt_lon, tgt_alt = target
+            task.sent_current_wp = True
+            self.sender.send_position_global(
+                task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
+            )
+
+        # ---- Phase 4: 完成檢查 ----
+        all_done = all(
+            t.done or t.status == TaskStatus.FALLBACK_LOITER
+            for t in self.tasks.values()
+        )
+        if all_done and self.tasks:
+            self._timer.stop()
+            self.state = MissionState.IDLE
+            self.mission_completed.emit()
+            _log("INFO", "任務全部完成")
+
+    def _maybe_log_progress(self, task, now, distance, stable_for):
+        """週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
+        if now - task.last_log_at < self.progress_log_interval_sec:
+            return
+        task.last_log_at = now
+        if stable_for > 0:
+            stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
+        else:
+            stable_info = ""
+        _log(
+            "INFO",
+            f"{task.drone_id} → WP {task.wp_index}: "
+            f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
+        )
+
+    def _advance_waypoint(self, task, arrived_distance):
+        """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
+        task.wp_index += 1
+        task.sent_current_wp = False
+        task.fail_count = 0
+        task.entered_radius_at = 0.0
+        task.last_log_at = 0.0
+        if task.wp_index >= task.total_waypoints:
+            task.done = True
+            self.drone_waypoint_reached.emit(
+                task.drone_id, task.wp_index, task.total_waypoints
+            )
+            _log(
+                "INFO",
+                f"{task.drone_id} 已完成所有航點 "
+                f"({task.total_waypoints}/{task.total_waypoints})",
+            )
+            return
+        self.drone_waypoint_reached.emit(
+            task.drone_id, task.wp_index, task.total_waypoints
+        )
+        _log(
+            "INFO",
+            f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
+            f"(到達距離: {arrived_distance:.1f}m)",
+        )
+
+    def _check_barriers(self, now):
+        """檢查每個有人在等的 barrier 能不能釋放。"""
+        # 蒐集目前有哪些 barrier 有人卡著
+        waiting_groups = {}  # barrier_idx -> [task, ...]
+        for task in self.tasks.values():
+            if task.status == TaskStatus.WAITING_AT_BARRIER:
+                waiting_groups.setdefault(task.wp_index, []).append(task)
+
+        if not waiting_groups:
+            return
+
+        for barrier_idx, waiting_tasks in waiting_groups.items():
+            # 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
+            everyone_arrived = True
+            for task in self.tasks.values():
+                if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+                    continue  # 失敗機/完成機不擋
+                if (task.status == TaskStatus.WAITING_AT_BARRIER
+                        and task.wp_index == barrier_idx):
+                    continue  # 在這裡等
+                if task.wp_index > barrier_idx:
+                    continue  # 早已越過（barrier 先前已對他釋放）
+                # 還在路上
+                everyone_arrived = False
+                break
+
+            force_reason = None
+            if not everyone_arrived:
+                # 檢查 timeout：以該 barrier 上最早進入等待的時間為準
+                oldest_wait = min(t.waiting_since for t in waiting_tasks)
+                if now - oldest_wait >= self.barrier_timeout_sec:
+                    force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
+                    _log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
+                else:
+                    continue  # 還沒到齊、也還沒 timeout → 繼續等
+
+            # 釋放：把所有在此 barrier 等待的機一起推進
+            tag = "全員到齊" if force_reason is None else force_reason
+            _log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
+            for task in waiting_tasks:
+                task.status = TaskStatus.NORMAL
+                msg = f"barrier WP {barrier_idx} released ({tag})"
+                self.task_status_changed.emit(
+                    task.drone_id, task.status.value, msg
+                )
+                self._advance_waypoint(task, self.arrival_radius)
+
+    # ------------------------------------------------------------------ 結果回呼
+
+    def _on_send_result(self, drone_id, sysid, success, message):
+        """Ros2CommandSender.send_result 的 slot"""
+        task = self.tasks.get(drone_id)
+        if task is None or task.done:
+            return
+        # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
+        if task.status == TaskStatus.WAITING_AT_BARRIER:
+            return
+
+        if success:
+            if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
+                task.status = TaskStatus.NORMAL
+                self.task_status_changed.emit(drone_id, task.status.value, "recovered")
+            task.fail_count = 0
+            return
+
+        task.fail_count += 1
+        _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
+
+        if task.fail_count < self.MAX_RETRY:
+            task.status = TaskStatus.RETRYING
+            task.sent_current_wp = False  # 下個 tick 會重送
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+            )
+        else:
+            task.status = TaskStatus.FALLBACK_LOITER
+            self.task_status_changed.emit(
+                drone_id, task.status.value,
+                f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
+            )
+            _log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER")
+            self._fallback_to_loiter(drone_id)
+
+    def _fallback_to_loiter(self, drone_id):
+        """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
+        if self.monitor is None:
+            _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
+            return
+        try:
+            loop = asyncio.get_event_loop()
+        except RuntimeError:
+            loop = asyncio.new_event_loop()
+            asyncio.set_event_loop(loop)
+        loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
+
+
+# ------------------------------------------------------------------ 工具函式
+
+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,640 @@
+#!/usr/bin/env python3
+"""
+任務群組模組
+管理多任務群組的資料結構與無人機分配對話框
+"""
+from PyQt6.QtWidgets import (
+    QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton,
+    QComboBox, QDialog, QCheckBox, QScrollArea, QFrame, QLineEdit
+)
+from PyQt6.QtCore import Qt, pyqtSignal
+from mission_executor import MissionExecutor, MissionState
+
+
+# 群組顏色（循環使用）
+GROUP_COLORS = [
+    "#E53935",  # Deep Red
+    "#FB8C00",  # Deep Orange
+    "#FDD835",  # Deep Yellow
+    "#43A047",  # Deep Green
+    "#1E88E5",  # Deep Blue
+    "#8E24AA",  # Deep Purple
+    "#D81B60",  # Deep Pink
+]
+
+DEFAULT_MISSION_PARAM_VALUES = {
+    'spacing': '5.0',
+    'base_altitude': '10.0',
+    'altitude_diff': '2.0',
+    'radius': '10.0',
+    'altitude': '10.0',
+    'start_angle': '0',
+    'lateral_offset': '3.0',
+    'longitudinal_spacing': '5.0',
+    'line_spacing': '5.0',
+    # 執行參數（所有任務類型共用）
+    'arrival_radius': '4.0',     # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2
+    'hover_stable_sec': '2.0',   # 在 radius 內穩定停留多久才視為到達 (s)
+}
+
+
+# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換）
+EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
+
+
+class MissionGroup:
+    """單一任務群組的資料"""
+
+    def __init__(self, group_id, color):
+        self.group_id = group_id             # 'A', 'B', 'C', ...
+        self.color = color                   # 顏色 hex
+        
+        # 唯一真實資料來源：該 group 選中的 drone ID
+        # 代表：group 擁有的 drone、被選中的 drone、可以操作的 drone
+        self.selected_drone_ids = set()
+        
+        self.mission_type = 'M_FORMATION'    # 預設任務類型
+        self.planned_waypoints = None        # 規劃結果 dict
+        self.executor = None                 # MissionExecutor 實例（延遲建立）
+        self.center_origin = None            # 規劃原點
+        self.leader_drone_id = None          # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
+
+    @property
+    def state(self):
+        if self.executor is None:
+            return MissionState.IDLE
+        return self.executor.state
+
+    @property
+    def display_name(self):
+        return f"Group {self.group_id}"
+
+
+class DroneAssignDialog(QDialog):
+    """無人機分配對話框"""
+
+    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned=None):
+        """
+        Args:
+            parent: 父 widget
+            all_drone_ids: 所有可用無人機 ID 列表
+            current_assigned: 當前群組已分配的無人機 set
+            other_assigned: (忽略 — 現在允許多 group 分配)
+        """
+        super().__init__(parent)
+        self.setWindowTitle("分配無人機")
+        self.setMinimumWidth(280)
+        self.setStyleSheet("""
+            QDialog { background-color: #2B2B2B; }
+            QLabel { color: #DDD; }
+            QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
+            QCheckBox::indicator { width: 16px; height: 16px; }
+        """)
+
+        layout = QVBoxLayout(self)
+
+        title = QLabel("選擇要分配到此群組的無人機：")
+        title.setStyleSheet("font-size: 13px; font-weight: bold; padding-bottom: 6px;")
+        layout.addWidget(title)
+
+        # 滾動區域
+        scroll = QScrollArea()
+        scroll.setWidgetResizable(True)
+        scroll.setMaximumHeight(300)
+        scroll_widget = QWidget()
+        scroll_layout = QVBoxLayout(scroll_widget)
+        scroll_layout.setSpacing(2)
+
+        self.checkboxes = {}
+        sorted_ids = sorted(all_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+
+        for drone_id in sorted_ids:
+            cb = QCheckBox(drone_id)
+
+            # 所有 drone 都能被選摘（支持多 group 分配）
+            if drone_id in current_assigned:
+                cb.setChecked(True)
+
+            self.checkboxes[drone_id] = cb
+            scroll_layout.addWidget(cb)
+
+        scroll_layout.addStretch()
+        scroll.setWidget(scroll_widget)
+        layout.addWidget(scroll)
+
+        # 按鈕
+        btn_layout = QHBoxLayout()
+        ok_btn = QPushButton("確定")
+        ok_btn.setStyleSheet("""
+            QPushButton { background-color: #4A9EFF; color: white; border: none;
+                padding: 8px 20px; border-radius: 4px; font-weight: bold; }
+            QPushButton:hover { background-color: #3A8EEF; }
+        """)
+        ok_btn.clicked.connect(self.accept)
+
+        cancel_btn = QPushButton("取消")
+        cancel_btn.setStyleSheet("""
+            QPushButton { background-color: #555; color: #DDD; border: none;
+                padding: 8px 20px; border-radius: 4px; }
+            QPushButton:hover { background-color: #666; }
+        """)
+        cancel_btn.clicked.connect(self.reject)
+
+        btn_layout.addStretch()
+        btn_layout.addWidget(cancel_btn)
+        btn_layout.addWidget(ok_btn)
+        layout.addLayout(btn_layout)
+
+    def get_selected(self):
+        """回傳勾選的無人機 ID set"""
+        return {did for did, cb in self.checkboxes.items() if cb.isChecked() and cb.isEnabled()}
+
+
+class GroupPanel(QWidget):
+    """單一群組的 UI 面板（嵌入到 tab 中）— 三欄式佈局"""
+
+    # 信號
+    assign_drones_requested = pyqtSignal(str)          # group_id
+    mission_type_changed = pyqtSignal(str, str)        # group_id, mission_type
+    start_requested = pyqtSignal(str)                  # group_id
+    pause_requested = pyqtSignal(str)                  # group_id
+    stop_requested = pyqtSignal(str)                   # group_id
+    mode_change_requested = pyqtSignal(str, str)       # group_id, mode
+    arm_requested = pyqtSignal(str)                    # group_id
+    takeoff_requested = pyqtSignal(str, float)         # group_id, altitude
+    reboot_requested = pyqtSignal(str)                 # group_id
+    box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
+    select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
+    clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
+    add_group_requested = pyqtSignal()                 # 新增群組
+    delete_group_requested = pyqtSignal(str)           # group_id — 刪除群組
+
+    BUTTON_STYLE = """
+        QPushButton {{ background-color: {bg}; color: {fg}; border: none;
+            padding: 5px 8px; border-radius: 4px; font-size: 11px; }}
+        QPushButton:hover {{ background-color: {hover}; }}
+        QPushButton:disabled {{ background-color: #444; color: #666; }}
+    """
+
+    def __init__(self, group: MissionGroup, parent=None, default_params=None):
+        super().__init__(parent)
+        self.group = group
+        self.all_btn_ref = None  # 保存全選按鈕的參考
+        self.default_params = dict(DEFAULT_MISSION_PARAM_VALUES)
+        if default_params:
+            self.default_params.update(default_params)
+        self._build_ui()
+
+    def _make_sep(self):
+        """建立垂直分隔線"""
+        sep = QFrame()
+        sep.setFrameShape(QFrame.Shape.VLine)
+        sep.setStyleSheet("color: #444;")
+        return sep
+
+    def _build_ui(self):
+        layout = QVBoxLayout(self)
+        layout.setContentsMargins(6, 4, 6, 4)
+        layout.setSpacing(0)
+
+        COMBO = ("QComboBox { background-color: #333; color: #DDD; "
+                 "border-radius: 3px; padding: 2px 6px; font-size: 11px; }")
+        BTN = self.BUTTON_STYLE
+        LBL = "color: #AAA; font-size: 11px;"
+        TITLE = "color: #DDD; font-size: 11px; font-weight: bold; padding-bottom: 2px;"
+
+        # ── 三欄主佈局 ──
+        cols = QHBoxLayout()
+        cols.setSpacing(6)
+
+        # ============================
+        # 左欄：控制指令
+        # ============================
+        left = QVBoxLayout()
+        left.setSpacing(3)
+
+        left_title = QLabel("控制指令")
+        left_title.setStyleSheet(TITLE)
+        left.addWidget(left_title)
+
+        # 模式切換
+        mode_row = QHBoxLayout()
+        mode_row.setSpacing(3)
+        self.mode_combo = QComboBox()
+        self.mode_combo.addItems([
+            "GUIDED", "AUTO", "LAND", "LOITER",
+            "STABILIZE", "ALT_HOLD", "RTL", "POSHOLD", "SMART_RTL"
+        ])
+        self.mode_combo.setStyleSheet(COMBO)
+        mode_btn = QPushButton("切換")
+        mode_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        mode_btn.clicked.connect(
+            lambda: self.mode_change_requested.emit(
+                self.group.group_id, self.mode_combo.currentText()))
+        mode_row.addWidget(self.mode_combo, 1)
+        mode_row.addWidget(mode_btn)
+        left.addLayout(mode_row)
+
+        # 解鎖
+        arm_btn = QPushButton("解鎖")
+        arm_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        arm_btn.clicked.connect(
+            lambda: self.arm_requested.emit(self.group.group_id))
+        left.addWidget(arm_btn)
+
+        # 起飛
+        takeoff_row = QHBoxLayout()
+        takeoff_row.setSpacing(3)
+        self.alt_input = QComboBox()
+        self.alt_input.setEditable(True)
+        self.alt_input.addItems(["5", "10", "15", "20"])
+        self.alt_input.setCurrentText("10")
+        self.alt_input.setStyleSheet(COMBO)
+        alt_lbl = QLabel("m")
+        alt_lbl.setStyleSheet(LBL)
+        takeoff_btn = QPushButton("起飛")
+        takeoff_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        takeoff_btn.clicked.connect(self._on_takeoff)
+        takeoff_row.addWidget(self.alt_input, 1)
+        takeoff_row.addWidget(alt_lbl)
+        takeoff_row.addWidget(takeoff_btn)
+        left.addLayout(takeoff_row)
+
+        left.addStretch()
+
+        # ============================
+        # 中欄：任務規劃（左右分割）
+        # ============================
+        mid = QVBoxLayout()
+        mid.setSpacing(2)
+
+        mid_title = QLabel("任務規劃")
+        mid_title.setStyleSheet(TITLE)
+        mid.addWidget(mid_title)
+
+        mid_body = QHBoxLayout()
+        mid_body.setSpacing(4)
+
+        # 左側：類型 + 狀態 + 座標
+        mid_left = QVBoxLayout()
+        mid_left.setSpacing(2)
+        self.type_combo = QComboBox()
+        self.type_combo.addItems([
+            "M_FORMATION", "CIRCLE_FORMATION", "LEADER_FOLLOWER", "GRID_SWEEP"
+        ])
+        self.type_combo.setStyleSheet(COMBO)
+        self.type_combo.currentTextChanged.connect(
+            lambda t: self.mission_type_changed.emit(self.group.group_id, t))
+        mid_left.addWidget(self.type_combo)
+        self.status_label = QLabel("○ 未規劃")
+        self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+        mid_left.addWidget(self.status_label)
+        self.center_label = QLabel("中心: --")
+        self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        mid_left.addWidget(self.center_label)
+        self.target_label = QLabel("目標: --")
+        self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        mid_left.addWidget(self.target_label)
+        mid_left.addStretch()
+
+        # 右側：執行 / 暫停 / 停止（垂直排列）
+        mid_right = QVBoxLayout()
+        mid_right.setSpacing(3)
+        self.start_btn = QPushButton("▶ 執行")
+        self.start_btn.setStyleSheet(BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
+        self.start_btn.clicked.connect(
+            lambda: self.start_requested.emit(self.group.group_id))
+        self.pause_btn = QPushButton("⏸ 暫停")
+        self.pause_btn.setStyleSheet(BTN.format(bg='#F57F17', fg='white', hover='#F9A825'))
+        self.pause_btn.clicked.connect(
+            lambda: self.pause_requested.emit(self.group.group_id))
+        self.stop_btn = QPushButton("■ 停止")
+        self.stop_btn.setStyleSheet(BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
+        self.stop_btn.clicked.connect(
+            lambda: self.stop_requested.emit(self.group.group_id))
+        mid_right.addWidget(self.start_btn)
+        mid_right.addWidget(self.pause_btn)
+        mid_right.addWidget(self.stop_btn)
+        mid_right.addStretch()
+
+        mid_body.addLayout(mid_left, 1)
+        mid_body.addLayout(mid_right)
+        mid.addLayout(mid_body)
+
+        # ============================
+        # 選取與分組（3x2 按鈕）
+        # ============================
+        right = QVBoxLayout()
+        right.setSpacing(3)
+
+        right_title = QLabel("選取與分組")
+        right_title.setStyleSheet(TITLE)
+        right.addWidget(right_title)
+
+        self.drone_list_label = QLabel("尚未分配")
+        self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        self.drone_list_label.setWordWrap(True)
+        right.addWidget(self.drone_list_label)
+
+        # 3x2 按鈕網格：第一行 框選 全選 新增群組
+        grid_r1 = QHBoxLayout()
+        grid_r1.setSpacing(3)
+        box_btn = QPushButton("框選")
+        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        box_btn.clicked.connect(
+            lambda: self.box_select_requested.emit(self.group.group_id))
+        all_btn = QPushButton("全選/取消")
+        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        all_btn.clicked.connect(self._on_all_select_clicked)
+        self.all_btn_ref = all_btn  # 保存按鈕參考（備用）
+        add_group_btn = QPushButton("+ 新增群組")
+        add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
+        add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
+        grid_r1.addWidget(box_btn)
+        grid_r1.addWidget(all_btn)
+        grid_r1.addWidget(add_group_btn)
+        right.addLayout(grid_r1)
+
+        # 第二行 編輯分配 清除分組 刪除群組
+        grid_r2 = QHBoxLayout()
+        grid_r2.setSpacing(3)
+        assign_btn = QPushButton("編輯分配")
+        assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+        assign_btn.clicked.connect(
+            lambda: self.assign_drones_requested.emit(self.group.group_id))
+        clear_btn = QPushButton("清除分組")
+        clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
+        clear_btn.clicked.connect(
+            lambda: self.clear_group_requested.emit(self.group.group_id))
+        self.delete_group_btn = QPushButton("刪除群組")
+        self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        self.delete_group_btn.clicked.connect(
+            lambda: self.delete_group_requested.emit(self.group.group_id))
+        grid_r2.addWidget(assign_btn)
+        grid_r2.addWidget(clear_btn)
+        grid_r2.addWidget(self.delete_group_btn)
+        right.addLayout(grid_r2)
+
+        right.addStretch()
+
+        # ============================
+        # 飛控操作
+        # ============================
+        reboot_col = QVBoxLayout()
+        reboot_col.setSpacing(3)
+
+        reboot_title = QLabel("飛控操作")
+        reboot_title.setStyleSheet(TITLE)
+        reboot_col.addWidget(reboot_title)
+
+        reboot_btn = QPushButton("重啟飛控")
+        reboot_btn.setStyleSheet(BTN.format(bg='#8A3A3A', fg='#FFF', hover='#A84646'))
+        reboot_btn.clicked.connect(
+            lambda: self.reboot_requested.emit(self.group.group_id))
+        reboot_col.addWidget(reboot_btn)
+        reboot_col.addStretch()
+
+        # ============================
+        # 第四欄：任務參數
+        # ============================
+        param_col = QVBoxLayout()
+        param_col.setSpacing(2)
+
+        param_title = QLabel("任務參數")
+        param_title.setStyleSheet(TITLE)
+        param_col.addWidget(param_title)
+
+        INPUT = ("QLineEdit { background-color: #333; color: #DDD; "
+                 "border: 1px solid #555; border-radius: 3px; "
+                 "padding: 1px 4px; font-size: 11px; }")
+
+        # 每種任務類型的參數定義: (key, label, default_value)
+        self._param_defs = {
+            'M_FORMATION': [
+                ('spacing', '間距 (m)', self.default_params['spacing']),
+                ('base_altitude', '基準高度 (m)', self.default_params['base_altitude']),
+                ('altitude_diff', '高低差 (m)', self.default_params['altitude_diff']),
+            ],
+            'CIRCLE_FORMATION': [
+                ('radius', '半徑 (m)', self.default_params['radius']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+                ('start_angle', '起始角 (°)', self.default_params['start_angle']),
+            ],
+            'LEADER_FOLLOWER': [
+                ('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
+                ('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+            ],
+            'GRID_SWEEP': [
+                ('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
+                ('altitude', '高度 (m)', self.default_params['altitude']),
+            ],
+        }
+
+        # 所有 mission type 共用的執行參數（不隨類型切換隱藏）
+        self._exec_param_defs = [
+            ('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
+            ('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
+        ]
+
+        # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
+        self._param_widgets = {}  # key → (label_widget, input_widget)
+        self._param_rows = []     # 所有 row layout 對應的 container widget
+
+        for mission_type, defs in self._param_defs.items():
+            for key, label_text, default in defs:
+                if key in self._param_widgets:
+                    continue  # 同名參數只建一次
+                row_w = QWidget()
+                row_l = QHBoxLayout(row_w)
+                row_l.setContentsMargins(0, 0, 0, 0)
+                row_l.setSpacing(3)
+                lbl = QLabel(label_text)
+                lbl.setStyleSheet(LBL)
+                inp = QLineEdit(default)
+                inp.setStyleSheet(INPUT)
+                inp.setFixedWidth(50)
+                row_l.addWidget(lbl, 1)
+                row_l.addWidget(inp)
+                param_col.addWidget(row_w)
+                self._param_widgets[key] = (row_w, inp)
+                self._param_rows.append(row_w)
+
+        # 執行參數列（所有 mission type 共用，永遠顯示）
+        for key, label_text, default in self._exec_param_defs:
+            row_w = QWidget()
+            row_l = QHBoxLayout(row_w)
+            row_l.setContentsMargins(0, 0, 0, 0)
+            row_l.setSpacing(3)
+            lbl = QLabel(label_text)
+            lbl.setStyleSheet(LBL)
+            inp = QLineEdit(default)
+            inp.setStyleSheet(INPUT)
+            inp.setFixedWidth(50)
+            row_l.addWidget(lbl, 1)
+            row_l.addWidget(inp)
+            param_col.addWidget(row_w)
+            self._param_widgets[key] = (row_w, inp)
+            self._param_rows.append(row_w)
+
+        # LEADER_FOLLOWER 專用：領隊下拉選單
+        self._leader_row = QWidget()
+        leader_layout = QHBoxLayout(self._leader_row)
+        leader_layout.setContentsMargins(0, 0, 0, 0)
+        leader_layout.setSpacing(3)
+        leader_lbl = QLabel("領隊")
+        leader_lbl.setStyleSheet(LBL)
+        self._leader_combo = QComboBox()
+        self._leader_combo.setStyleSheet(COMBO)
+        self._leader_combo.setFixedWidth(70)
+        self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
+        leader_layout.addWidget(leader_lbl, 1)
+        leader_layout.addWidget(self._leader_combo)
+        param_col.addWidget(self._leader_row)
+
+        param_col.addStretch()
+
+        # 初始顯示對應的參數
+        self._update_param_visibility()
+
+        # 當任務類型切換時更新參數顯示
+        self.type_combo.currentTextChanged.connect(self._update_param_visibility)
+
+        # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ──
+        # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+        cols.addLayout(left, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(mid, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(param_col, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(right, 0)
+        cols.addWidget(self._make_sep())
+        cols.addLayout(reboot_col, 0)
+        cols.addStretch()  # 填充剩餘空間，使欄位置左
+
+        layout.addLayout(cols)
+
+    def update_drone_list(self):
+        """更新無人機列表顯示"""
+        if not self.group.selected_drone_ids:
+            self.drone_list_label.setText("尚未分配")
+            self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+        else:
+            sorted_ids = sorted(self.group.selected_drone_ids,
+                                key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+            self.drone_list_label.setText("、".join(sorted_ids))
+            self.drone_list_label.setStyleSheet(
+                f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+        self._refresh_leader_options()
+
+    def _refresh_leader_options(self):
+        """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
+        sorted_ids = sorted(self.group.selected_drone_ids,
+                            key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+        current = self.group.leader_drone_id
+        self._leader_combo.blockSignals(True)
+        self._leader_combo.clear()
+        self._leader_combo.addItems(sorted_ids)
+        if current and current in sorted_ids:
+            self._leader_combo.setCurrentText(current)
+        elif sorted_ids:
+            self._leader_combo.setCurrentText(sorted_ids[0])
+            self.group.leader_drone_id = sorted_ids[0]
+        else:
+            self.group.leader_drone_id = None
+        self._leader_combo.blockSignals(False)
+
+    def _on_leader_changed(self, drone_id):
+        self.group.leader_drone_id = drone_id if drone_id else None
+
+    def update_status(self):
+        """更新任務狀態顯示"""
+        state = self.group.state
+        if self.group.planned_waypoints is None:
+            self.status_label.setText("○ 未規劃")
+            self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+        elif state == MissionState.RUNNING:
+            self.status_label.setText("▶ 執行中")
+            self.status_label.setStyleSheet("color: #4CAF50; font-size: 11px; font-weight: bold;")
+        elif state == MissionState.PAUSED:
+            self.status_label.setText("⏸ 已暫停")
+            self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
+        else:
+            n = len(self.group.selected_drone_ids)
+            total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
+            self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
+            self.status_label.setStyleSheet(
+                f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+
+    def _on_all_select_clicked(self):
+        """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
+        self.select_all_requested.emit(self.group.group_id)
+    
+    def set_delete_enabled(self, enabled):
+        """啟用或禁用刪除群組按鈕"""
+        self.delete_group_btn.setEnabled(enabled)
+
+    def _update_param_visibility(self, _=None):
+        """根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。"""
+        mission_type = self.type_combo.currentText()
+        mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+        exec_keys = {d[0] for d in self._exec_param_defs}
+        visible_keys = mission_keys | exec_keys
+        for key, (row_w, _inp) in self._param_widgets.items():
+            row_w.setVisible(key in visible_keys)
+        self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
+
+    def get_mission_params(self):
+        """讀取當前任務類型的規劃參數，回傳 dict（不含執行參數）"""
+        mission_type = self.type_combo.currentText()
+        params = {}
+        for key, _label, default in self._param_defs.get(mission_type, []):
+            if key in self._param_widgets:
+                _row_w, inp = self._param_widgets[key]
+                try:
+                    params[key] = float(inp.text())
+                except ValueError:
+                    params[key] = float(default)
+        return params
+
+    def get_execution_params(self):
+        """讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict"""
+        params = {}
+        for key, _label, default in self._exec_param_defs:
+            if key in self._param_widgets:
+                _row_w, inp = self._param_widgets[key]
+                try:
+                    params[key] = float(inp.text())
+                except ValueError:
+                    params[key] = float(default)
+        return params
+
+    def set_param_value(self, key, value):
+        """更新指定參數欄位的文字值。"""
+        if key not in self._param_widgets:
+            return
+        _row_w, inp = self._param_widgets[key]
+        inp.setText(str(value))
+
+    def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
+        """更新中心點 / 目標點顯示"""
+        info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
+        self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
+        self.center_label.setStyleSheet(info_style)
+        self.target_label.setText(f"目標: {target_lat:.6f}°, {target_lon:.6f}°")
+        self.target_label.setStyleSheet(info_style)
+
+    def clear_mission_info(self):
+        """清除中心點 / 目標點顯示"""
+        self.center_label.setText("中心: --")
+        self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+        self.target_label.setText("目標: --")
+        self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+
+    def _on_takeoff(self):
+        try:
+            alt = float(self.alt_input.currentText())
+        except ValueError:
+            alt = 10.0
+        self.takeoff_requested.emit(self.group.group_id, alt)

src/GUI/mission_planner.py

@@ -0,0 +1,550 @@
+#!/usr/bin/env python3
+"""
+飛行任務規劃模組
+支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
+"""
+import math
+from typing import List, Tuple, Optional, Dict, Any
+from enum import Enum
+
+
+class MissionType(Enum):
+    """任務類型"""
+    M_FORMATION = "m_formation"
+    CIRCLE_FORMATION = "circle_formation"
+    LEADER_FOLLOWER = "leader_follower"
+    GRID_SWEEP = "grid_sweep"
+
+
+class CoordinateConverter:
+    """GPS 座標與 Local 座標的轉換器"""
+
+    def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
+        self.origin_lat = origin_lat
+        self.origin_lon = origin_lon
+        self.origin_alt = origin_alt
+        self.R = 6371000.0
+
+    def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
+        lat_rad = math.radians(lat)
+        lon_rad = math.radians(lon)
+        origin_lat_rad = math.radians(self.origin_lat)
+        origin_lon_rad = math.radians(self.origin_lon)
+
+        dlat = lat_rad - origin_lat_rad
+        dlon = lon_rad - origin_lon_rad
+
+        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
+        y = self.R * dlat
+        z = alt - self.origin_alt
+
+        return x, y, z
+
+    def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
+        origin_lat_rad = math.radians(self.origin_lat)
+        origin_lon_rad = math.radians(self.origin_lon)
+
+        lat_rad = origin_lat_rad + (y / self.R)
+        lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
+
+        lat = math.degrees(lat_rad)
+        lon = math.degrees(lon_rad)
+        alt = self.origin_alt + z
+
+        return lat, lon, alt
+
+
+class FormationPlanner:
+    """隊形規劃器"""
+
+    def __init__(self, spacing: float = 5.0,
+                 base_altitude: float = 10.0,
+                 altitude_diff: float = 2.0):
+        self.spacing = spacing
+        self.base_altitude = base_altitude
+        self.altitude_diff = altitude_diff
+        self.current_origin = None
+        self.converter = None
+
+    def plan_formation_mission(self,
+                               drone_gps_positions: List[Tuple[float, float, float]],
+                               target_gps: Tuple[float, float, float],
+                               mission_type: MissionType = MissionType.M_FORMATION,
+                               params: Optional[Dict[str, Any]] = None
+                               ) -> Tuple[List[List[Tuple[float, float, float]]],
+                                          Tuple[float, float, float],
+                                          List[int]]:
+        """
+        回傳 (waypoints_gps, origin, rendezvous_indices)。
+        rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
+        """
+        if len(drone_gps_positions) == 0:
+            raise ValueError("無人機位置列表不能為空")
+
+        center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
+        center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
+
+        # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
+        # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
+        self.current_origin = (center_lat, center_lon, 0.0)
+        self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
+
+        drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
+        target_local = self.converter.gps_to_local(*target_gps)
+
+        if mission_type == MissionType.M_FORMATION:
+            s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+            # 起點集合後一起出發到 stage2
+            rendezvous_indices = [0]
+
+        elif mission_type == MissionType.CIRCLE_FORMATION:
+            s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+            # 半程集合後一起進最終圓環位置
+            rendezvous_indices = [0]
+
+        elif mission_type == MissionType.LEADER_FOLLOWER:
+            params = params or {}
+            route_wps_gps = params.get('route_waypoints')
+            if route_wps_gps is None or len(route_wps_gps) < 2:
+                raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
+            route_wps_local = [
+                self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
+                for wp in route_wps_gps
+            ]
+            waypoints_local, rendezvous_indices = self._calculate_leader_follower(
+                drone_local, route_wps_local, params
+            )
+
+        elif mission_type == MissionType.GRID_SWEEP:
+            params = params or {}
+            rect_corners_gps = params.get('rect_corners')
+            if rect_corners_gps is None or len(rect_corners_gps) != 4:
+                raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
+            rect_corners_local = [
+                self.converter.gps_to_local(c[0], c[1], 0)[:2]
+                for c in rect_corners_gps
+            ]
+            waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
+                drone_local, rect_corners_local, params
+            )
+        else:
+            raise ValueError(f"不支援的任務類型: {mission_type}")
+
+        waypoints_gps = []
+        for drone_wps in waypoints_local:
+            gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
+            waypoints_gps.append(gps_wps)
+
+        return waypoints_gps, self.current_origin, rendezvous_indices
+
+    # ------------------------------------------------------------------ M-Formation
+
+    def _calculate_m_formation(self, drone_positions, target_point, params):
+        params = params or {}
+        N = len(drone_positions)
+        spacing = params.get('spacing', self.spacing)
+        base_altitude = params.get('base_altitude', self.base_altitude)
+        altitude_diff = params.get('altitude_diff', self.altitude_diff)
+
+        C_x = sum(pos[0] for pos in drone_positions) / N
+        C_y = sum(pos[1] for pos in drone_positions) / N
+
+        T_x, T_y, T_z = target_point
+        V_x, V_y = T_x - C_x, T_y - C_y
+        P_x, P_y = -V_y, V_x
+
+        length = math.sqrt(P_x ** 2 + P_y ** 2)
+        P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
+
+        projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
+                       for i, pos in enumerate(drone_positions)]
+        projections.sort()
+
+        stage1_positions = [None] * N
+        stage2_positions = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            offset = (rank - (N - 1) / 2) * spacing
+            altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
+            stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
+            stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
+
+        return stage1_positions, stage2_positions
+
+    # ------------------------------------------------------------------ Circle
+
+    def _calculate_circle_formation(self, drone_positions, target_point, params):
+        params = params or {}
+        N = len(drone_positions)
+        radius = params.get('radius', 10.0)
+        altitude = params.get('altitude', 10.0)
+        start_angle = params.get('start_angle', 0.0)
+
+        center_x, center_y, center_z = target_point
+        stage1_positions = []
+        stage2_positions = []
+        angle_step = 360.0 / N
+
+        for i in range(N):
+            angle_rad = math.radians(start_angle + angle_step * i)
+            final_x = center_x + radius * math.cos(angle_rad)
+            final_y = center_y + radius * math.sin(angle_rad)
+            final_z = altitude
+
+            current_x, current_y, _current_z = drone_positions[i]
+            # stage1 高度直接用 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):
+        """
+        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化
+
+        center_path = 使用者畫的路徑點（直線段連接，不插弧），
+        每架 drone 沿弧長 s 參數化做隊形偏移：
+            s_follower = s_leader - lon_k
+            position = lookup(s_follower) + lat_k × perp(tangent)
+        s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
+
+        Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
+        隊形 (俯視):
+            D0 (lat=-L, lon=0)   ← leader
+                D1 (lat=+L, lon=5)
+            D2 (lat=-L, lon=10)
+                D3 (lat=+L, lon=15)
+        """
+        N = len(drone_positions)
+        lateral_offset = params.get('lateral_offset', 3.0)
+        longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
+        altitude = params.get('altitude', self.base_altitude)
+
+        # Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
+        center_path, barrier_indices = self._build_center_path(route_wps_local)
+
+        s_list = [pt['s'] for pt in center_path]
+        s_max = s_list[-1]
+        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__":
+    planner = FormationPlanner()
+
+    drones = [
+        (24.123450, 120.567800, 0.0),
+        (24.123470, 120.567820, 0.0),
+        (24.123440, 120.567810, 0.0),
+        (24.123460, 120.567830, 0.0),
+    ]
+    target = (24.12400, 120.56795, 10.0)
+
+    # M-Formation
+    wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+    print("M-Formation:")
+    for i, wp_list in enumerate(wps):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
+
+    # Leader-Follower (虛擬領隊 / 弧長參數化)
+    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(f"  Drone {i}: {len(wp_list)} waypoints")
+        for j, wp in enumerate(wp_list):
+            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
+
+    # Grid Sweep
+    rect = [
+        (24.1237, 120.5679),
+        (24.1237, 120.5683),
+        (24.1240, 120.5683),
+        (24.1240, 120.5679)
+    ]
+    wps2, origin2, rv2 = planner.plan_formation_mission(
+        drones, target, MissionType.GRID_SWEEP,
+        params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
+    )
+    print(f"\nGrid Sweep:")
+    for i, wp_list in enumerate(wps2):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")

src/GUI/overview_table.py

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

src/GUI/validation/test_mission.py

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

src/GUI/validation/verify_waypoints.py

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

src/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

src/external/mavros_msgs/msg/WaypointReached.msg

@@ -0,0 +1,7 @@
+# That message represent MISSION_ITEM_REACHED
+#
+#   :wp_seq:    index number of reached waypoint
+
+std_msgs/Header header
+
+uint16 wp_seq

src/external/mavros_msgs/msg/WheelOdomStamped.msg

@@ -0,0 +1,6 @@
+# Stamped wheel odometry message
+#
+# For streaming timestamped data from FCU wheel encoders (RPM or WHEEL_DISTANCE).
+
+std_msgs/Header header
+float64[] data

src/external/mavros_msgs/package.xml

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mavros_msgs</name>
+  <version>2.9.0</version>
+  <description>
+    mavros_msgs defines messages for <a href="http://wiki.ros.org/mavros">MAVROS</a>.
+  </description>
+
+  <maintainer email="vooon341@gmail.com">Vladimir Ermakov</maintainer>
+
+  <license>GPLv3</license>
+  <license>LGPLv3</license>
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/mavros_msgs</url>
+  <url type="repository">https://github.com/mavlink/mavros.git</url>
+  <url type="bugtracker">https://github.com/mavlink/mavros/issues</url>
+
+  <author email="vooon341@gmail.com">Vladimir Ermakov</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <!-- <depend>builtin_interfaces</depend> -->
+  <depend>rcl_interfaces</depend>
+  <depend>geographic_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <!-- <depend>std_msgs</depend> -->
+  <!-- XXX needed for users of mavlink_convert.h
+  <build_export_depend>libmavconn</build_export_depend>
+  -->
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <ros1_bridge mapping_rules="mavros_msgs_mapping_rule.yaml" />
+  </export>
+</package>

src/external/mavros_msgs/srv/CommandAck.srv

@@ -0,0 +1,11 @@
+# Generic COMMAND_ACK
+
+uint16 command
+uint8 result
+uint8 progress
+uint32 result_param2
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result

src/external/mavros_msgs/srv/CommandBool.srv

@@ -0,0 +1,6 @@
+# Common type for switch commands
+
+bool value
+---
+bool success
+uint8 result

src/external/mavros_msgs/srv/CommandHome.srv

@@ -0,0 +1,10 @@
+# request set new home position
+
+bool current_gps
+float32 yaw
+float32 latitude
+float32 longitude
+float32 altitude
+---
+bool success
+uint8 result

src/external/mavros_msgs/srv/CommandInt.srv

@@ -0,0 +1,19 @@
+# Generic COMMAND_INT
+
+bool broadcast # send this command in broadcast mode
+
+uint8 frame
+uint16 command
+uint8 current
+uint8 autocontinue
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+int32 x	# latitude in deg * 1E7 or local x * 1E4 m
+int32 y	# longitude in deg * 1E7 or local y * 1E4 m
+float32 z	# altitude
+---
+bool success
+# seems that this message don't produce andy COMMAND_ACK messages
+# so no result field