Force sync local master to remote ken

主專案整合

.gitignore

@@ -8,7 +8,8 @@
 # 編譯的資料夾
 **/build/
 **/install/
-log/
+**/log/
+**/logs/
 
 # CMAKE 的衍生檔
 CMakeCache.txt
@@ -24,3 +25,4 @@ Makefile
 **/*.class
 **/*.pyc
 **/*.pyo
+**/.cursor/

.gitmodules

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

README.md

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

src/GUI/comm_panel.py

@@ -306,8 +306,7 @@ class CommPanel(QWidget):
         # 發送信號通知主窗口
         self.udp_connection_added.emit(ip, port)
         
-        # 清空輸入框
+        # 只清空 port 輸入框，保留 IP
-        self.udp_ip_input.clear()
         self.udp_port_input.clear()
     
     def _handle_add_ws(self):

src/GUI/command_sender.py

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

src/GUI/communication.py

@@ -4,10 +4,14 @@ import math
 import re
 import threading
 from threading import Lock
+from concurrent.futures import ThreadPoolExecutor
 import asyncio
 import websockets
 import json
 import socket
+import sys
+import os
+import traceback
 from pymavlink import mavutil
 from geometry_msgs.msg import Point, Vector3
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -15,6 +19,23 @@ from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
 from mavros_msgs.srv import CommandBool, CommandTOL
 
+# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
+_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _src_path not in sys.path:
+    sys.path.insert(0, _src_path)
+
+# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
+try:
+    from fc_network_apps.longCommand import CommandLongClient
+except ImportError as e:
+    import traceback
+    print(f"⚠️  警告: 無法導入 CommandLongClient")
+    print(f"   错误: {e}")
+    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+    print(f"   完整堆栈跟踪:")
+    traceback.print_exc()
+    CommandLongClient = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -27,8 +48,7 @@ class UDPMavlinkReceiver(threading.Thread):
         self.signals = signals
         self.connection_name = connection_name
         self.monitor = monitor  # 保存 monitor 引用
-        # UDP 使用原始 socket_id = 8
+        self.socket_id = monitor.get_next_socket_id() if monitor else 0
-        self.socket_id = monitor.get_or_assign_socket_id('8') if monitor else 0
         self.running = False
         self.sock = None
         
@@ -67,6 +87,10 @@ class UDPMavlinkReceiver(threading.Thread):
             drone_id = f"s{self.socket_id}_{system_id}"
             
             if msg_type == "HEARTBEAT":
+                # 先發送連接類型資訊
+                self.signals.update_signal.emit('connection_type', drone_id, {
+                    'type': 'UDP'
+                })
                 mode = mavutil.mode_string_v10(msg)
                 armed = bool(msg.base_mode & 128)
                 self.signals.update_signal.emit('state', drone_id, {
@@ -110,12 +134,15 @@ class UDPMavlinkReceiver(threading.Thread):
                 })
 
             elif msg_type == "ATTITUDE":
+                # 從 MAVLink 訊息中提取並轉為角度
                 pitch = math.degrees(msg.pitch)
+                roll = math.degrees(msg.roll)
+                yaw = math.degrees(msg.yaw)
                 self.signals.update_signal.emit('attitude', drone_id, {
                     'pitch': pitch,
-                    'roll': 0,
+                    'roll': roll,
-                    'yaw': 0,
+                    'yaw': yaw,
-                    'rates': (0, 0, 0)
+                    'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
                 })
 
             elif msg_type == "VFR_HUD":
@@ -144,8 +171,7 @@ class SerialMavlinkReceiver(threading.Thread):
         self.signals = signals
         self.connection_name = connection_name
         self.monitor = monitor  # 保存 monitor 引用
-        # Serial 使用原始 socket_id = 5
+        self.socket_id = monitor.get_next_socket_id() if monitor else 0
-        self.socket_id = monitor.get_or_assign_socket_id('5') if monitor else 0
         self.running = False
         self.mav = None
         
@@ -195,6 +221,10 @@ class SerialMavlinkReceiver(threading.Thread):
             drone_id = f"s{self.socket_id}_{system_id}"
             
             if msg_type == "HEARTBEAT":
+                # 先發送連接類型資訊
+                self.signals.update_signal.emit('connection_type', drone_id, {
+                    'type': 'Serial'
+                })
                 mode = mavutil.mode_string_v10(msg)
                 armed = bool(msg.base_mode & 128)
                 self.signals.update_signal.emit('state', drone_id, {
@@ -238,12 +268,15 @@ class SerialMavlinkReceiver(threading.Thread):
                 })
 
             elif msg_type == "ATTITUDE":
+                # 從 MAVLink 訊息中提取並轉為角度
                 pitch = math.degrees(msg.pitch)
+                roll = math.degrees(msg.roll)
+                yaw = math.degrees(msg.yaw)
                 self.signals.update_signal.emit('attitude', drone_id, {
                     'pitch': pitch,
-                    'roll': 0,
+                    'roll': roll,
-                    'yaw': 0,
+                    'yaw': yaw,
-                    'rates': (0, 0, 0)
+                    'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
                 })
 
             elif msg_type == "VFR_HUD":
@@ -336,6 +369,10 @@ class WebSocketMavlinkReceiver(threading.Thread):
 
             # 模式
             if 'mode' in data:
+                # 先發送連接類型資訊
+                self.signals.update_signal.emit('connection_type', drone_id, {
+                    'type': 'WS'
+                })
                 self.signals.update_signal.emit('state', drone_id, {
                     'mode': data['mode'],
                 })
@@ -382,8 +419,13 @@ class WebSocketMavlinkReceiver(threading.Thread):
 
 class DroneMonitor(Node):
     # Subscribe to drone ROS2 topics
+    _instance = None  # Singleton pattern to prevent duplicate nodes
+    
     def __init__(self):
-        super().__init__('drone_monitor')
+        # Use a unique node name with timestamp to avoid conflicts on restart
+        import time
+        node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
+        super().__init__(node_name)
         self.signals = DroneSignals()
         self.drone_topics = {}
         self.lock = Lock()
@@ -424,9 +466,27 @@ class DroneMonitor(Node):
         # ================================================================================
         self.serial_receivers = []
         
+        # ================================================================================
+        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # ================================================================================
+        self.command_long_client = None
+        try:
+            self.command_long_client = CommandLongClient()
+        except Exception as e:
+            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
+            self.command_long_client = None
+        # ================================================================================
+        
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
+    def get_next_socket_id(self):
+        """获取下一个可用的 socket_id（从 0 开始连续分配）"""
+        with self.socket_id_lock:
+            current_id = self.socket_id_counter
+            self.socket_id_counter += 1
+            return current_id
+    
     def get_or_assign_socket_id(self, original_socket_id):
         """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配）
         同一個原始 socket_id 會得到同一個分配的 ID
@@ -437,7 +497,6 @@ class DroneMonitor(Node):
             if original_socket_id not in self.socket_id_mapping:
                 # 分配新的 socket_id
                 self.socket_id_mapping[original_socket_id] = self.socket_id_counter
-                print(f"🆕 Socket ID 映射: 原始 {original_socket_id} -> 分配 {self.socket_id_counter}")
                 self.socket_id_counter += 1
             
             return self.socket_id_mapping[original_socket_id]
@@ -455,11 +514,11 @@ class DroneMonitor(Node):
                     self.drone_topics.setdefault(sys_id, set()).add(topic_type)
         
         for sys_id in found_drones:
-            # 為每個 sys_id 分配 socket_id（如果還沒有分配）
+            # 为每个 sys_id 分配 socket_id（如果还没有分配）
-            # 注意：如果後續 summary 提供了 socket_id，會使用 summary 的映射覆蓋
+            # 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖
             if sys_id not in self.sys_to_socket_id:
-                # 暫時所有 ROS2 topic 共享同一個原始 socket_id，等 summary 提供實際的 socket_id
+                # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
-                self.sys_to_socket_id[sys_id] = self.get_or_assign_socket_id('0')
+                self.sys_to_socket_id[sys_id] = 0
             
             if not hasattr(self, f'drone_{sys_id}_subs'):
                 self.setup_drone(sys_id)
@@ -514,44 +573,145 @@ class DroneMonitor(Node):
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
-    async def arm_drone(self, drone_id, arm):
+    # ================================================================================
-        if drone_id not in self.arm_clients:
+    # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
-            return False
+    # ================================================================================
-        
+    MODE_MAPPING = {
-        client = self.arm_clients[drone_id]
+        "STABILIZE": 0,
-        if not client.wait_for_service(timeout_sec=1.0):
+        "ACRO": 1,
-            return False
+        "ALT_HOLD": 2,
-            
+        "AUTO": 3,
-        request = CommandBool.Request()
+        "GUIDED": 4,
-        request.value = arm
+        "LOITER": 5,
-        
+        "RTL": 6,
-        future = client.call_async(request)
+        "CIRCLE": 7,
+        "POSITION": 8,
+        "LAND": 9,
+        "OF_LOITER": 10,
+        "DRIFT": 11,
+        "SPORT": 13,
+        "FLIP": 14,
+        "AUTOTUNE": 15,
+        "POSHOLD": 16,
+        "BRAKE": 17,
+        "THROW": 18,
+        "AVOID_ADSB": 19,
+        "GUIDED_NOGPS": 20,
+        "SMART_RTL": 21,
+    }
+    # ================================================================================
+
+    async def set_mode(self, drone_id, mode_name):
+        """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
         try:
-            response = await future
+            # 解析 drone_id 提取 sysid
-            return response.success
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            # 獲取模式對應的 custom_mode 值
+            custom_mode = self.MODE_MAPPING.get(mode_name)
+            if custom_mode is None:
+                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+                return False
+            
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+            
+            if not self.command_long_client:
+                print(f"❌ [SET_MODE] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.change_mode_async(
+                target_sysid=sysid,
+                custom_mode=float(custom_mode),
+                target_compid=0,
+                base_mode=1.0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [SET_MODE] 模式切換成功")
+                return True
+            else:
+                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Arm service call failed: {e}')
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
-    async def takeoff_drone(self, drone_id, altitude=10.0):
+    async def arm_drone(self, drone_id, arm):
-        if drone_id not in self.takeoff_clients:
+        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
-            return False
-            
-        client = self.takeoff_clients[drone_id]
-        if not client.wait_for_service(timeout_sec=1.0):
-            return False
-            
-        request = CommandTOL.Request()
-        request.altitude = altitude
-        request.min_pitch = 0.0
-        request.yaw = 0.0
-        
-        future = client.call_async(request)
         try:
-            response = await future
+            # 解析 drone_id 提取 sysid
-            return response.success
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            action_name = "解鎖" if arm else "上鎖"
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
+            
+            if not self.command_long_client:
+                print(f"❌ [ARM] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.arm_disarm_async(
+                target_sysid=sysid,
+                arm=arm,
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [ARM] {action_name}成功")
+                return True
+            else:
+                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Takeoff service call failed: {e}')
+            print(f"❌ [ARM] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude):
+        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+            
+            if not self.command_long_client:
+                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.takeoff_async(
+                target_sysid=sysid,
+                altitude_m=float(altitude),
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [TAKEOFF] 起飛成功")
+                return True
+            else:
+                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                return False
+        except Exception as e:
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
     def send_setpoint(self, drone_id, x, y, z):
@@ -646,6 +806,11 @@ class DroneMonitor(Node):
                 actual_drone_id = f's{assigned_socket_id}_{sys_num}'
                 self.sys_to_actual_id[sys_id] = actual_drone_id
             
+            # 先發送連接類型資訊
+            self.signals.update_signal.emit('connection_type', actual_drone_id, {
+                'type': 'ROS2'
+            })
+            
             self.latest_data[(actual_drone_id, 'state')] = {
                 'mode': mode,
                 'armed': data.get('armed', False),

src/GUI/drone_panel.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python3
-from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, 
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
-                            QPushButton, QSizePolicy, QCheckBox)
 from PyQt6.QtCore import pyqtSignal
+from PyQt6.QtGui import QPainter, QPen, QColor, QFont, QPolygonF
+from PyQt6.QtCore import QPointF, Qt
+import math
 
 class DronePanel(QWidget):
     """單個無人機面板類別"""
@@ -16,7 +18,19 @@ class DronePanel(QWidget):
     def __init__(self, drone_id, parent=None):
         super().__init__(parent)
         self.drone_id = drone_id
-        self.display_id = 's' + drone_id.split('_')[1]
+        
+        # 提取資訊 (格式: s{socket_seq}_{system_id}, 如 s0_11, s1_12)
+        parts = drone_id.split('_')
+        if len(parts) >= 2:
+            self.socket_seq = parts[0][1:]  # socket 序號 (移除 's' 前綴)
+            self.system_id = parts[1]       # system ID
+            self.display_id = f"ID:{self.system_id}"  # 顯示為 ID:11, ID:12
+        else:
+            self.socket_seq = "?"
+            self.system_id = "?"
+            self.display_id = drone_id
+        
+        self.attitude_indicator = None
         self._init_ui()
     
     def _init_ui(self):
@@ -43,12 +57,12 @@ class DronePanel(QWidget):
         # 左側資訊區域
         info_widget = self._create_info_section()
         
-        # 右側控制按鈕區域
+        # 右側態度指示器
-        control_widget = self._create_control_section()
+        attitude_widget = self._create_attitude_indicator()
         
-        # 將 info 和 control 加入內容容器
+        # 將 info 和 attitude 加入內容容器
-        content_layout.addWidget(info_widget)
+        content_layout.addWidget(info_widget, 1)
-        content_layout.addWidget(control_widget)
+        content_layout.addWidget(attitude_widget, 0)
         
         # 將內容容器加入主佈局
         main_layout.addWidget(content_widget)
@@ -68,7 +82,22 @@ class DronePanel(QWidget):
         # 勾選框
         self.checkbox = QCheckBox()
         self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
-        self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }")
+        self.checkbox.setStyleSheet("""
+            QCheckBox { 
+                color: #DDD;
+            }
+            QCheckBox::indicator {
+                width: 16px;
+                height: 16px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }
+            QCheckBox::indicator:checked {
+                background-color: #7FFFD4;
+                border: 2px solid #888888;
+            }
+        """)
         self.checkbox.stateChanged.connect(
             lambda state: self.selection_changed.emit(self.drone_id, state)
         )
@@ -92,13 +121,13 @@ class DronePanel(QWidget):
         status_row = self._create_status_row()
         info_layout.addWidget(status_row)
         
-        # 第二行：電池
+        # 第二行：電池（拆成百分比與電壓兩欄）
         battery_row = self._create_battery_row()
         info_layout.addWidget(battery_row)
         
-        # 第三行：位置 + 高度
+        # 第三行：高度
-        position_row = self._create_position_row()
+        altitude_row = self._create_altitude_row()
-        info_layout.addWidget(position_row)
+        info_layout.addWidget(altitude_row)
         
         # 第四行：航向 + 速度
         nav_row = self._create_nav_row()
@@ -106,6 +135,12 @@ class DronePanel(QWidget):
         
         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()
@@ -130,132 +165,120 @@ class DronePanel(QWidget):
         
         return status_row
     
+    def _create_connection_row(self):
+        """創建連接資訊行 (Socket Seq + 連接方式)"""
+        connection_row = QWidget()
+        connection_layout = QHBoxLayout(connection_row)
+        connection_layout.setContentsMargins(0, 0, 0, 0)
+        
+        connection_title = QLabel("Socket：")
+        connection_title.setStyleSheet("color: #888; min-width: 50px;")
+        
+        # 根據解析的 drone_id 資訊設定初始值
+        self.socket_seq_label = QLabel(self.socket_seq)
+        self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq")
+        self.socket_seq_label.setStyleSheet("color: #DDD;")
+        
+        connection_sep = QLabel(" - ")
+        connection_sep.setStyleSheet("color: #DDD;")
+        
+        # 設定連接方式顯示
+        connection_type_map = {
+            'r': 'ROS2',
+            'u': 'UDP',
+            's': 'Serial',
+            'w': 'WS'
+        }
+        connection_type = connection_type_map.get(self.type_prefix, 'Unknown')
+        
+        self.connection_type_label = QLabel(connection_type)
+        self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
+        self.connection_type_label.setStyleSheet("color: #DDD;")
+        
+        connection_layout.addWidget(connection_title)
+        connection_layout.addWidget(self.socket_seq_label)
+        connection_layout.addWidget(connection_sep)
+        connection_layout.addWidget(self.connection_type_label)
+        connection_layout.addStretch()
+        
+        return connection_row
+    
     def _create_battery_row(self):
         """創建電池行"""
         battery_row = QWidget()
         battery_layout = QHBoxLayout(battery_row)
         battery_layout.setContentsMargins(0, 0, 0, 0)
-        
+        # 顯示百分比
         battery_title = QLabel("電池：")
         battery_title.setStyleSheet("color: #888; min-width: 50px;")
-        
+
-        self.battery_label = QLabel("--")
+        self.battery_pct_label = QLabel("--")
-        self.battery_label.setObjectName(f"{self.drone_id}_battery")
+        self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
-        self.battery_label.setStyleSheet("color: #DDD;")
+        self.battery_pct_label.setStyleSheet("color: #DDD;")
-        
+
+        # 分隔符
+        separator1 = QLabel(" - ")
+        separator1.setStyleSheet("color: #DDD;")
+
+        # 顯示電壓
+        self.battery_vol_label = QLabel("--")
+        self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
+        self.battery_vol_label.setStyleSheet("color: #DDD;")
+
+        # 分隔符
+        separator2 = QLabel(" - ")
+        separator2.setStyleSheet("color: #DDD;")
+
+        # 顯示電池節數 (S count)
+        self.battery_cells_label = QLabel("--")
+        self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
+        self.battery_cells_label.setStyleSheet("color: #DDD;")
+
         battery_layout.addWidget(battery_title)
-        battery_layout.addWidget(self.battery_label)
+        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_position_row(self):
+    def _create_altitude_row(self):
-        """創建位置行"""
+        """創建高度和速度行"""
-        position_row = QWidget()
+        altitude_row = QWidget()
-        position_layout = QHBoxLayout(position_row)
+        altitude_layout = QHBoxLayout(altitude_row)
-        position_layout.setContentsMargins(0, 0, 0, 0)
+        altitude_layout.setContentsMargins(0, 0, 0, 0)
-        
-        position_title = QLabel("位置：")
-        position_title.setStyleSheet("color: #888; min-width: 50px;")
-        
-        self.local_label = QLabel("--")
-        self.local_label.setObjectName(f"{self.drone_id}_local")
-        self.local_label.setStyleSheet("color: #DDD;")
         
         altitude_title = QLabel("高度：")
-        altitude_title.setStyleSheet("color: #888; margin-left: 10px;")
+        altitude_title.setStyleSheet("color: #888; min-width: 50px;")
         
         self.altitude_label = QLabel("--")
         self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
         self.altitude_label.setStyleSheet("color: #DDD;")
         
-        position_layout.addWidget(position_title)
-        position_layout.addWidget(self.local_label)
-        position_layout.addWidget(altitude_title)
-        position_layout.addWidget(self.altitude_label)
-        position_layout.addStretch()
-        
-        return position_row
-    
-    def _create_nav_row(self):
-        """創建導航行"""
-        nav_row = QWidget()
-        nav_layout = QHBoxLayout(nav_row)
-        nav_layout.setContentsMargins(0, 0, 0, 0)
-        
-        heading_title = QLabel("航向：")
-        heading_title.setStyleSheet("color: #888; min-width: 50px;")
-        
-        self.heading_label = QLabel("--")
-        self.heading_label.setObjectName(f"{self.drone_id}_heading")
-        self.heading_label.setStyleSheet("color: #DDD;")
-        
         speed_title = QLabel("速度：")
         speed_title.setStyleSheet("color: #888; margin-left: 10px;")
         
-        self.groundspeed_label = QLabel("--")
+        self.speed_label = QLabel("--")
-        self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed")
+        self.speed_label.setObjectName(f"{self.drone_id}_speed")
-        self.groundspeed_label.setStyleSheet("color: #DDD;")
+        self.speed_label.setStyleSheet("color: #DDD;")
         
-        nav_layout.addWidget(heading_title)
+        altitude_layout.addWidget(altitude_title)
-        nav_layout.addWidget(self.heading_label)
+        altitude_layout.addWidget(self.altitude_label)
-        nav_layout.addWidget(speed_title)
+        altitude_layout.addWidget(speed_title)
-        nav_layout.addWidget(self.groundspeed_label)
+        altitude_layout.addWidget(self.speed_label)
-        nav_layout.addStretch()
+        altitude_layout.addStretch()
         
-        return nav_row
+        return altitude_row
     
-    def _create_control_section(self):
+    def _create_position_row(self):
-        """創建控制按鈕區域"""
+        """位置行已移除（位置座標不再顯示於面板）。"""
-        control_widget = QWidget()
+        return QWidget()
-        control_layout = QVBoxLayout(control_widget)
+    
-        control_layout.setContentsMargins(0, 0, 0, 0)
+    def _create_nav_row(self):
-        control_layout.setSpacing(6)
+        """創建導航行（已移除，不再顯示）"""
-        
+        return QWidget()
-        control_widget.setFixedWidth(80)
-        
-        btn_style = """
-	        QPushButton {
-                background-color: #444;
-                color: #DDD;
-                border: none;
-                border-radius: 4px;
-                font-size: 11px;
-            }
-            QPushButton:hover {
-                background-color: #555;
-            }
-        """
-        # 模式切換按鈕
-        mode_btn = QPushButton("切換模式")
-        mode_btn.setStyleSheet(btn_style)
-        mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
-        mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id))
-        
-        # 解鎖按鈕
-        arm_btn = QPushButton("解鎖")
-        arm_btn.setStyleSheet(btn_style)
-        arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
-        arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id))
-        
-        # 起飛按鈕
-        takeoff_btn = QPushButton("起飛")
-        takeoff_btn.setStyleSheet(btn_style)
-        takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
-        takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id))
-        
-        # Setpoint 按鈕
-        setpoint_btn = QPushButton("Setpoint")
-        setpoint_btn.setStyleSheet(btn_style)
-        setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
-        setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id))
-        
-        control_layout.addWidget(mode_btn)
-        control_layout.addWidget(arm_btn)
-        control_layout.addWidget(takeoff_btn)
-        control_layout.addWidget(setpoint_btn)
-        
-        return control_widget
     
     def update_field(self, field, text, color=None):
         """更新指定欄位的值"""
@@ -265,6 +288,27 @@ class DronePanel(QWidget):
             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
@@ -281,10 +325,11 @@ class SocketGroupPanel(QWidget):
     # 定義信號
     group_selection_changed = pyqtSignal(str, int)  # socket_id, state
     
-    def __init__(self, socket_id, color='#AAAAAA', parent=None):
+    def __init__(self, socket_id, color='#AAAAAA', socket_type=None, parent=None):
         super().__init__(parent)
         self.socket_id = socket_id
         self.color = color
+        self.socket_type = socket_type
         self._init_ui()
     
     def _init_ui(self):
@@ -330,8 +375,12 @@ class SocketGroupPanel(QWidget):
         )
         
         # Socket 分組標題
-        title_label = QLabel(f"Socket {self.socket_id}")
+        if self.socket_type:
-        title_label.setStyleSheet(f"""
+            title_text = f"{self.socket_type} {self.socket_id}"
+        else:
+            title_text = f"Socket {self.socket_id}"
+        self.title_label = QLabel(title_text)
+        self.title_label.setStyleSheet(f"""
             font-weight: bold; 
             font-size: 16px; 
             color: {self.color};
@@ -341,7 +390,7 @@ class SocketGroupPanel(QWidget):
         """)
         
         title_layout.addWidget(self.group_checkbox)
-        title_layout.addWidget(title_label)
+        title_layout.addWidget(self.title_label)
         title_layout.addStretch()
         
         layout.addWidget(title_row)
@@ -373,6 +422,165 @@ class SocketGroupPanel(QWidget):
         """設置分組勾選狀態"""
         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)
+        self.group_checkbox.setCheckState(state)
+
+
+class AttitudeIndicator(QWidget):
+    """
+    人工地平儀 (ADI) — 仿 Mission Planner 風格
+    上半部顯示 roll/pitch 人工地平儀，下方細條顯示航向
+    """
+
+    def __init__(self, drone_id, parent=None):
+        super().__init__(parent)
+        self.drone_id = drone_id
+        self.heading = 0.0   # 航向 yaw (0–360)
+        self.roll    = 0.0   # 滾轉 (deg, left- negative)
+        self.pitch   = 0.0   # 俯仰 (deg, nose-up positive)
+        self.setStyleSheet("background-color: transparent;")
+
+    def update_attitude(self, heading, roll, pitch):
+        self.heading = heading % 360
+        self.roll    = roll
+        self.pitch   = pitch
+        self.update()
+
+    # ------------------------------------------------------------------ helpers
+    def _adi_rect(self):
+        """Returns the square rect used for the ADI ball."""
+        w, h = self.width(), self.height()
+        side  = min(w, h - 14)   # leave 14 px at bottom for heading strip
+        x = (w - side) / 2
+        y = 0
+        return x, y, side, side
+
+    # ------------------------------------------------------------------ paint
+    def paintEvent(self, event):
+        p = QPainter(self)
+        p.setRenderHint(QPainter.RenderHint.Antialiasing)
+        self._draw_adi(p)
+        self._draw_heading_strip(p)
+
+    # ---- artificial horizon ------------------------------------------------
+    def _draw_adi(self, p):
+        from PyQt6.QtGui import QPainterPath
+        x0, y0, side, _ = self._adi_rect()
+        cx = x0 + side / 2
+        cy = y0 + side / 2
+        r  = side / 2 - 1
+
+        # clip to circle
+        clip_path = QPainterPath()
+        clip_path.addEllipse(QPointF(cx, cy), r, r)
+        p.setClipPath(clip_path)
+
+        # pixels-per-degree for pitch (10 deg ≈ side/5)
+        ppd = side / 50.0
+
+        # ---- rotate + translate canvas for roll & pitch
+        p.save()
+        p.translate(cx, cy)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
+        pitch_offset = self.pitch * ppd
+
+        # sky (above horizon)
+        sky_color = QColor(30, 100, 180)
+        p.fillRect(int(-r*2), int(-r*2 + pitch_offset), int(r*4), int(r*4), sky_color)
+
+        # ground (below horizon)
+        ground_color = QColor(140, 90, 40)
+        p.fillRect(int(-r*2), int(pitch_offset), int(r*4), int(r*4), ground_color)
+
+        # horizon line
+        p.setPen(QPen(QColor("#FFFFFF"), 2))
+        p.drawLine(int(-r), int(pitch_offset), int(r), int(pitch_offset))
+
+        # pitch ladder (every 10°, ±30°)
+        p.setPen(QPen(QColor(255, 255, 255, 180), 1))
+        p.setFont(QFont("Arial", 6))
+        for deg in range(-30, 31, 10):
+            if deg == 0:
+                continue
+            yy = int(pitch_offset - deg * ppd)
+            half = int(r * (0.35 if deg % 20 == 0 else 0.22))
+            p.drawLine(-half, yy, half, yy)
+
+        p.restore()
+        p.setClipping(False)
+
+        # ---- roll arc & tick marks (outside clip, fixed frame) ----
+        p.save()
+        p.translate(cx, cy)
+
+        arc_r = r - 2
+        p.setPen(QPen(QColor("#FFFFFF"), 1))
+        # draw arc from -60° to +60°  (Qt arc: 0=3o'clock, CCW, 16ths of deg)
+        p.drawArc(int(-arc_r), int(-arc_r), int(2*arc_r), int(2*arc_r),
+                  (90 - 60) * 16, 120 * 16)
+
+        # tick marks at 0, ±10, ±20, ±30, ±45, ±60
+        for deg in [0, 10, 20, 30, 45, 60, -10, -20, -30, -45, -60]:
+            rad = math.radians(deg - 90)
+            tick = 6 if deg % 30 == 0 else 4
+            x1 = arc_r * math.cos(rad)
+            y1 = arc_r * math.sin(rad)
+            x2 = (arc_r - tick) * math.cos(rad)
+            y2 = (arc_r - tick) * math.sin(rad)
+            p.drawLine(QPointF(x1, y1), QPointF(x2, y2))
+
+        # roll pointer triangle (rotates with roll)
+        p.rotate(-self.roll)  # ✅ 負值：NED 坐標系轉換
+        ptr_r = arc_r - 1
+        tri = QPolygonF([
+            QPointF(0, -ptr_r),
+            QPointF(-5, -ptr_r + 9),
+            QPointF(5,  -ptr_r + 9),
+        ])
+        p.setBrush(QColor("#FFFFFF"))
+        p.setPen(Qt.PenStyle.NoPen)
+        p.drawPolygon(tri)
+        p.restore()
+
+        # ---- fixed aircraft symbol ----
+        p.save()
+        p.translate(cx, cy)
+        p.setPen(QPen(QColor("#FFD700"), 2))
+        # left wing
+        p.drawLine(int(-r*0.5), 0, int(-r*0.15), 0)
+        p.drawLine(int(-r*0.15), 0, int(-r*0.15), int(r*0.12))
+        # right wing
+        p.drawLine(int(r*0.15), 0, int(r*0.5), 0)
+        p.drawLine(int(r*0.15), 0, int(r*0.15), int(r*0.12))
+        # centre dot
+        p.setBrush(QColor("#FFD700"))
+        p.drawEllipse(QPointF(0, 0), 2.5, 2.5)
+        p.restore()
+
+        # ---- outer ring ----
+        p.setPen(QPen(QColor("#888888"), 1))
+        p.setBrush(Qt.BrushStyle.NoBrush)
+        p.drawEllipse(QPointF(cx, cy), r, r)
+
+    # ---- heading strip at bottom ------------------------------------------
+    def _draw_heading_strip(self, p):
+        w         = self.width()
+        x0, y0, side, _ = self._adi_rect()
+        strip_y   = y0 + side
+        strip_h   = self.height() - strip_y
+        if strip_h < 4:
+            return
+
+        # background
+        p.fillRect(0, int(strip_y), w, strip_h, QColor(30, 30, 30))
+
+        # heading text centred (bigger)
+        p.setPen(QPen(QColor("#FFFFFF")))
+        p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
+        hdg_str = f"{int(self.heading)}°"
+        p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)

src/GUI/mission_executor.py

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

src/GUI/mission_group.py

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

src/GUI/mission_planner.py

@@ -1,323 +1,538 @@
 #!/usr/bin/env python3
 """
 飛行任務規劃模組
-負責將 GPS 點擊座標轉換為隊形飛行任務
+支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep
 """
 import math
-from typing import List, Tuple, Optional
+from typing import List, Tuple, Optional, Dict, Any
+from enum import Enum
+
+
+class MissionType(Enum):
+    """任務類型"""
+    M_FORMATION = "m_formation"
+    CIRCLE_FORMATION = "circle_formation"
+    LEADER_FOLLOWER = "leader_follower"
+    GRID_SWEEP = "grid_sweep"
 
 
 class CoordinateConverter:
     """GPS 座標與 Local 座標的轉換器"""
-    
+
     def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
-        """
-        初始化座標轉換器
-        
-        Args:
-            origin_lat: 參考原點緯度
-            origin_lon: 參考原點經度
-            origin_alt: 參考原點高度（公尺，相對於海平面）
-        """
         self.origin_lat = origin_lat
         self.origin_lon = origin_lon
         self.origin_alt = origin_alt
-        self.R = 6371000.0  # 地球半徑（公尺）
+        self.R = 6371000.0
-    
+
     def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
-        """
-        GPS 座標轉換為 Local 座標（ENU 系統：East-North-Up）
-        
-        Args:
-            lat: 緯度
-            lon: 經度
-            alt: 高度（公尺，相對於海平面）
-        
-        Returns:
-            (x, y, z): Local 座標（公尺）
-                x: East（東方）
-                y: North（北方）
-                z: Up（向上）
-        """
         lat_rad = math.radians(lat)
         lon_rad = math.radians(lon)
         origin_lat_rad = math.radians(self.origin_lat)
         origin_lon_rad = math.radians(self.origin_lon)
-        
+
         dlat = lat_rad - origin_lat_rad
         dlon = lon_rad - origin_lon_rad
-        
+
-        # 使用 Equirectangular projection（適用於小範圍 < 10 km）
+        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
-        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)  # East
+        y = self.R * dlat
-        y = self.R * dlat  # North
+        z = alt - self.origin_alt
-        z = alt - self.origin_alt  # Up
+
-        
         return x, y, z
-    
+
     def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
-        """
-        Local 座標轉換為 GPS 座標
-        
-        Args:
-            x: East（東方，公尺）
-            y: North（北方，公尺）
-            z: Up（向上，公尺）
-        
-        Returns:
-            (lat, lon, alt): GPS 座標
-        """
         origin_lat_rad = math.radians(self.origin_lat)
         origin_lon_rad = math.radians(self.origin_lon)
-        
+
         lat_rad = origin_lat_rad + (y / self.R)
         lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
-        
+
         lat = math.degrees(lat_rad)
         lon = math.degrees(lon_rad)
         alt = self.origin_alt + z
-        
+
         return lat, lon, alt
 
 
 class FormationPlanner:
     """隊形規劃器"""
-    
+
-    def __init__(self, spacing: float = 5.0, 
+    def __init__(self, spacing: float = 5.0,
-                 base_altitude: float = 10.0, 
+                 base_altitude: float = 10.0,
                  altitude_diff: float = 2.0):
-        """
-        初始化隊形規劃器
-        
-        Args:
-            spacing: 無人機間距（公尺）
-            base_altitude: 基準飛行高度（公尺，相對於參考原點）
-            altitude_diff: M 字形的高低差（公尺）
-        """
         self.spacing = spacing
         self.base_altitude = base_altitude
         self.altitude_diff = altitude_diff
         self.current_origin = None
         self.converter = None
-    
+
-    def plan_formation_mission(self, 
+    def plan_formation_mission(self,
-                              drone_gps_positions: List[Tuple[float, float, float]], 
+                               drone_gps_positions: List[Tuple[float, float, float]],
-                              target_gps: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], 
+                               target_gps: Tuple[float, float, float],
-                                                                                List[Tuple[float, float, float]]]:
+                               mission_type: MissionType = MissionType.M_FORMATION,
-        """
+                               params: Optional[Dict[str, Any]] = None
-        規劃兩階段隊形任務
+                               ) -> Tuple[List[List[Tuple[float, float, float]]],
-        
+                                          Tuple[float, float, float]]:
-        Args:
-            drone_gps_positions: 當前無人機 GPS 位置列表 [(lat, lon, alt), ...]
-            target_gps: 目標點 GPS 座標 (lat, lon, alt)
-        
-        Returns:
-            stage1_gps: 階段 1（集合點）的 GPS 航點列表
-            stage2_gps: 階段 2（目標點）的 GPS 航點列表
-            origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt)
-            origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt)
-        """
         if len(drone_gps_positions) == 0:
             raise ValueError("無人機位置列表不能為空")
-        
+
-        # ===== 步驟 1: 計算中央點並設為參考原點 =====
         center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
         center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
         center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
-        
+
         self.current_origin = (center_lat, center_lon, center_alt)
         self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
-        
+
-        print(f"📍 參考原點: ({center_lat:.6f}, {center_lon:.6f}, {center_alt:.1f}m)")
+        drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
-        
+        target_local = self.converter.gps_to_local(*target_gps)
-        # ===== 步驟 2: 轉換到 Local 座標系 =====
+
-        drone_local_positions = []
+        if mission_type == MissionType.M_FORMATION:
-        for lat, lon, alt in drone_gps_positions:
+            s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
-            x, y, z = self.converter.gps_to_local(lat, lon, alt)
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
-            drone_local_positions.append((x, y, z))
+
-        
+        elif mission_type == MissionType.CIRCLE_FORMATION:
-        target_local = self.converter.gps_to_local(
+            s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
-            target_gps[0], target_gps[1], target_gps[2]
+            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
-        )
+
-        
+        elif mission_type == MissionType.LEADER_FOLLOWER:
-        # ===== 步驟 3: 在 Local 座標系中計算隊形 =====
+            params = params or {}
-        stage1_local, stage2_local = self._calculate_formation_local(
+            route_wps_gps = params.get('route_waypoints')
-            drone_local_positions,
+            if route_wps_gps is None or len(route_wps_gps) < 2:
-            target_local
+                raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
-        )
+            route_wps_local = [
-        
+                self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
-        # ===== 步驟 4: 轉回 GPS 座標 =====
+                for wp in route_wps_gps
-        stage1_gps = []
+            ]
-        for x, y, z in stage1_local:
+            waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params)
-            lat, lon, alt = self.converter.local_to_gps(x, y, z)
+
-            stage1_gps.append((lat, lon, alt))
+        elif mission_type == MissionType.GRID_SWEEP:
-        
+            params = params or {}
-        stage2_gps = []
+            rect_corners_gps = params.get('rect_corners')
-        for x, y, z in stage2_local:
+            if rect_corners_gps is None or len(rect_corners_gps) != 4:
-            lat, lon, alt = self.converter.local_to_gps(x, y, z)
+                raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
-            stage2_gps.append((lat, lon, alt))
+            rect_corners_local = [
-        
+                self.converter.gps_to_local(c[0], c[1], 0)[:2]
-        print(f"✅ 任務規劃完成: {len(stage1_gps)} 台無人機，2 階段飛行")
+                for c in rect_corners_gps
-        
+            ]
-        # ================================================================================
+            waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params)
-        # 【修改】回傳中心點座標供地圖視覺化使用
+        else:
-        # ================================================================================
+            raise ValueError(f"不支援的任務類型: {mission_type}")
-        return stage1_gps, stage2_gps, self.current_origin
+
-        # ================================================================================
+        waypoints_gps = []
-    
+        for drone_wps in waypoints_local:
-    def _calculate_formation_local(self, 
+            gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
-                                   drone_positions: List[Tuple[float, float, float]], 
+            waypoints_gps.append(gps_wps)
-                                   target_point: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], 
+
-                                                                                       List[Tuple[float, float, float]]]:
+        return waypoints_gps, self.current_origin
-        """
+
-        在 Local 座標系中計算隊形
+    # ------------------------------------------------------------------ M-Formation
-        
+
-        Args:
+    def _calculate_m_formation(self, drone_positions, target_point, params):
-            drone_positions: Local 座標的無人機位置 [(x, y, z), ...]
+        params = params or {}
-            target_point: Local 座標的目標點 (x, y, z)
-        
-        Returns:
-            stage1_positions: 階段 1（中央點分佈）的 Local 座標
-            stage2_positions: 階段 2（目標點分佈）的 Local 座標
-        """
         N = len(drone_positions)
-        
+        spacing = params.get('spacing', self.spacing)
-        # ===== 步驟 1: 計算中央點（在 Local 座標系中應該接近原點）=====
+        base_altitude = params.get('base_altitude', self.base_altitude)
+        altitude_diff = params.get('altitude_diff', self.altitude_diff)
+
         C_x = sum(pos[0] for pos in drone_positions) / N
         C_y = sum(pos[1] for pos in drone_positions) / N
-        C_z = sum(pos[2] for pos in drone_positions) / N
+
-        
-        print(f"   中央點 Local: ({C_x:.2f}, {C_y:.2f}, {C_z:.2f})")
-        
-        # ===== 步驟 2: 計算方向向量（中央點 → 目標點）=====
         T_x, T_y, T_z = target_point
-        V_x = T_x - C_x
+        V_x, V_y = T_x - C_x, T_y - C_y
-        V_y = T_y - C_y
+        P_x, P_y = -V_y, V_x
-        
+
-        print(f"   方向向量: ({V_x:.2f}, {V_y:.2f})")
+        length = math.sqrt(P_x ** 2 + P_y ** 2)
-        
+        P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
-        # ===== 步驟 3: 計算垂直向量（逆時針旋轉 90°）=====
+
-        P_x = -V_y
+        projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
-        P_y = V_x
+                       for i, pos in enumerate(drone_positions)]
-        
-        # 單位化垂直向量
-        length = math.sqrt(P_x**2 + P_y**2)
-        if length < 0.01:  # 避免除以零
-            # 如果目標點與中央點重合，使用默認方向（向東）
-            P_x_unit, P_y_unit = 1.0, 0.0
-        else:
-            P_x_unit = P_x / length
-            P_y_unit = P_y / length
-        
-        print(f"   垂直單位向量: ({P_x_unit:.3f}, {P_y_unit:.3f})")
-        
-        # ===== 步驟 4: 計算無人機在垂直方向的投影並排序 =====
-        projections = []
-        for i, pos in enumerate(drone_positions):
-            relative_x = pos[0] - C_x
-            relative_y = pos[1] - C_y
-            projection = relative_x * P_x_unit + relative_y * P_y_unit
-            projections.append((projection, i))
-        
-        # 按投影值排序（保持左右順序）
         projections.sort()
-        sorted_indices = [idx for _, idx in projections]
+
-        
+        stage1_positions = [None] * N
-        print(f"   排序後的無人機索引: {sorted_indices}")
-        
-        # ===== 步驟 5: 分佈無人機位置 =====
-        stage1_positions = [None] * N  # 預分配列表
         stage2_positions = [None] * N
-        
+
-        for rank, original_idx in enumerate(sorted_indices):
+        for rank, (_, original_idx) in enumerate(projections):
-            # 計算相對中心的水平偏移
+            offset = (rank - (N - 1) / 2) * spacing
-            offset = (rank - (N - 1) / 2) * self.spacing
+            altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
-            
+            stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
-            # 計算 M 字形高度（交替高低）
+            stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
-            if rank % 2 == 0:
+
-                altitude = self.base_altitude + self.altitude_diff  # 高
-            else:
-                altitude = self.base_altitude - self.altitude_diff  # 低
-            
-            # 階段 1：在中央點附近分佈
-            stage1_x = C_x + P_x_unit * offset
-            stage1_y = C_y + P_y_unit * offset
-            stage1_z = altitude
-            
-            # 階段 2：在目標點附近分佈（保持相同的相對位置）
-            stage2_x = T_x + P_x_unit * offset
-            stage2_y = T_y + P_y_unit * offset
-            stage2_z = altitude
-            
-            # 按照原始索引存儲（保持無人機 ID 順序）
-            stage1_positions[original_idx] = (stage1_x, stage1_y, stage1_z)
-            stage2_positions[original_idx] = (stage2_x, stage2_y, stage2_z)
-        
         return stage1_positions, stage2_positions
-    
+
-    def update_parameters(self, spacing: Optional[float] = None, 
+    # ------------------------------------------------------------------ Circle
-                         base_altitude: Optional[float] = None, 
+
-                         altitude_diff: Optional[float] = None):
+    def _calculate_circle_formation(self, drone_positions, target_point, params):
+        params = params or {}
+        N = len(drone_positions)
+        radius = params.get('radius', 10.0)
+        altitude = params.get('altitude', 10.0)
+        start_angle = params.get('start_angle', 0.0)
+
+        center_x, center_y, center_z = target_point
+        stage1_positions = []
+        stage2_positions = []
+        angle_step = 360.0 / N
+
+        for i in range(N):
+            angle_rad = math.radians(start_angle + angle_step * i)
+            final_x = center_x + radius * math.cos(angle_rad)
+            final_y = center_y + radius * math.sin(angle_rad)
+            final_z = altitude
+
+            current_x, current_y, current_z = drone_positions[i]
+            stage1_positions.append((
+                current_x + (final_x - current_x) * 0.5,
+                current_y + (final_y - current_y) * 0.5,
+                current_z + (final_z - current_z) * 0.5
+            ))
+            stage2_positions.append((final_x, final_y, final_z))
+
+        return stage1_positions, stage2_positions
+
+    # ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎)
+
+    def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
         """
-        更新隊形參數
+        路徑跟隨編隊 — Bezier 曲線轉彎版
-        
+
+        步驟:
+            1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
+            2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
+
+        隊形 (俯視):
+            |     (前進方向)         |
+            |           D1          |  ← 左偏移, 後 0m
+            |                D2     |  ← 右偏移, 後 5m
+            |           D3          |  ← 左偏移, 後 10m
+            |                D4     |  ← 右偏移, 後 15m
+        """
+        N = len(drone_positions)
+        lateral_offset = params.get('lateral_offset', 3.0)
+        longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
+        altitude = params.get('altitude', self.base_altitude)
+        turn_margin = params.get('turn_margin', 0.35)      # 轉彎切入距離佔段長比例
+        curve_resolution = params.get('curve_resolution', 8)  # 每個彎道的插值點數
+
+        # Step 1: 建立帶 Bezier 轉彎的中心路徑
+        center_path = self._build_center_path(
+            route_wps_local, turn_margin, curve_resolution
+        )
+
+        # Step 2: 固定排序
+        first_dir = (center_path[0][2], center_path[0][3])
+        first_perp = (-first_dir[1], first_dir[0])
+        C_x = sum(p[0] for p in drone_positions) / N
+        C_y = sum(p[1] for p in drone_positions) / N
+
+        projections = [
+            ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
+            for i, pos in enumerate(drone_positions)
+        ]
+        projections.sort()
+
+        # Step 3: 偏移
+        all_waypoints = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            lat_sign = -1 if rank % 2 == 0 else 1
+            lat = lat_sign * lateral_offset
+            lon = rank * longitudinal_spacing
+
+            waypoints = []
+            for (cx, cy, dx, dy) in center_path:
+                perp_x, perp_y = -dy, dx
+                ox = cx + lat * perp_x - lon * dx
+                oy = cy + lat * perp_y - lon * dy
+                waypoints.append((ox, oy, altitude))
+
+            all_waypoints[original_idx] = waypoints
+
+        return all_waypoints
+
+    def _build_center_path(self, waypoints, turn_margin, curve_resolution):
+        """
+        建立帶 Bezier 曲線轉彎的中心路徑
+
+        在每個轉折 WP 處:
+            1. 計算 pre_turn = WP - d_in × T
+            2. 計算 post_turn = WP + d_out × T
+            3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post
+            4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP)
+
         Args:
-            spacing: 無人機間距（公尺）
+            waypoints: 路徑航點 [(x, y), ...]
-            base_altitude: 基準飛行高度（公尺）
+            turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
-            altitude_diff: M 字形的高低差（公尺）
+            curve_resolution: 每個彎道的 Bezier 插值點數
+
+        Returns:
+            [(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向
         """
-        if spacing is not None:
+        num_wps = len(waypoints)
-            self.spacing = spacing
+
-            print(f"   間距更新為: {spacing} m")
+        if num_wps < 2:
-        
+            return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)]
-        if base_altitude is not None:
+
-            self.base_altitude = base_altitude
+        # 計算每段方向和長度
-            print(f"   基準高度更新為: {base_altitude} m")
+        segments = []
-        
+        for i in range(num_wps - 1):
-        if altitude_diff is not None:
+            dx = waypoints[i + 1][0] - waypoints[i][0]
-            self.altitude_diff = altitude_diff
+            dy = waypoints[i + 1][1] - waypoints[i][1]
-            print(f"   高低差更新為: {altitude_diff} m")
+            length = math.sqrt(dx ** 2 + dy ** 2)
+            if length < 0.01:
+                segments.append((0.0, 1.0, length))
+            else:
+                segments.append((dx / length, dy / length, length))
+
+        path = []
+
+        # 第一個航點
+        path.append((waypoints[0][0], waypoints[0][1],
+                      segments[0][0], segments[0][1]))
+
+        # 中間航點 (轉彎處)
+        for i in range(1, num_wps - 1):
+            d_in_x, d_in_y, len_in = segments[i - 1]
+            d_out_x, d_out_y, len_out = segments[i]
+
+            # 切入距離 T: 取相鄰兩段中較短的 × turn_margin
+            T = min(len_in, len_out) * turn_margin
+
+            if T < 0.5:
+                # 段太短，不插彎，直接加一個平均方向點
+                avg_dx = d_in_x + d_out_x
+                avg_dy = d_in_y + d_out_y
+                avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
+                if avg_len > 0.01:
+                    avg_dx /= avg_len
+                    avg_dy /= avg_len
+                else:
+                    avg_dx, avg_dy = d_in_x, d_in_y
+                path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy))
+                continue
+
+            # P0 = pre_turn (弧線起始)
+            p0_x = waypoints[i][0] - d_in_x * T
+            p0_y = waypoints[i][1] - d_in_y * T
+
+            # P1 = WP 本身 (控制點)
+            p1_x = waypoints[i][0]
+            p1_y = waypoints[i][1]
+
+            # P2 = post_turn (弧線結束)
+            p2_x = waypoints[i][0] + d_out_x * T
+            p2_y = waypoints[i][1] + d_out_y * T
+
+            # 加入 pre_turn 點 (方向 = incoming)
+            path.append((p0_x, p0_y, d_in_x, d_in_y))
+
+            # 加入 Bezier 插值點
+            for j in range(1, curve_resolution):
+                t = j / curve_resolution
+
+                # B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2
+                one_minus_t = 1.0 - t
+                bx = one_minus_t * one_minus_t * p0_x + \
+                     2 * t * one_minus_t * p1_x + \
+                     t * t * p2_x
+                by = one_minus_t * one_minus_t * p0_y + \
+                     2 * t * one_minus_t * p1_y + \
+                     t * t * p2_y
+
+                # B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向
+                tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x)
+                tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y)
+
+                # 正規化
+                t_len = math.sqrt(tdx ** 2 + tdy ** 2)
+                if t_len > 0.01:
+                    tdx /= t_len
+                    tdy /= t_len
+                else:
+                    tdx, tdy = d_in_x, d_in_y
+
+                path.append((bx, by, tdx, tdy))
+
+            # 加入 post_turn 點 (方向 = outgoing)
+            path.append((p2_x, p2_y, d_out_x, d_out_y))
+
+        # 最後一個航點
+        path.append((waypoints[-1][0], waypoints[-1][1],
+                      segments[-1][0], segments[-1][1]))
+
+        return path
+
+    # ------------------------------------------------------------------ 柵狀掃描
+
+    def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
+        """柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊"""
+        N = len(drone_positions)
+        line_spacing = params.get('line_spacing', 5.0)
+        altitude = params.get('altitude', self.base_altitude)
+
+        c0, c1, c2, c3 = rect_corners_local
+
+        edge_a = (c1[0] - c0[0], c1[1] - c0[1])
+        len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
+        edge_b = (c3[0] - c0[0], c3[1] - c0[1])
+        len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)
+
+        if len_a >= len_b:
+            sweep_vec = edge_a
+            sweep_len = len_a
+            cut_vec = edge_b
+            cut_len = len_b
+            sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
+            sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
+            cut_start_corner = c0
+        else:
+            sweep_vec = edge_b
+            sweep_len = len_b
+            cut_vec = edge_a
+            cut_len = len_a
+            sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
+            sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
+            cut_start_corner = c0
+
+        sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
+        cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)
+
+        C_x = sum(p[0] for p in drone_positions) / N
+        C_y = sum(p[1] for p in drone_positions) / N
+
+        dist_to_start = math.sqrt(
+            (C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
+        )
+        dist_to_end = math.sqrt(
+            (C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
+        )
+
+        if dist_to_start <= dist_to_end:
+            near_corner_a = cut_start_corner
+        else:
+            sweep_dir = (-sweep_dir[0], -sweep_dir[1])
+            near_corner_a = (cut_start_corner[0] + sweep_vec[0],
+                            cut_start_corner[1] + sweep_vec[1])
+
+        projections = [
+            ((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
+            for i, pos in enumerate(drone_positions)
+        ]
+        projections.sort()
+
+        strip_width = cut_len / N
+        drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
+        near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
+        gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2
+
+        all_waypoints = [None] * N
+
+        for rank, (_, original_idx) in enumerate(projections):
+            strip_center_offset = (rank + 0.5) * strip_width
+            base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
+            base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset
+
+            waypoints_list = []
+
+            gather_offset = gather_sweep_proj - near_sweep_proj
+            gx = base_x + sweep_dir[0] * gather_offset
+            gy = base_y + sweep_dir[1] * gather_offset
+            waypoints_list.append((gx, gy, altitude))
+
+            num_lines = max(1, int(strip_width / line_spacing))
+            actual_spacing = strip_width / num_lines
+            first_line_offset = (rank * strip_width) + actual_spacing / 2
+
+            entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
+            entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
+            waypoints_list.append((entry_x, entry_y, altitude))
+
+            current_cut_offset = first_line_offset
+
+            for line_idx in range(num_lines):
+                line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
+                line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
+                line_far_x = line_near_x + sweep_dir[0] * sweep_len
+                line_far_y = line_near_y + sweep_dir[1] * sweep_len
+
+                if line_idx % 2 == 0:
+                    waypoints_list.append((line_far_x, line_far_y, altitude))
+                else:
+                    waypoints_list.append((line_near_x, line_near_y, altitude))
+
+                if line_idx < num_lines - 1:
+                    next_cut_offset = current_cut_offset + actual_spacing
+                    next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
+                    next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
+                    next_far_x = next_near_x + sweep_dir[0] * sweep_len
+                    next_far_y = next_near_y + sweep_dir[1] * sweep_len
+
+                    if line_idx % 2 == 0:
+                        waypoints_list.append((next_far_x, next_far_y, altitude))
+                    else:
+                        waypoints_list.append((next_near_x, next_near_y, altitude))
+                    current_cut_offset = next_cut_offset
+
+            all_waypoints[original_idx] = waypoints_list
+
+        return all_waypoints
 
 
-# ===== 測試程式碼 =====
+# ================================================================================
+# 測試
+# ================================================================================
 if __name__ == "__main__":
-    print("=" * 60)
+    planner = FormationPlanner()
-    print("隊形任務規劃器測試")
+
-    print("=" * 60)
+    drones = [
-    
+        (24.123450, 120.567800, 0.0),
-    # 模擬 5 台無人機的 GPS 位置
+        (24.123470, 120.567820, 0.0),
-    drone_gps = [
+        (24.123440, 120.567810, 0.0),
-        (24.123450, 120.567800, 100.0),  # 無人機 0
+        (24.123460, 120.567830, 0.0),
-        (24.123470, 120.567820, 102.0),  # 無人機 1
-        (24.123440, 120.567810, 98.0),   # 無人機 2
-        (24.123460, 120.567830, 101.0),  # 無人機 3
-        (24.123445, 120.567795, 99.0),   # 無人機 4
     ]
-    
+    target = (24.12400, 120.56795, 10.0)
-    # 目標點
+
-    target_gps = (24.123600, 120.568000, 105.0)
+    # M-Formation
-    
+    wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
-    # 建立規劃器
+    print("M-Formation:")
-    planner = FormationPlanner(
+    for i, wp_list in enumerate(wps):
-        spacing=5.0,           # 5 公尺間距
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
-        base_altitude=10.0,    # 基準高度 10 公尺
+
-        altitude_diff=2.0      # 高低差 2 公尺
+    # Leader-Follower (Bezier 轉彎)
+    route = [
+        (24.12345, 120.56780),
+        (24.12370, 120.56800),
+        (24.12390, 120.56820),
+        (24.12400, 120.56800),
+        (24.12420, 120.56790),
+    ]
+    wps_lf, origin_lf = planner.plan_formation_mission(
+        drones, target, MissionType.LEADER_FOLLOWER,
+        params={
+            'route_waypoints': route,
+            'lateral_offset': 3.0,
+            'longitudinal_spacing': 5.0,
+            'turn_margin': 0.35,
+            'curve_resolution': 8,
+            'altitude': 10.0
+        }
     )
-    
+    print(f"\nLeader-Follower (Bezier turns):")
-    # 規劃任務
+    for i, wp_list in enumerate(wps_lf):
-    print("\n開始規劃任務...")
+        print(f"  Drone {i}: {len(wp_list)} waypoints")
-    stage1, stage2 = planner.plan_formation_mission(drone_gps, target_gps)
+        for j, wp in enumerate(wp_list):
-    
+            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
-    # 顯示結果
+
-    print("\n" + "=" * 60)
+    # Grid Sweep
-    print("階段 1：集合點位置（GPS）")
+    rect = [
-    print("=" * 60)
+        (24.1237, 120.5679),
-    for i, (lat, lon, alt) in enumerate(stage1):
+        (24.1237, 120.5683),
-        print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)")
+        (24.1240, 120.5683),
-    
+        (24.1240, 120.5679)
-    print("\n" + "=" * 60)
+    ]
-    print("階段 2：目標點位置（GPS）")
+    wps2, origin2 = planner.plan_formation_mission(
-    print("=" * 60)
+        drones, target, MissionType.GRID_SWEEP,
-    for i, (lat, lon, alt) in enumerate(stage2):
+        params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
-        print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)")
+    )
-    
+    print(f"\nGrid Sweep:")
-    print("\n✅ 測試完成！")
+    for i, wp_list in enumerate(wps2):
+        print(f"  Drone {i}: {len(wp_list)} waypoints")

src/GUI/overview_table.py

@@ -6,7 +6,7 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向",
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
                           "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {

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