Compare commits

..

No commits in common. 'master' and 'master' have entirely different histories.

10
.gitignore vendored

@ -8,8 +8,7 @@
# 編譯的資料夾
**/build/
**/install/
**/log/
**/logs/
log/
# CMAKE 的衍生檔
CMakeCache.txt
@ -25,10 +24,3 @@ Makefile
**/*.class
**/*.pyc
**/*.pyo
**/.cursor/
CLAUDE.md
# 本地筆記與硬體 dump
my_env_notes.md
notes/
**/eeprom.bin

7
.gitmodules vendored

@ -1,7 +0,0 @@
[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

@ -1,97 +1,20 @@
這是天雷系統的專案
===
## 功能簡介
1. mavlink 多對多支援平台
2. 不允許進到 ros 系統有相同 sysid
3. 假設一台載具上所有 component 共用同一 socket
===
## 運行環境
### 專案核心框架
專案核心環境
1. ROS2 Humble
2. Python 3.8.10
2. Python
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
===
## 使用說明
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 # 自己定義的
Package 簡述
```
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 (核心)
建立、維護與飛控韌體的連接
構築 mavlink 封包
處理無線模組的通訊格式 (XBee)
--同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
3. fc_interfaces (重要)
自定義的 ROS2 介面檔
4. fc_network_apps
與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
使用者的外層包裝
5. someotherpkg
如何使用 fc_network_apps 的範例檔案
6. GUI
由 PyQt6 開發的互動式介面
N. logs 是執行時期的記錄檔
===

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

@ -1,774 +0,0 @@
#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
QPushButton, QLineEdit, QComboBox, QApplication)
from PyQt6.QtGui import QFont
from PyQt6.QtCore import pyqtSignal
import glob
import os
def _get_font_scale():
app = QApplication.instance()
if app is None:
return 1.0
scale = app.property("font_scale")
try:
return float(scale) if scale is not None else 1.0
except (TypeError, ValueError):
return 1.0
def _scale_stylesheet_font_sizes(stylesheet, scale):
if not stylesheet or 'font-size' not in stylesheet:
return stylesheet
import re
def repl(match):
size = float(match.group(1))
unit = match.group(2)
scaled = max(1.0, size * scale)
text = f"{scaled:.2f}".rstrip('0').rstrip('.')
return f"font-size: {text}{unit}"
return re.sub(r'font-size\s*:\s*([0-9]+(?:\.[0-9]+)?)\s*(px|pt)', repl, stylesheet)
def _set_scaled_stylesheet(widget, stylesheet):
widget._base_stylesheet = stylesheet
scaled = _scale_stylesheet_font_sizes(stylesheet, _get_font_scale())
widget._applied_stylesheet = scaled
widget.setStyleSheet(scaled)
def _reapply_scaled_stylesheet(widget):
current_stylesheet = widget.styleSheet()
base_stylesheet = getattr(widget, '_base_stylesheet', None)
applied_stylesheet = getattr(widget, '_applied_stylesheet', None)
if current_stylesheet != applied_stylesheet:
base_stylesheet = current_stylesheet
widget._base_stylesheet = base_stylesheet
if base_stylesheet is not None:
scaled = _scale_stylesheet_font_sizes(base_stylesheet, _get_font_scale())
widget._applied_stylesheet = scaled
if current_stylesheet != scaled:
widget.setStyleSheet(scaled)
def _apply_scaled_font(widget):
base_font = getattr(widget, '_base_font_for_scale', None)
if base_font is None:
app = QApplication.instance()
app_base_font = app.property("base_app_font") if app else None
base_font = QFont(app_base_font) if app_base_font is not None else QFont(widget.font())
widget._base_font_for_scale = QFont(base_font)
scaled_font = QFont(base_font)
scale = _get_font_scale()
if base_font.pointSizeF() > 0:
scaled_font.setPointSizeF(max(1.0, base_font.pointSizeF() * scale))
elif base_font.pointSize() > 0:
scaled_font.setPointSize(max(1, int(round(base_font.pointSize() * scale))))
widget.setFont(scaled_font)
class CommPanel(QWidget):
"""通讯设置面板类"""
# 定义信号
udp_connection_added = pyqtSignal(str, int) # ip, port
udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
ws_connection_added = pyqtSignal(str) # url
ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
serial_connection_added = pyqtSignal(str, int) # port, baudrate
serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
serial_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
status_message = pyqtSignal(str, int) # message, timeout
def __init__(self, parent=None):
super().__init__(parent)
self.udp_connections = []
self.ws_connections = []
self.serial_connections = []
self._init_ui()
self.apply_font_scale()
def _init_ui(self):
"""初始化UI"""
layout = QVBoxLayout(self)
layout.setContentsMargins(10, 10, 10, 10)
layout.setSpacing(10)
# ========== UDP MAVLink 區域 ==========
udp_title = QLabel("UDP")
_set_scaled_stylesheet(udp_title, """
color: #DDD;
font-size: 14px;
font-weight: bold;
padding: 5px;
background-color: #333;
border-radius: 4px;
""")
layout.addWidget(udp_title)
# UDP 連接列表容器
self.udp_list_widget = QWidget()
self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
self.udp_list_layout.setSpacing(5)
layout.addWidget(self.udp_list_widget)
# UDP 添加新連接區域
add_udp_widget = QWidget()
add_udp_layout = QHBoxLayout(add_udp_widget)
add_udp_layout.setContentsMargins(0, 0, 0, 0)
self.udp_ip_input = QLineEdit()
self.udp_ip_input.setText("127.0.0.1")
self.udp_ip_input.setPlaceholderText("IP")
_set_scaled_stylesheet(self.udp_ip_input, """
QLineEdit {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
""")
self.udp_port_input = QLineEdit()
self.udp_port_input.setText("14550")
self.udp_port_input.setPlaceholderText("Port")
self.udp_port_input.setFixedWidth(80)
_set_scaled_stylesheet(self.udp_port_input, """
QLineEdit {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
""")
add_udp_btn = QPushButton("添加")
add_udp_btn.clicked.connect(self._handle_add_udp)
_set_scaled_stylesheet(add_udp_btn, """
QPushButton {
background-color: #4CAF50;
color: white;
border: none;
padding: 6px 12px;
border-radius: 4px;
min-width: 30px;
}
QPushButton:hover { background-color: #45a049; }
""")
ip_label = QLabel("IP:")
_set_scaled_stylesheet(ip_label, "color: #DDD;")
add_udp_layout.addWidget(ip_label)
add_udp_layout.addWidget(self.udp_ip_input)
port_label = QLabel("Port:")
_set_scaled_stylesheet(port_label, "color: #DDD;")
add_udp_layout.addWidget(port_label)
add_udp_layout.addWidget(self.udp_port_input)
add_udp_layout.addWidget(add_udp_btn)
layout.addWidget(add_udp_widget)
# 分隔線
separator = QWidget()
separator.setFixedHeight(20)
layout.addWidget(separator)
# ========== Serial 區域 ==========
serial_title = QLabel("Serial")
_set_scaled_stylesheet(serial_title, """
color: #DDD;
font-size: 14px;
font-weight: bold;
padding: 5px;
background-color: #333;
border-radius: 4px;
""")
layout.addWidget(serial_title)
# Serial 連接列表容器
self.serial_list_widget = QWidget()
self.serial_list_layout = QVBoxLayout(self.serial_list_widget)
self.serial_list_layout.setContentsMargins(0, 0, 0, 0)
self.serial_list_layout.setSpacing(5)
layout.addWidget(self.serial_list_widget)
# Serial 添加新連接區域
add_serial_widget = QWidget()
add_serial_layout = QHBoxLayout(add_serial_widget)
add_serial_layout.setContentsMargins(0, 0, 0, 0)
self.serial_port_combo = QComboBox()
_set_scaled_stylesheet(self.serial_port_combo, """
QComboBox {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
QComboBox::drop-down {
border: none;
}
QComboBox::down-arrow {
image: none;
border-left: 5px solid transparent;
border-right: 5px solid transparent;
border-top: 5px solid #DDD;
}
""")
self._refresh_serial_ports()
refresh_ports_btn = QPushButton("")
refresh_ports_btn.setFixedWidth(35)
refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
refresh_ports_btn.setToolTip("重新掃描串口")
_set_scaled_stylesheet(refresh_ports_btn, """
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 6px;
border-radius: 4px;
font-size: 16px;
}
QPushButton:hover { background-color: #555; }
""")
self.serial_baudrate_combo = QComboBox()
self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
self.serial_baudrate_combo.setCurrentText('57600')
self.serial_baudrate_combo.setFixedWidth(100)
_set_scaled_stylesheet(self.serial_baudrate_combo, """
QComboBox {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
QComboBox::drop-down {
border: none;
}
QComboBox::down-arrow {
image: none;
border-left: 5px solid transparent;
border-right: 5px solid transparent;
border-top: 5px solid #DDD;
}
""")
add_serial_btn = QPushButton("添加")
add_serial_btn.clicked.connect(self._handle_add_serial)
_set_scaled_stylesheet(add_serial_btn, """
QPushButton {
background-color: #4CAF50;
color: white;
border: none;
padding: 6px 12px;
border-radius: 4px;
min-width: 30px;
}
QPushButton:hover { background-color: #45a049; }
""")
serial_port_label = QLabel("Port:")
_set_scaled_stylesheet(serial_port_label, "color: #DDD;")
add_serial_layout.addWidget(serial_port_label)
add_serial_layout.addWidget(self.serial_port_combo)
add_serial_layout.addWidget(refresh_ports_btn)
baud_label = QLabel("Baud:")
_set_scaled_stylesheet(baud_label, "color: #DDD;")
add_serial_layout.addWidget(baud_label)
add_serial_layout.addWidget(self.serial_baudrate_combo)
add_serial_layout.addWidget(add_serial_btn)
layout.addWidget(add_serial_widget)
# 分隔線
separator = QWidget()
separator.setFixedHeight(20)
layout.addWidget(separator)
# ========== WebSocket 區域 ==========
ws_title = QLabel("WebSocket")
_set_scaled_stylesheet(ws_title, """
color: #DDD;
font-size: 14px;
font-weight: bold;
padding: 5px;
background-color: #333;
border-radius: 4px;
""")
layout.addWidget(ws_title)
# WebSocket 連接列表容器
self.ws_list_widget = QWidget()
self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
self.ws_list_layout.setSpacing(5)
layout.addWidget(self.ws_list_widget)
# WebSocket 添加新連接區域
add_ws_widget = QWidget()
add_ws_layout = QHBoxLayout(add_ws_widget)
add_ws_layout.setContentsMargins(0, 0, 0, 0)
self.ws_url_input = QLineEdit()
self.ws_url_input.setPlaceholderText("host")
_set_scaled_stylesheet(self.ws_url_input, """
QLineEdit {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
""")
add_ws_btn = QPushButton("添加")
add_ws_btn.clicked.connect(self._handle_add_ws)
_set_scaled_stylesheet(add_ws_btn, """
QPushButton {
background-color: #4CAF50;
color: white;
border: none;
padding: 6px 12px;
border-radius: 4px;
min-width: 30px;
}
QPushButton:hover { background-color: #45a049; }
""")
url_label = QLabel("URL:")
_set_scaled_stylesheet(url_label, "color: #DDD;")
add_ws_layout.addWidget(url_label)
add_ws_layout.addWidget(self.ws_url_input)
add_ws_layout.addWidget(add_ws_btn)
layout.addWidget(add_ws_widget)
layout.addStretch()
def _handle_add_udp(self):
"""處理添加 UDP 連接"""
ip = self.udp_ip_input.text().strip()
port_text = self.udp_port_input.text().strip()
if not ip or not port_text:
self.status_message.emit("請輸入 IP 和 Port", 3000)
return
try:
port = int(port_text)
if port < 1 or port > 65535:
raise ValueError("Port 超出範圍")
except ValueError:
self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
return
# 檢查是否已存在相同連接
for conn in self.udp_connections:
if conn['ip'] == ip and conn['port'] == port:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.udp_connection_added.emit(ip, port)
# 只清空 port 輸入框,保留 IP
self.udp_port_input.clear()
def _handle_add_ws(self):
"""處理添加 WebSocket 連接"""
input_url = self.ws_url_input.text().strip()
if not input_url:
self.status_message.emit("請輸入 WebSocket URL", 3000)
return
# 自動添加 ws:// 前綴
if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
url = f'ws://{input_url}'
else:
url = input_url
# 基本 URL 格式驗證
try:
if '://' in url:
parts = url.split('://', 1)
if len(parts) == 2 and ':' not in parts[1]:
self.status_message.emit("URL 格式錯誤,需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
return
except:
self.status_message.emit("URL 格式錯誤", 3000)
return
# 檢查是否已存在相同連接
for conn in self.ws_connections:
if conn['url'] == url:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.ws_connection_added.emit(url)
# 清空輸入框
self.ws_url_input.clear()
def _refresh_serial_ports(self):
"""重新掃描可用的串口"""
self.serial_port_combo.clear()
# 掃描 Linux 下的串口設備
ports = []
# 掃描 USB 串口
usb_ports = glob.glob('/dev/ttyUSB*')
ports.extend(usb_ports)
# 掃描 ACM 串口
acm_ports = glob.glob('/dev/ttyACM*')
ports.extend(acm_ports)
# 排序
ports.sort()
if ports:
self.serial_port_combo.addItems(ports)
else:
self.serial_port_combo.addItem("沒有找到串口")
self.serial_port_combo.setEnabled(False)
return
self.serial_port_combo.setEnabled(True)
def _handle_add_serial(self):
"""處理添加 Serial 連接"""
port = self.serial_port_combo.currentText()
baudrate_text = self.serial_baudrate_combo.currentText()
if not port or port == "沒有找到串口":
self.status_message.emit("請選擇有效的串口", 3000)
return
try:
baudrate = int(baudrate_text)
except ValueError:
self.status_message.emit("波特率格式錯誤", 3000)
return
# 檢查是否已存在相同連接
for conn in self.serial_connections:
if conn['port'] == port:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.serial_connection_added.emit(port, baudrate)
def _handle_add_ws(self):
"""處理添加 WebSocket 連接"""
input_url = self.ws_url_input.text().strip()
if not input_url:
self.status_message.emit("請輸入 WebSocket URL", 3000)
return
# 自動添加 ws:// 前綴
if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
url = f'ws://{input_url}'
else:
url = input_url
# 基本 URL 格式驗證
try:
if '://' in url:
parts = url.split('://', 1)
if len(parts) == 2 and ':' not in parts[1]:
self.status_message.emit("URL 格式錯誤,需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
return
except:
self.status_message.emit("URL 格式錯誤", 3000)
return
# 檢查是否已存在相同連接
for conn in self.ws_connections:
if conn['url'] == url:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.ws_connection_added.emit(url)
# 清空輸入框
self.ws_url_input.clear()
def create_udp_connection_panel(self, conn):
"""創建 UDP 連接面板"""
panel = QWidget()
_set_scaled_stylesheet(panel, """
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
padding: 8px;
border: 1px solid #444;
}
""")
layout = QHBoxLayout(panel)
layout.setContentsMargins(8, 8, 8, 8)
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
_set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
# 狀態指示器
status_label = QLabel("")
if conn.get('enabled', False):
_set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
_set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
# 控制按鈕
toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
toggle_btn.setFixedWidth(60)
toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label))
_set_scaled_stylesheet(toggle_btn, """
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #555; }
""")
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
_set_scaled_stylesheet(remove_btn, """
QPushButton {
background-color: #d32f2f;
color: white;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #b71c1c; }
""")
layout.addWidget(status_label)
layout.addWidget(info_label)
layout.addStretch()
layout.addWidget(toggle_btn)
layout.addWidget(remove_btn)
# 儲存引用
panel.connection = conn
panel.toggle_btn = toggle_btn
panel.status_label = status_label
return panel
def create_ws_connection_panel(self, conn):
"""創建 WebSocket 連接面板"""
panel = QWidget()
_set_scaled_stylesheet(panel, """
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
padding: 8px;
border: 1px solid #444;
}
""")
layout = QHBoxLayout(panel)
layout.setContentsMargins(8, 8, 8, 8)
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['url']}")
_set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
# 狀態指示器
status_label = QLabel("")
if conn.get('enabled', False):
_set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
_set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
# 控制按鈕
toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
toggle_btn.setFixedWidth(60)
toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label))
_set_scaled_stylesheet(toggle_btn, """
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #555; }
""")
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
_set_scaled_stylesheet(remove_btn, """
QPushButton {
background-color: #d32f2f;
color: white;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #b71c1c; }
""")
layout.addWidget(status_label)
layout.addWidget(info_label)
layout.addStretch()
layout.addWidget(toggle_btn)
layout.addWidget(remove_btn)
# 儲存引用
panel.connection = conn
panel.toggle_btn = toggle_btn
panel.status_label = status_label
return panel
def add_udp_panel(self, conn):
"""添加 UDP 連接面板到列表"""
panel = self.create_udp_connection_panel(conn)
self.udp_list_layout.addWidget(panel)
self.udp_connections.append(conn)
return panel
def add_ws_panel(self, conn):
"""添加 WebSocket 連接面板到列表"""
panel = self.create_ws_connection_panel(conn)
self.ws_list_layout.addWidget(panel)
self.ws_connections.append(conn)
return panel
def remove_udp_connection_from_list(self, conn):
"""從列表中移除 UDP 連接"""
if conn in self.udp_connections:
self.udp_connections.remove(conn)
def remove_ws_connection_from_list(self, conn):
"""從列表中移除 WebSocket 連接"""
if conn in self.ws_connections:
self.ws_connections.remove(conn)
def create_serial_connection_panel(self, conn):
"""創建 Serial 連接面板"""
panel = QWidget()
_set_scaled_stylesheet(panel, """
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
padding: 8px;
border: 1px solid #444;
}
""")
layout = QHBoxLayout(panel)
layout.setContentsMargins(8, 8, 8, 8)
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
_set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
# 狀態指示器
status_label = QLabel("")
if conn.get('enabled', False):
_set_scaled_stylesheet(status_label, "color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
_set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
# 控制按鈕
toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
toggle_btn.setFixedWidth(60)
toggle_btn.clicked.connect(lambda: self.serial_connection_toggled.emit(conn, toggle_btn, status_label))
_set_scaled_stylesheet(toggle_btn, """
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #555; }
""")
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
_set_scaled_stylesheet(remove_btn, """
QPushButton {
background-color: #d32f2f;
color: white;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #b71c1c; }
""")
layout.addWidget(status_label)
layout.addWidget(info_label)
layout.addStretch()
layout.addWidget(toggle_btn)
layout.addWidget(remove_btn)
# 儲存引用
panel.connection = conn
panel.toggle_btn = toggle_btn
panel.status_label = status_label
return panel
def add_serial_panel(self, conn):
"""添加 Serial 連接面板到列表"""
panel = self.create_serial_connection_panel(conn)
self.serial_list_layout.addWidget(panel)
self.serial_connections.append(conn)
return panel
def remove_serial_connection_from_list(self, conn):
"""從列表中移除 Serial 連接"""
if conn in self.serial_connections:
self.serial_connections.remove(conn)
def apply_font_scale(self):
"""重新套用目前字體倍率到通訊面板。"""
_apply_scaled_font(self)
_reapply_scaled_stylesheet(self)
for child in self.findChildren(QWidget):
_apply_scaled_font(child)
_reapply_scaled_stylesheet(child)

@ -1,169 +0,0 @@
#!/usr/bin/env python3
"""
指令發送模組
負責將目標位置轉成具體的通訊指令發送到飛控
目前使用 Ros2CommandSender fc_network_adapter
/fc_network/vehicle/pos_global_int service 發送
MavlinkSender 保留作為直連 SITL fallback 工具但已不是預設路徑
"""
import asyncio
import traceback
from abc import ABC, abstractmethod
from PyQt6.QtCore import QObject, pyqtSignal
from pymavlink import mavutil
def _log(level, message):
print(f"[{level}] {message}")
class CommandSender(ABC):
"""指令發送抽象介面"""
@abstractmethod
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""
發送全球座標位置指令
Args:
drone_id: GUI 用的 drone 識別字串 ( 's0_11')
sysid: 目標無人機的 MAVLink system ID
lat: 緯度 ()
lon: 經度 ()
alt: 高度 (公尺)
"""
pass
@abstractmethod
def close(self):
"""關閉連線"""
pass
class MavlinkSender(CommandSender):
"""
MAVLink 直接發送 (驗證階段用)
透過 SET_POSITION_TARGET_GLOBAL_INT 發送目標位置
使用方式:
sender = MavlinkSender("udpout:127.0.0.1:14550")
sender.send_position_global(sysid=1, lat=24.123, lon=120.567, alt=10.0)
"""
# type_mask: 只使用位置 (lat, lon, alt)
# 忽略速度 (bit 3,4,5)、加速度 (bit 6,7,8)、yaw (bit 10)、yaw_rate (bit 11)
TYPE_MASK_POSITION_ONLY = (
0b0000_1101_1111_1000 # = 0x0DF8
)
def __init__(self, connection_string="udpout:127.0.0.1:14550"):
"""
Args:
connection_string: MAVLink 連線字串
SITL 範例: "udpout:127.0.0.1:14550"
"""
self.connection_string = connection_string
self.mav = mavutil.mavlink_connection(connection_string)
_log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用,保留為介面相容。"""
self.mav.mav.set_position_target_global_int_send(
0, # time_boot_ms (不使用)
sysid, # target_system
1, # target_component (autopilot)
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
self.TYPE_MASK_POSITION_ONLY,
int(lat * 1e7), # lat_int
int(lon * 1e7), # lon_int
float(alt), # alt
0, 0, 0, # vx, vy, vz (忽略)
0, 0, 0, # afx, afy, afz (忽略)
0, 0 # yaw, yaw_rate (忽略)
)
def close(self):
"""關閉 MAVLink 連線"""
if self.mav:
self.mav.close()
self.mav = None
_log("INFO", "MavlinkSender 已關閉")
class Ros2CommandSender(QObject):
"""
透過 fc_network_apps.PositionTargetGlobalIntClient goto 指令
設計重點:
- send_position_global 是同步介面立刻回傳結果透過 send_result signal 回報
- 每台 drone 維護自己的 asyncio.Task pipeline新的目標會 cancel 掉舊的 in-flight
- client DroneMonitor.get_or_create_position_client 管理per-drone 獨立節點
"""
# (drone_id, sysid, success, message)
send_result = pyqtSignal(str, int, bool, str)
DEFAULT_TIMEOUT_SEC = 2.0
def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
super().__init__()
self._monitor = monitor
self._timeout_sec = timeout_sec
self._pending = {} # drone_id -> asyncio.Task
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""同步介面。立刻回,結果透過 send_result signal 回報。"""
# 取消該機上一個未完成的 task避免舊指令覆蓋新指令
old = self._pending.get(drone_id)
if old is not None and not old.done():
old.cancel()
try:
loop = asyncio.get_event_loop()
except RuntimeError:
loop = asyncio.new_event_loop()
asyncio.set_event_loop(loop)
task = loop.create_task(
self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
)
self._pending[drone_id] = task
async def _do_send(self, drone_id, sysid, lat, lon, alt):
try:
client = self._monitor.get_or_create_position_client(drone_id)
if client is None:
self.send_result.emit(
drone_id, sysid, False,
"PositionTargetGlobalIntClient 無法建立"
)
return
result = await client.goto_position_only_async(
target_sysid=sysid,
target_compid=0,
latitude_deg=lat,
longitude_deg=lon,
alt=alt,
timeout_sec=self._timeout_sec,
)
self.send_result.emit(
drone_id, sysid, bool(result.success), str(result.message or "")
)
except asyncio.CancelledError:
# 被新的目標取代,正常狀況,不用回報
pass
except Exception as e:
traceback.print_exc()
self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
def close(self):
"""取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
for task in self._pending.values():
if not task.done():
task.cancel()
self._pending.clear()
_log("INFO", "Ros2CommandSender 已關閉")

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

@ -1,382 +0,0 @@
#!/usr/bin/env python3
"""
任務執行模組
管理多架無人機的 GUIDED 模式飛行控制迴圈
設計:
- 每架無人機持有一個航點序列逐點推進
- 各自到達就各自切換到下一個航點
- 事件驅動發送航點切換時送一次收到 send_result 才決定重送/前進
- 失敗策略 (b)單機重試 MAX_RETRY 次仍失敗 該機 fallback LOITER其他機繼續
- Rendezvous barrier在指定 wp index 等所有活著的機到齊才一起推進 timeout 保護
- QTimer 驅動到達判定 Qt 主線程執行
- 暫停 = 停止到達判定 + 停止新指令
"""
import asyncio
import math
import time
from enum import Enum
from PyQt6.QtCore import QObject, QTimer, pyqtSignal
def _log(level, message):
print(f"[{level}] {message}")
class MissionState(Enum):
"""整體任務狀態"""
IDLE = "idle"
RUNNING = "running"
PAUSED = "paused"
class TaskStatus(Enum):
"""單機任務狀態"""
NORMAL = "normal"
RETRYING = "retrying"
WAITING_AT_BARRIER = "waiting_at_barrier"
FALLBACK_LOITER = "fallback_loiter"
class DroneTask:
"""單架無人機的任務資料"""
__slots__ = (
'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
'sent_current_wp', 'fail_count', 'status', 'waiting_since',
)
def __init__(self, drone_id, sysid, waypoints):
self.drone_id = drone_id
self.sysid = sysid
self.waypoints = waypoints
self.wp_index = 0
self.done = len(waypoints) == 0
self.sent_current_wp = False
self.fail_count = 0
self.status = TaskStatus.NORMAL
self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
@property
def current_target(self):
if self.done or self.wp_index >= len(self.waypoints):
return None
return self.waypoints[self.wp_index]
@property
def total_waypoints(self):
return len(self.waypoints)
class MissionExecutor(QObject):
"""
任務執行器 (事件驅動版 + rendezvous barrier)
planned_waypoints 格式:
{
'drone_ids': ['s0_1', 's0_2', ...],
'waypoints': [
[(lat,lon,alt), ...], # drone 0
[(lat,lon,alt), ...], # drone 1
],
'rendezvous_indices': [3, 7, ...], # 選填;缺省 = 全程不同步(各自跑)
}
"""
MAX_RETRY = 3
drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total)
task_status_changed = pyqtSignal(str, str, str) # (drone_id, status, message)
mission_completed = pyqtSignal()
def __init__(self, sender, drone_gps, monitor=None,
arrival_radius=2.0, tick_rate_hz=2.0,
barrier_timeout_sec=20.0):
super().__init__()
self.sender = sender
self.drone_gps = drone_gps
self.monitor = monitor # 用於失敗 fallback 到 LOITER
self.arrival_radius = arrival_radius
self.barrier_timeout_sec = barrier_timeout_sec
self.state = MissionState.IDLE
self.tasks = {}
self.rendezvous_indices = set()
self._interval_ms = int(1000 / tick_rate_hz)
self._timer = QTimer()
self._timer.timeout.connect(self._tick)
# 只有 Ros2CommandSender 有 send_result signalMavlinkSender 沒有
if hasattr(sender, 'send_result'):
sender.send_result.connect(self._on_send_result)
# ------------------------------------------------------------------ 公開方法
def start(self, planned_waypoints):
if self.state == MissionState.RUNNING:
_log("WARN", "任務已在執行中")
return
self.tasks.clear()
drone_ids = planned_waypoints['drone_ids']
waypoints_list = planned_waypoints['waypoints']
self.rendezvous_indices = set(
planned_waypoints.get('rendezvous_indices', []) or []
)
for i, drone_id in enumerate(drone_ids):
sysid = int(drone_id.split('_')[1])
self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i])
self.state = MissionState.RUNNING
self._timer.start(self._interval_ms)
total_wps = sum(t.total_waypoints for t in self.tasks.values())
rv_info = (
f"rendezvous WP={sorted(self.rendezvous_indices)}"
if self.rendezvous_indices else "無 rendezvous (各自跑)"
)
_log(
"INFO",
f"任務已啟動: {len(self.tasks)} 架無人機, "
f"{total_wps} 個航點, "
f"到達半徑={self.arrival_radius}m, "
f"tick 週期={self._interval_ms}ms, "
f"barrier timeout={self.barrier_timeout_sec}s, "
f"{rv_info}",
)
def pause(self):
if self.state == MissionState.RUNNING:
self._timer.stop()
self.state = MissionState.PAUSED
_log("INFO", "任務已暫停")
def resume(self):
if self.state == MissionState.PAUSED:
self._timer.start(self._interval_ms)
self.state = MissionState.RUNNING
_log("INFO", "任務已恢復")
def stop(self):
self._timer.stop()
self.tasks.clear()
self.rendezvous_indices = set()
self.state = MissionState.IDLE
_log("INFO", "任務已停止")
# ------------------------------------------------------------------ 控制迴圈
def _tick(self):
"""
控制 tick
Phase 1: 每台機做到達判定 barrier 點直接推進barrier 點改成 WAITING
Phase 2: 檢查 barrier 能不能釋放全員到齊或 timeout
Phase 3: 每台未送過當前航點的機送一次
Phase 4: 全部完成檢查
"""
now = time.monotonic()
# ---- Phase 1: 到達判定 ----
for task in self.tasks.values():
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
continue
# 已在 barrier 等待的不再跑到達判定
if task.status == TaskStatus.WAITING_AT_BARRIER:
continue
target = task.current_target
if target is None:
continue
gps = self.drone_gps.get(task.drone_id)
if gps is None:
continue
tgt_lat, tgt_lon, _ = target
distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
if distance >= self.arrival_radius:
continue
# 到達當前 wp_index
if (task.wp_index in self.rendezvous_indices
and task.wp_index < task.total_waypoints - 1):
# rendezvous 點 → 不推進,進入 barrier 等待
task.status = TaskStatus.WAITING_AT_BARRIER
task.waiting_since = now
_log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index},等待同伴")
self.task_status_changed.emit(
task.drone_id, task.status.value,
f"waiting at WP {task.wp_index}"
)
continue
# 一般點 → 直接推進
self._advance_waypoint(task, distance)
# ---- Phase 2: barrier 釋放檢查 ----
self._check_barriers(now)
# ---- Phase 3: 發送未送過的目標 ----
for task in self.tasks.values():
if task.done or task.status in (
TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
):
continue
if task.sent_current_wp:
continue
target = task.current_target
if target is None:
continue
tgt_lat, tgt_lon, tgt_alt = target
task.sent_current_wp = True
self.sender.send_position_global(
task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
)
# ---- Phase 4: 完成檢查 ----
all_done = all(
t.done or t.status == TaskStatus.FALLBACK_LOITER
for t in self.tasks.values()
)
if all_done and self.tasks:
self._timer.stop()
self.state = MissionState.IDLE
self.mission_completed.emit()
_log("INFO", "任務全部完成")
def _advance_waypoint(self, task, arrived_distance):
"""把 task 推進一個航點,重置發送旗標。不處理 barrier 邏輯。"""
task.wp_index += 1
task.sent_current_wp = False
task.fail_count = 0
if task.wp_index >= task.total_waypoints:
task.done = True
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
_log(
"INFO",
f"{task.drone_id} 已完成所有航點 "
f"({task.total_waypoints}/{task.total_waypoints})",
)
return
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
_log(
"INFO",
f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
f"(到達距離: {arrived_distance:.1f}m)",
)
def _check_barriers(self, now):
"""檢查每個有人在等的 barrier 能不能釋放。"""
# 蒐集目前有哪些 barrier 有人卡著
waiting_groups = {} # barrier_idx -> [task, ...]
for task in self.tasks.values():
if task.status == TaskStatus.WAITING_AT_BARRIER:
waiting_groups.setdefault(task.wp_index, []).append(task)
if not waiting_groups:
return
for barrier_idx, waiting_tasks in waiting_groups.items():
# 判斷是否「全員到齊」:所有仍活著的機都要麼在此 barrier 等,要麼早已越過此 idx
everyone_arrived = True
for task in self.tasks.values():
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
continue # 失敗機/完成機不擋
if (task.status == TaskStatus.WAITING_AT_BARRIER
and task.wp_index == barrier_idx):
continue # 在這裡等
if task.wp_index > barrier_idx:
continue # 早已越過barrier 先前已對他釋放)
# 還在路上
everyone_arrived = False
break
force_reason = None
if not everyone_arrived:
# 檢查 timeout以該 barrier 上最早進入等待的時間為準
oldest_wait = min(t.waiting_since for t in waiting_tasks)
if now - oldest_wait >= self.barrier_timeout_sec:
force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
_log("WARN", f"barrier WP {barrier_idx} {force_reason},強制放行")
else:
continue # 還沒到齊、也還沒 timeout → 繼續等
# 釋放:把所有在此 barrier 等待的機一起推進
tag = "全員到齊" if force_reason is None else force_reason
_log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag}),推進 {len(waiting_tasks)}")
for task in waiting_tasks:
task.status = TaskStatus.NORMAL
msg = f"barrier WP {barrier_idx} released ({tag})"
self.task_status_changed.emit(
task.drone_id, task.status.value, msg
)
self._advance_waypoint(task, self.arrival_radius)
# ------------------------------------------------------------------ 結果回呼
def _on_send_result(self, drone_id, sysid, success, message):
"""Ros2CommandSender.send_result 的 slot"""
task = self.tasks.get(drone_id)
if task is None or task.done:
return
# 若已在 barrier 等待,舊指令的遲到回應不要再觸發重試/fallback
if task.status == TaskStatus.WAITING_AT_BARRIER:
return
if success:
if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
task.status = TaskStatus.NORMAL
self.task_status_changed.emit(drone_id, task.status.value, "recovered")
task.fail_count = 0
return
task.fail_count += 1
_log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
if task.fail_count < self.MAX_RETRY:
task.status = TaskStatus.RETRYING
task.sent_current_wp = False # 下個 tick 會重送
self.task_status_changed.emit(
drone_id, task.status.value,
f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
)
else:
task.status = TaskStatus.FALLBACK_LOITER
self.task_status_changed.emit(
drone_id, task.status.value,
f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
)
_log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次,切換至 LOITER")
self._fallback_to_loiter(drone_id)
def _fallback_to_loiter(self, drone_id):
"""用 monitor.set_mode 切 LOITER。set_mode 是 coroutine透過 event loop 派送。"""
if self.monitor is None:
_log("WARN", f"無 monitor無法將 {drone_id} 切換至 LOITER")
return
try:
loop = asyncio.get_event_loop()
except RuntimeError:
loop = asyncio.new_event_loop()
asyncio.set_event_loop(loop)
loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
# ------------------------------------------------------------------ 工具函式
def _haversine(lat1, lon1, lat2, lon2):
"""計算兩個 GPS 座標間的水平距離 (公尺)"""
R = 6371000.0
lat1_r = math.radians(lat1)
lat2_r = math.radians(lat2)
dlat = math.radians(lat2 - lat1)
dlon = math.radians(lon2 - lon1)
a = (math.sin(dlat / 2) ** 2 +
math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

@ -1,577 +0,0 @@
#!/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', # 粉
]
DEFAULT_MISSION_PARAM_VALUES = {
'spacing': '5.0',
'base_altitude': '10.0',
'altitude_diff': '2.0',
'radius': '10.0',
'altitude': '10.0',
'start_angle': '0',
'lateral_offset': '3.0',
'longitudinal_spacing': '5.0',
'line_spacing': '5.0',
}
class MissionGroup:
"""單一任務群組的資料"""
def __init__(self, group_id, color):
self.group_id = group_id # 'A', 'B', 'C', ...
self.color = color # 顏色 hex
# 唯一真實資料來源:該 group 選中的 drone ID
# 代表group 擁有的 drone、被選中的 drone、可以操作的 drone
self.selected_drone_ids = set()
self.mission_type = 'M_FORMATION' # 預設任務類型
self.planned_waypoints = None # 規劃結果 dict
self.executor = None # MissionExecutor 實例(延遲建立)
self.center_origin = None # 規劃原點
self.leader_drone_id = None # LEADER_FOLLOWER 專用:指定的領隊無人機 ID
@property
def state(self):
if self.executor is None:
return MissionState.IDLE
return self.executor.state
@property
def display_name(self):
return f"Group {self.group_id}"
class DroneAssignDialog(QDialog):
"""無人機分配對話框"""
def __init__(self, parent, all_drone_ids, current_assigned, other_assigned=None):
"""
Args:
parent: widget
all_drone_ids: 所有可用無人機 ID 列表
current_assigned: 當前群組已分配的無人機 set
other_assigned: (忽略 現在允許多 group 分配)
"""
super().__init__(parent)
self.setWindowTitle("分配無人機")
self.setMinimumWidth(280)
self.setStyleSheet("""
QDialog { background-color: #2B2B2B; }
QLabel { color: #DDD; }
QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
QCheckBox::indicator { width: 16px; height: 16px; }
""")
layout = QVBoxLayout(self)
title = QLabel("選擇要分配到此群組的無人機:")
title.setStyleSheet("font-size: 13px; font-weight: bold; padding-bottom: 6px;")
layout.addWidget(title)
# 滾動區域
scroll = QScrollArea()
scroll.setWidgetResizable(True)
scroll.setMaximumHeight(300)
scroll_widget = QWidget()
scroll_layout = QVBoxLayout(scroll_widget)
scroll_layout.setSpacing(2)
self.checkboxes = {}
sorted_ids = sorted(all_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
for drone_id in sorted_ids:
cb = QCheckBox(drone_id)
# 所有 drone 都能被選摘(支持多 group 分配)
if drone_id in current_assigned:
cb.setChecked(True)
self.checkboxes[drone_id] = cb
scroll_layout.addWidget(cb)
scroll_layout.addStretch()
scroll.setWidget(scroll_widget)
layout.addWidget(scroll)
# 按鈕
btn_layout = QHBoxLayout()
ok_btn = QPushButton("確定")
ok_btn.setStyleSheet("""
QPushButton { background-color: #4A9EFF; color: white; border: none;
padding: 8px 20px; border-radius: 4px; font-weight: bold; }
QPushButton:hover { background-color: #3A8EEF; }
""")
ok_btn.clicked.connect(self.accept)
cancel_btn = QPushButton("取消")
cancel_btn.setStyleSheet("""
QPushButton { background-color: #555; color: #DDD; border: none;
padding: 8px 20px; border-radius: 4px; }
QPushButton:hover { background-color: #666; }
""")
cancel_btn.clicked.connect(self.reject)
btn_layout.addStretch()
btn_layout.addWidget(cancel_btn)
btn_layout.addWidget(ok_btn)
layout.addLayout(btn_layout)
def get_selected(self):
"""回傳勾選的無人機 ID set"""
return {did for did, cb in self.checkboxes.items() if cb.isChecked() and cb.isEnabled()}
class GroupPanel(QWidget):
"""單一群組的 UI 面板(嵌入到 tab 中)— 三欄式佈局"""
# 信號
assign_drones_requested = pyqtSignal(str) # group_id
mission_type_changed = pyqtSignal(str, str) # group_id, mission_type
start_requested = pyqtSignal(str) # group_id
pause_requested = pyqtSignal(str) # group_id
stop_requested = pyqtSignal(str) # group_id
mode_change_requested = pyqtSignal(str, str) # group_id, mode
arm_requested = pyqtSignal(str) # group_id
takeoff_requested = pyqtSignal(str, float) # group_id, altitude
box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
clear_group_requested = pyqtSignal(str) # group_id — 清除分組
add_group_requested = pyqtSignal() # 新增群組
delete_group_requested = pyqtSignal(str) # group_id — 刪除群組
BUTTON_STYLE = """
QPushButton {{ background-color: {bg}; color: {fg}; border: none;
padding: 5px 8px; border-radius: 4px; font-size: 11px; }}
QPushButton:hover {{ background-color: {hover}; }}
QPushButton:disabled {{ background-color: #444; color: #666; }}
"""
def __init__(self, group: MissionGroup, parent=None, default_params=None):
super().__init__(parent)
self.group = group
self.all_btn_ref = None # 保存全選按鈕的參考
self.default_params = dict(DEFAULT_MISSION_PARAM_VALUES)
if default_params:
self.default_params.update(default_params)
self._build_ui()
def _make_sep(self):
"""建立垂直分隔線"""
sep = QFrame()
sep.setFrameShape(QFrame.Shape.VLine)
sep.setStyleSheet("color: #444;")
return sep
def _build_ui(self):
layout = QVBoxLayout(self)
layout.setContentsMargins(6, 4, 6, 4)
layout.setSpacing(0)
COMBO = ("QComboBox { background-color: #333; color: #DDD; "
"border-radius: 3px; padding: 2px 6px; font-size: 11px; }")
BTN = self.BUTTON_STYLE
LBL = "color: #AAA; font-size: 11px;"
TITLE = "color: #DDD; font-size: 11px; font-weight: bold; padding-bottom: 2px;"
# ── 三欄主佈局 ──
cols = QHBoxLayout()
cols.setSpacing(6)
# ============================
# 左欄:控制指令
# ============================
left = QVBoxLayout()
left.setSpacing(3)
left_title = QLabel("控制指令")
left_title.setStyleSheet(TITLE)
left.addWidget(left_title)
# 模式切換
mode_row = QHBoxLayout()
mode_row.setSpacing(3)
self.mode_combo = QComboBox()
self.mode_combo.addItems([
"GUIDED", "AUTO", "LAND", "LOITER",
"STABILIZE", "ALT_HOLD", "RTL", "POSHOLD", "SMART_RTL"
])
self.mode_combo.setStyleSheet(COMBO)
mode_btn = QPushButton("切換")
mode_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
mode_btn.clicked.connect(
lambda: self.mode_change_requested.emit(
self.group.group_id, self.mode_combo.currentText()))
mode_row.addWidget(self.mode_combo, 1)
mode_row.addWidget(mode_btn)
left.addLayout(mode_row)
# 解鎖
arm_btn = QPushButton("解鎖")
arm_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
arm_btn.clicked.connect(
lambda: self.arm_requested.emit(self.group.group_id))
left.addWidget(arm_btn)
# 起飛
takeoff_row = QHBoxLayout()
takeoff_row.setSpacing(3)
self.alt_input = QComboBox()
self.alt_input.setEditable(True)
self.alt_input.addItems(["5", "10", "15", "20"])
self.alt_input.setCurrentText("10")
self.alt_input.setStyleSheet(COMBO)
alt_lbl = QLabel("m")
alt_lbl.setStyleSheet(LBL)
takeoff_btn = QPushButton("起飛")
takeoff_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
takeoff_btn.clicked.connect(self._on_takeoff)
takeoff_row.addWidget(self.alt_input, 1)
takeoff_row.addWidget(alt_lbl)
takeoff_row.addWidget(takeoff_btn)
left.addLayout(takeoff_row)
left.addStretch()
# ============================
# 中欄:任務規劃(左右分割)
# ============================
mid = QVBoxLayout()
mid.setSpacing(2)
mid_title = QLabel("任務規劃")
mid_title.setStyleSheet(TITLE)
mid.addWidget(mid_title)
mid_body = QHBoxLayout()
mid_body.setSpacing(4)
# 左側:類型 + 狀態 + 座標
mid_left = QVBoxLayout()
mid_left.setSpacing(2)
self.type_combo = QComboBox()
self.type_combo.addItems([
"M_FORMATION", "CIRCLE_FORMATION", "LEADER_FOLLOWER", "GRID_SWEEP"
])
self.type_combo.setStyleSheet(COMBO)
self.type_combo.currentTextChanged.connect(
lambda t: self.mission_type_changed.emit(self.group.group_id, t))
mid_left.addWidget(self.type_combo)
self.status_label = QLabel("○ 未規劃")
self.status_label.setStyleSheet("color: #888; font-size: 11px;")
mid_left.addWidget(self.status_label)
self.center_label = QLabel("中心: --")
self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
mid_left.addWidget(self.center_label)
self.target_label = QLabel("目標: --")
self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
mid_left.addWidget(self.target_label)
mid_left.addStretch()
# 右側:執行 / 暫停 / 停止(垂直排列)
mid_right = QVBoxLayout()
mid_right.setSpacing(3)
self.start_btn = QPushButton("▶ 執行")
self.start_btn.setStyleSheet(BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
self.start_btn.clicked.connect(
lambda: self.start_requested.emit(self.group.group_id))
self.pause_btn = QPushButton("⏸ 暫停")
self.pause_btn.setStyleSheet(BTN.format(bg='#F57F17', fg='white', hover='#F9A825'))
self.pause_btn.clicked.connect(
lambda: self.pause_requested.emit(self.group.group_id))
self.stop_btn = QPushButton("■ 停止")
self.stop_btn.setStyleSheet(BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
self.stop_btn.clicked.connect(
lambda: self.stop_requested.emit(self.group.group_id))
mid_right.addWidget(self.start_btn)
mid_right.addWidget(self.pause_btn)
mid_right.addWidget(self.stop_btn)
mid_right.addStretch()
mid_body.addLayout(mid_left, 1)
mid_body.addLayout(mid_right)
mid.addLayout(mid_body)
# ============================
# 選取與分組3x2 按鈕)
# ============================
right = QVBoxLayout()
right.setSpacing(3)
right_title = QLabel("選取與分組")
right_title.setStyleSheet(TITLE)
right.addWidget(right_title)
self.drone_list_label = QLabel("尚未分配")
self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
self.drone_list_label.setWordWrap(True)
right.addWidget(self.drone_list_label)
# 3x2 按鈕網格:第一行 框選 全選 新增群組
grid_r1 = QHBoxLayout()
grid_r1.setSpacing(3)
box_btn = QPushButton("框選")
box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
box_btn.clicked.connect(
lambda: self.box_select_requested.emit(self.group.group_id))
all_btn = QPushButton("全選/取消")
all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
all_btn.clicked.connect(self._on_all_select_clicked)
self.all_btn_ref = all_btn # 保存按鈕參考(備用)
add_group_btn = QPushButton("+ 新增群組")
add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
grid_r1.addWidget(box_btn)
grid_r1.addWidget(all_btn)
grid_r1.addWidget(add_group_btn)
right.addLayout(grid_r1)
# 第二行 編輯分配 清除分組 刪除群組
grid_r2 = QHBoxLayout()
grid_r2.setSpacing(3)
assign_btn = QPushButton("編輯分配")
assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
assign_btn.clicked.connect(
lambda: self.assign_drones_requested.emit(self.group.group_id))
clear_btn = QPushButton("清除分組")
clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
clear_btn.clicked.connect(
lambda: self.clear_group_requested.emit(self.group.group_id))
self.delete_group_btn = QPushButton("刪除群組")
self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
self.delete_group_btn.clicked.connect(
lambda: self.delete_group_requested.emit(self.group.group_id))
grid_r2.addWidget(assign_btn)
grid_r2.addWidget(clear_btn)
grid_r2.addWidget(self.delete_group_btn)
right.addLayout(grid_r2)
right.addStretch()
# ============================
# 第四欄:任務參數
# ============================
param_col = QVBoxLayout()
param_col.setSpacing(2)
param_title = QLabel("任務參數")
param_title.setStyleSheet(TITLE)
param_col.addWidget(param_title)
INPUT = ("QLineEdit { background-color: #333; color: #DDD; "
"border: 1px solid #555; border-radius: 3px; "
"padding: 1px 4px; font-size: 11px; }")
# 每種任務類型的參數定義: (key, label, default_value)
self._param_defs = {
'M_FORMATION': [
('spacing', '間距 (m)', self.default_params['spacing']),
('base_altitude', '基準高度 (m)', self.default_params['base_altitude']),
('altitude_diff', '高低差 (m)', self.default_params['altitude_diff']),
],
'CIRCLE_FORMATION': [
('radius', '半徑 (m)', self.default_params['radius']),
('altitude', '高度 (m)', self.default_params['altitude']),
('start_angle', '起始角 (°)', self.default_params['start_angle']),
],
'LEADER_FOLLOWER': [
('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
('altitude', '高度 (m)', self.default_params['altitude']),
],
'GRID_SWEEP': [
('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
('altitude', '高度 (m)', self.default_params['altitude']),
],
}
# 建立所有參數列的 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)
# LEADER_FOLLOWER 專用:領隊下拉選單
self._leader_row = QWidget()
leader_layout = QHBoxLayout(self._leader_row)
leader_layout.setContentsMargins(0, 0, 0, 0)
leader_layout.setSpacing(3)
leader_lbl = QLabel("領隊")
leader_lbl.setStyleSheet(LBL)
self._leader_combo = QComboBox()
self._leader_combo.setStyleSheet(COMBO)
self._leader_combo.setFixedWidth(70)
self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
leader_layout.addWidget(leader_lbl, 1)
leader_layout.addWidget(self._leader_combo)
param_col.addWidget(self._leader_row)
param_col.addStretch()
# 初始顯示對應的參數
self._update_param_visibility()
# 當任務類型切換時更新參數顯示
self.type_combo.currentTextChanged.connect(self._update_param_visibility)
# ── 組裝四欄:控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
# 使用伸展因子 0 讓列根據內容自動調整寬度,而不是均等分配
cols.addLayout(left, 0)
cols.addWidget(self._make_sep())
cols.addLayout(mid, 0)
cols.addWidget(self._make_sep())
cols.addLayout(param_col, 0)
cols.addWidget(self._make_sep())
cols.addLayout(right, 0)
cols.addStretch() # 填充剩餘空間,使四列置左
layout.addLayout(cols)
def update_drone_list(self):
"""更新無人機列表顯示"""
if not self.group.selected_drone_ids:
self.drone_list_label.setText("尚未分配")
self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
else:
sorted_ids = sorted(self.group.selected_drone_ids,
key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
self.drone_list_label.setText("".join(sorted_ids))
self.drone_list_label.setStyleSheet(
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
self._refresh_leader_options()
def _refresh_leader_options(self):
"""依目前群組成員重新填充領隊下拉選單,保留目前選擇若仍有效"""
sorted_ids = sorted(self.group.selected_drone_ids,
key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
current = self.group.leader_drone_id
self._leader_combo.blockSignals(True)
self._leader_combo.clear()
self._leader_combo.addItems(sorted_ids)
if current and current in sorted_ids:
self._leader_combo.setCurrentText(current)
elif sorted_ids:
self._leader_combo.setCurrentText(sorted_ids[0])
self.group.leader_drone_id = sorted_ids[0]
else:
self.group.leader_drone_id = None
self._leader_combo.blockSignals(False)
def _on_leader_changed(self, drone_id):
self.group.leader_drone_id = drone_id if drone_id else None
def update_status(self):
"""更新任務狀態顯示"""
state = self.group.state
if self.group.planned_waypoints is None:
self.status_label.setText("○ 未規劃")
self.status_label.setStyleSheet("color: #888; font-size: 11px;")
elif state == MissionState.RUNNING:
self.status_label.setText("▶ 執行中")
self.status_label.setStyleSheet("color: #4CAF50; font-size: 11px; font-weight: bold;")
elif state == MissionState.PAUSED:
self.status_label.setText("⏸ 已暫停")
self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
else:
n = len(self.group.selected_drone_ids)
total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
self.status_label.setStyleSheet(
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
def _on_all_select_clicked(self):
"""全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
self.select_all_requested.emit(self.group.group_id)
def set_delete_enabled(self, enabled):
"""啟用或禁用刪除群組按鈕"""
self.delete_group_btn.setEnabled(enabled)
def _update_param_visibility(self, _=None):
"""根據當前任務類型,顯示/隱藏對應的參數列"""
mission_type = self.type_combo.currentText()
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)
self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
def get_mission_params(self):
"""讀取當前顯示的參數值,回傳 dict"""
mission_type = self.type_combo.currentText()
params = {}
for key, _label, default in self._param_defs.get(mission_type, []):
if key in self._param_widgets:
_row_w, inp = self._param_widgets[key]
try:
params[key] = float(inp.text())
except ValueError:
params[key] = float(default)
return params
def set_param_value(self, key, value):
"""更新指定參數欄位的文字值。"""
if key not in self._param_widgets:
return
_row_w, inp = self._param_widgets[key]
inp.setText(str(value))
def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
"""更新中心點 / 目標點顯示"""
info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
self.center_label.setStyleSheet(info_style)
self.target_label.setText(f"目標: {target_lat:.6f}°, {target_lon:.6f}°")
self.target_label.setStyleSheet(info_style)
def clear_mission_info(self):
"""清除中心點 / 目標點顯示"""
self.center_label.setText("中心: --")
self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
self.target_label.setText("目標: --")
self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
def _on_takeoff(self):
try:
alt = float(self.alt_input.currentText())
except ValueError:
alt = 10.0
self.takeoff_requested.emit(self.group.group_id, alt)

@ -1,703 +0,0 @@
#!/usr/bin/env python3
"""
飛行任務規劃模組
支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
"""
import math
from typing import List, Tuple, Optional, Dict, Any
from enum import Enum
class MissionType(Enum):
"""任務類型"""
M_FORMATION = "m_formation"
CIRCLE_FORMATION = "circle_formation"
LEADER_FOLLOWER = "leader_follower"
GRID_SWEEP = "grid_sweep"
class CoordinateConverter:
"""GPS 座標與 Local 座標的轉換器"""
def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
self.origin_lat = origin_lat
self.origin_lon = origin_lon
self.origin_alt = origin_alt
self.R = 6371000.0
def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
lat_rad = math.radians(lat)
lon_rad = math.radians(lon)
origin_lat_rad = math.radians(self.origin_lat)
origin_lon_rad = math.radians(self.origin_lon)
dlat = lat_rad - origin_lat_rad
dlon = lon_rad - origin_lon_rad
x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
y = self.R * dlat
z = alt - self.origin_alt
return x, y, z
def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
origin_lat_rad = math.radians(self.origin_lat)
origin_lon_rad = math.radians(self.origin_lon)
lat_rad = origin_lat_rad + (y / self.R)
lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
lat = math.degrees(lat_rad)
lon = math.degrees(lon_rad)
alt = self.origin_alt + z
return lat, lon, alt
class FormationPlanner:
"""隊形規劃器"""
def __init__(self, spacing: float = 5.0,
base_altitude: float = 10.0,
altitude_diff: float = 2.0):
self.spacing = spacing
self.base_altitude = base_altitude
self.altitude_diff = altitude_diff
self.current_origin = None
self.converter = None
def plan_formation_mission(self,
drone_gps_positions: List[Tuple[float, float, float]],
target_gps: Tuple[float, float, float],
mission_type: MissionType = MissionType.M_FORMATION,
params: Optional[Dict[str, Any]] = None
) -> Tuple[List[List[Tuple[float, float, float]]],
Tuple[float, float, float],
List[int]]:
"""
回傳 (waypoints_gps, origin, rendezvous_indices)
rendezvous_indices航點序列裡該群組需要等全員到齊才推進的 index 清單
"""
if len(drone_gps_positions) == 0:
raise ValueError("無人機位置列表不能為空")
center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
self.current_origin = (center_lat, center_lon, center_alt)
self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
target_local = self.converter.gps_to_local(*target_gps)
if mission_type == MissionType.M_FORMATION:
s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
# 起點集合後一起出發到 stage2
rendezvous_indices = [0]
elif mission_type == MissionType.CIRCLE_FORMATION:
s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
# 半程集合後一起進最終圓環位置
rendezvous_indices = [0]
elif mission_type == MissionType.LEADER_FOLLOWER:
params = params or {}
route_wps_gps = params.get('route_waypoints')
if route_wps_gps is None or len(route_wps_gps) < 2:
raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
route_wps_local = [
self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
for wp in route_wps_gps
]
waypoints_local, rendezvous_indices = self._calculate_leader_follower(
drone_local, route_wps_local, params
)
elif mission_type == MissionType.GRID_SWEEP:
params = params or {}
rect_corners_gps = params.get('rect_corners')
if rect_corners_gps is None or len(rect_corners_gps) != 4:
raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
rect_corners_local = [
self.converter.gps_to_local(c[0], c[1], 0)[:2]
for c in rect_corners_gps
]
waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
drone_local, rect_corners_local, params
)
else:
raise ValueError(f"不支援的任務類型: {mission_type}")
waypoints_gps = []
for drone_wps in waypoints_local:
gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
waypoints_gps.append(gps_wps)
return waypoints_gps, self.current_origin, rendezvous_indices
# ------------------------------------------------------------------ M-Formation
def _calculate_m_formation(self, drone_positions, target_point, params):
params = params or {}
N = len(drone_positions)
spacing = params.get('spacing', self.spacing)
base_altitude = params.get('base_altitude', self.base_altitude)
altitude_diff = params.get('altitude_diff', self.altitude_diff)
C_x = sum(pos[0] for pos in drone_positions) / N
C_y = sum(pos[1] for pos in drone_positions) / N
T_x, T_y, T_z = target_point
V_x, V_y = T_x - C_x, T_y - C_y
P_x, P_y = -V_y, V_x
length = math.sqrt(P_x ** 2 + P_y ** 2)
P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
for i, pos in enumerate(drone_positions)]
projections.sort()
stage1_positions = [None] * N
stage2_positions = [None] * N
for rank, (_, original_idx) in enumerate(projections):
offset = (rank - (N - 1) / 2) * spacing
altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
return stage1_positions, stage2_positions
# ------------------------------------------------------------------ Circle
def _calculate_circle_formation(self, drone_positions, target_point, params):
params = params or {}
N = len(drone_positions)
radius = params.get('radius', 10.0)
altitude = params.get('altitude', 10.0)
start_angle = params.get('start_angle', 0.0)
center_x, center_y, center_z = target_point
stage1_positions = []
stage2_positions = []
angle_step = 360.0 / N
for i in range(N):
angle_rad = math.radians(start_angle + angle_step * i)
final_x = center_x + radius * math.cos(angle_rad)
final_y = center_y + radius * math.sin(angle_rad)
final_z = altitude
current_x, current_y, current_z = drone_positions[i]
stage1_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
# ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
"""
路徑跟隨編隊 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
核心想法
- center_path 建好後計算每個 sample 的累積弧長 s
- 每架 drone (lat_k, lon_k) 的隊形偏移
- leader 想成沿 s 參數化的點follower k 的目標 s = s_leader - lon_k
- follower k i waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
- 超過 path 起點/終點以 clip 處理避免倒退或衝出
解決前版空間偏移的三個 bug
1. 起點暴衝s<0 clip follower 起點就是 start 加橫向偏移不往後倒退
2. 弧段角度爆炸lon 不再換算成弧度直接沿 polyline lookup
3. straight/arc 切換不連續統一走 polyline 線性內插與 segment 切線
Rank 定序規則
** drone_positions 的輸入順序為準**= GUI 選取順序
drone_positions[0] = rank 0 = leader (lon=0)
drone_positions[1] = rank 1 (lon=5)依此類推
刻意用位置投影自動排序避免浮點噪音造成 run-to-run
leader 漂移以確保整個 mission 像軍隊縱隊那樣順序固定
隊形 (俯視, 以路徑行進方向為前):
D0 (lat=-L, lon=0) front-right (leader)
D1 (lat=+L, lon=5)
D2 (lat=-L, lon=10)
D3 (lat=+L, lon=15)
"""
N = len(drone_positions)
lateral_offset = params.get('lateral_offset', 3.0)
longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
altitude = params.get('altitude', self.base_altitude)
min_inner_radius = params.get('min_inner_radius', 3.0)
arc_resolution = params.get('arc_resolution', 12)
sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0) # cos(90°)=0
# Step 1: 建立中心路徑(含圓弧、銳角單點;每點帶累積 s
center_path, barrier_indices = self._build_center_path(
route_wps_local,
formation_max_lateral=lateral_offset,
min_inner_radius=min_inner_radius,
arc_resolution=arc_resolution,
sharp_turn_cos_threshold=sharp_turn_cos,
)
s_list = [pt['s'] for pt in center_path]
s_max = s_list[-1]
n_pts = len(center_path)
def lookup(s_target):
"""
center path 上以弧長 s 回傳 (x, y, tangent_dir)
s<0 clip starts>s_max clip end
tangent 方向取當前 polyline segment 的切線避免對不連續的 dir 做插值
"""
if n_pts == 1:
pt = center_path[0]
return pt['x'], pt['y'], pt['dir']
if s_target <= 0.0:
a = center_path[0]
b = center_path[1]
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
return a['x'], a['y'], (sdx / slen, sdy / slen)
return a['x'], a['y'], a['dir']
if s_target >= s_max:
a = center_path[-2]
b = center_path[-1]
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
return b['x'], b['y'], (sdx / slen, sdy / slen)
return b['x'], b['y'], b['dir']
# Binary searchs_list[lo] <= s_target < s_list[hi]
lo, hi = 0, n_pts - 1
while lo + 1 < hi:
mid = (lo + hi) // 2
if s_list[mid] <= s_target:
lo = mid
else:
hi = mid
a = center_path[lo]
b = center_path[hi]
ds = s_list[hi] - s_list[lo]
if ds < 1e-9:
return a['x'], a['y'], a['dir']
t = (s_target - s_list[lo]) / ds
x = a['x'] + t * (b['x'] - a['x'])
y = a['y'] + t * (b['y'] - a['y'])
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
return x, y, (sdx / slen, sdy / slen)
return x, y, a['dir']
# Step 2: rank = 輸入順序GUI 選取順序),不再做 projection sort
# 避免浮點噪音在投影值相近時翻轉 leader保證 run-to-run 穩定
all_waypoints = [None] * N
# 預先算好路徑終點的切線(給收尾點用)
end = center_path[-1]
end_dx, end_dy = end['dir']
if n_pts >= 2:
a = center_path[-2]
sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
end_dx, end_dy = sdx / slen, sdy / slen
for rank in range(N):
lat_sign = -1 if rank % 2 == 0 else 1
lat = lat_sign * lateral_offset
lon = rank * longitudinal_spacing
waypoints = []
for i in range(n_pts):
s_follower = s_list[i] - lon
x, y, (dx, dy) = lookup(s_follower)
perp_x, perp_y = -dy, dx
waypoints.append((
x + lat * perp_x,
y + lat * perp_y,
altitude,
))
# 收尾:全員到 path 終點的 rank 位置lon 歸零)
waypoints.append((
end['x'] + lat * (-end_dy),
end['y'] + lat * end_dx,
altitude,
))
all_waypoints[rank] = waypoints
# barrier 索引仍指向 center_path 內 (range [0, n_pts-1])
# 不觸及末尾收尾點,直接沿用即可
return all_waypoints, barrier_indices
def _build_center_path(self, waypoints,
formation_max_lateral,
min_inner_radius,
arc_resolution,
sharp_turn_cos_threshold):
"""
建立以圓弧連接直線段的中心路徑
每個中間 waypoint 的處理
1. 計算入向 u_in出向 u_out 的夾角 β = acos(u_in·u_out)
2. cos β < sharp_turn_cos_threshold 銳角只插單點 (hover + 原地轉向)
barrier 放在該點讓隊伍整體停下再繼續
3. 否則 平滑弧
R_base = formation_max_lateral + min_inner_radius
d = R_base × tan(β/2)並以相鄰段長的 45% 為上限
R_actual = d / tan(β/2)
tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
arc center = T_in + R_actual·sign·n_left其中 sign=+1 左轉/CCW-1 右轉/CW
arc theta_in theta_out arc_resolution 等分
path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
barrier 放在 T_out (轉完彎後的集合點)
path 是一個 list[dict]每個 dict 至少含:
{'seg': 'straight' | 'arc',
'x': float, 'y': float,
'dir': (dx, dy)}
'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
Returns:
(path, barrier_indices)
"""
num_wps = len(waypoints)
if num_wps < 2:
return [{
'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': (0.0, 1.0),
}], []
segments = []
for i in range(num_wps - 1):
dx = waypoints[i + 1][0] - waypoints[i][0]
dy = waypoints[i + 1][1] - waypoints[i][1]
length = math.hypot(dx, dy)
if length < 0.01:
segments.append(((0.0, 1.0), length))
else:
segments.append(((dx / length, dy / length), length))
R_base = formation_max_lateral + min_inner_radius
path = []
barrier_indices = []
# 起點
path.append({
'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': segments[0][0],
})
for i in range(1, num_wps - 1):
u_in, len_in = segments[i - 1]
u_out, len_out = segments[i]
wp = waypoints[i]
dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
dot = max(-1.0, min(1.0, dot))
# 銳角hover + 原地轉向
if dot < sharp_turn_cos_threshold:
avg_dx = u_in[0] + u_out[0]
avg_dy = u_in[1] + u_out[1]
avg_len = math.hypot(avg_dx, avg_dy)
if avg_len > 0.01:
avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
else:
avg_dir = u_in
path.append({
'seg': 'straight',
'x': wp[0], 'y': wp[1],
'dir': avg_dir,
})
barrier_indices.append(len(path) - 1)
continue
# 平滑弧
beta = math.acos(dot)
if beta < 1e-4:
# 幾乎直線,不插弧
path.append({
'seg': 'straight',
'x': wp[0], 'y': wp[1],
'dir': u_in,
})
continue
half_tan = math.tan(beta / 2.0)
d_ideal = R_base * half_tan
d_max = 0.45 * min(len_in, len_out)
d = min(d_ideal, d_max)
R_actual = d / half_tan
T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
# +1 CCW (左轉) / -1 CW (右轉)
cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
sign = 1 if cross > 0 else -1
# 入向左法線
n_left = (-u_in[1], u_in[0])
center = (
T_in[0] + R_actual * sign * n_left[0],
T_in[1] + R_actual * sign * n_left[1],
)
theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
# T_in (straight, 方向 u_in)
path.append({
'seg': 'straight',
'x': T_in[0], 'y': T_in[1],
'dir': u_in,
})
# 弧段採樣 k=1..arc_resolution-1不含兩端
for k in range(1, arc_resolution):
t = k / arc_resolution
theta = theta_in + sign * beta * t
px = center[0] + R_actual * math.cos(theta)
py = center[1] + R_actual * math.sin(theta)
# 切線: sign × (-sin θ, cos θ)
tx = sign * (-math.sin(theta))
ty = sign * math.cos(theta)
path.append({
'seg': 'arc',
'x': px, 'y': py,
'dir': (tx, ty),
'arc_center': center,
'arc_R': R_actual,
'arc_sign': sign,
'theta': theta,
})
# T_out (straight, 方向 u_out)barrier 放這
path.append({
'seg': 'straight',
'x': T_out[0], 'y': T_out[1],
'dir': u_out,
})
barrier_indices.append(len(path) - 1)
# 終點
path.append({
'seg': 'straight',
'x': waypoints[-1][0], 'y': waypoints[-1][1],
'dir': segments[-1][0],
})
# 後處理:為每個 sample 加上累積 polyline 弧長 s
path[0]['s'] = 0.0
for i in range(1, len(path)):
prev = path[i - 1]
cur = path[i]
cur['s'] = prev['s'] + math.hypot(
cur['x'] - prev['x'], cur['y'] - prev['y']
)
return path, barrier_indices
# ------------------------------------------------------------------ 柵狀掃描
def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
"""柵狀掃描:掃描方向沿矩形長邊,切割方向沿短邊"""
N = len(drone_positions)
line_spacing = params.get('line_spacing', 5.0)
altitude = params.get('altitude', self.base_altitude)
c0, c1, c2, c3 = rect_corners_local
edge_a = (c1[0] - c0[0], c1[1] - c0[1])
len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
edge_b = (c3[0] - c0[0], c3[1] - c0[1])
len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)
if len_a >= len_b:
sweep_vec = edge_a
sweep_len = len_a
cut_vec = edge_b
cut_len = len_b
sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
cut_start_corner = c0
else:
sweep_vec = edge_b
sweep_len = len_b
cut_vec = edge_a
cut_len = len_a
sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
cut_start_corner = c0
sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)
C_x = sum(p[0] for p in drone_positions) / N
C_y = sum(p[1] for p in drone_positions) / N
dist_to_start = math.sqrt(
(C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
)
dist_to_end = math.sqrt(
(C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
)
if dist_to_start <= dist_to_end:
near_corner_a = cut_start_corner
else:
sweep_dir = (-sweep_dir[0], -sweep_dir[1])
near_corner_a = (cut_start_corner[0] + sweep_vec[0],
cut_start_corner[1] + sweep_vec[1])
projections = [
((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
for i, pos in enumerate(drone_positions)
]
projections.sort()
strip_width = cut_len / N
drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2
all_waypoints = [None] * N
for rank, (_, original_idx) in enumerate(projections):
strip_center_offset = (rank + 0.5) * strip_width
base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset
waypoints_list = []
gather_offset = gather_sweep_proj - near_sweep_proj
gx = base_x + sweep_dir[0] * gather_offset
gy = base_y + sweep_dir[1] * gather_offset
waypoints_list.append((gx, gy, altitude))
num_lines = max(1, int(strip_width / line_spacing))
actual_spacing = strip_width / num_lines
first_line_offset = (rank * strip_width) + actual_spacing / 2
entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
waypoints_list.append((entry_x, entry_y, altitude))
current_cut_offset = first_line_offset
for line_idx in range(num_lines):
line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
line_far_x = line_near_x + sweep_dir[0] * sweep_len
line_far_y = line_near_y + sweep_dir[1] * sweep_len
if line_idx % 2 == 0:
waypoints_list.append((line_far_x, line_far_y, altitude))
else:
waypoints_list.append((line_near_x, line_near_y, altitude))
if line_idx < num_lines - 1:
next_cut_offset = current_cut_offset + actual_spacing
next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
next_far_x = next_near_x + sweep_dir[0] * sweep_len
next_far_y = next_near_y + sweep_dir[1] * sweep_len
if line_idx % 2 == 0:
waypoints_list.append((next_far_x, next_far_y, altitude))
else:
waypoints_list.append((next_near_x, next_near_y, altitude))
current_cut_offset = next_cut_offset
all_waypoints[original_idx] = waypoints_list
# 每一條掃描線的「起點」都做一次同步:
# wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
# wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
# 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1i 從 0 開始)
# 所有 drone 的 waypoint 數量相同num_lines 對所有 strip 都一致),
# 所以用同一份 rendezvous_indices
rendezvous_indices = [2 * i + 1 for i in range(num_lines)]
return all_waypoints, rendezvous_indices
# ================================================================================
# 測試
# ================================================================================
if __name__ == "__main__":
planner = FormationPlanner()
drones = [
(24.123450, 120.567800, 0.0),
(24.123470, 120.567820, 0.0),
(24.123440, 120.567810, 0.0),
(24.123460, 120.567830, 0.0),
]
target = (24.12400, 120.56795, 10.0)
# M-Formation
wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
print("M-Formation:")
for i, wp_list in enumerate(wps):
print(f" Drone {i}: {len(wp_list)} waypoints")
# Leader-Follower (虛擬領隊 / 弧長參數化)
route = [
(24.12345, 120.56780),
(24.12370, 120.56800),
(24.12390, 120.56820),
(24.12400, 120.56800),
(24.12420, 120.56790),
]
wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
drones, target, MissionType.LEADER_FOLLOWER,
params={
'route_waypoints': route,
'lateral_offset': 3.0,
'longitudinal_spacing': 5.0,
'min_inner_radius': 3.0,
'arc_resolution': 8,
'altitude': 10.0
}
)
print(f"\nLeader-Follower (arc-length virtual leader):")
for i, wp_list in enumerate(wps_lf):
print(f" Drone {i}: {len(wp_list)} waypoints")
for j, wp in enumerate(wp_list):
print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
# Grid Sweep
rect = [
(24.1237, 120.5679),
(24.1237, 120.5683),
(24.1240, 120.5683),
(24.1240, 120.5679)
]
wps2, origin2, rv2 = planner.plan_formation_mission(
drones, target, MissionType.GRID_SWEEP,
params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
)
print(f"\nGrid Sweep:")
for i, wp_list in enumerate(wps2):
print(f" Drone {i}: {len(wp_list)} waypoints")

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

@ -1,299 +0,0 @@
#!/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()

@ -1,482 +0,0 @@
#!/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()

@ -1 +0,0 @@
Subproject commit a96224f9ab3ac51fe8fd981c1e1554528dc4345a

@ -1 +0,0 @@
Subproject commit 24806adc767414eb3a34a58aefeb648ee415b09a

File diff suppressed because it is too large Load Diff

@ -1,208 +0,0 @@
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:

@ -1,145 +0,0 @@
//
// 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_

@ -1,8 +0,0 @@
# 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'

@ -1,66 +0,0 @@
# 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

@ -1,16 +0,0 @@
# 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

@ -1,12 +0,0 @@
# 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

@ -1,17 +0,0 @@
# 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

@ -1,4 +0,0 @@
# 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

@ -1,11 +0,0 @@
# 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.

@ -1,9 +0,0 @@
#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

@ -1,200 +0,0 @@
# 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)

@ -1,19 +0,0 @@
# 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

@ -1,26 +0,0 @@
# 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

@ -1,14 +0,0 @@
# 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

@ -1,12 +0,0 @@
# 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

@ -1,9 +0,0 @@
# 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

@ -1,11 +0,0 @@
# 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

@ -1,10 +0,0 @@
# 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

@ -1,15 +0,0 @@
# 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

@ -1,23 +0,0 @@
# 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

@ -1,19 +0,0 @@
# 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

@ -1,12 +0,0 @@
# 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

@ -1,37 +0,0 @@
# 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.

@ -1,37 +0,0 @@
# 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

@ -1,18 +0,0 @@
# 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.

@ -1,32 +0,0 @@
# 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.

@ -1,34 +0,0 @@
# 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)

@ -1,18 +0,0 @@
# 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.

@ -1,29 +0,0 @@
# 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)

@ -1,24 +0,0 @@
# 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.

@ -1,27 +0,0 @@
# 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).

@ -1,19 +0,0 @@
# 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.

@ -1,34 +0,0 @@
# 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

@ -1,10 +0,0 @@
# 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

@ -1,18 +0,0 @@
# 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

@ -1,17 +0,0 @@
# 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

@ -1,16 +0,0 @@
# 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

@ -1,15 +0,0 @@
# 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

@ -1,10 +0,0 @@
# 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

@ -1,32 +0,0 @@
# 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

@ -1,11 +0,0 @@
# 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

@ -1,15 +0,0 @@
# 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

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

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

@ -1,38 +0,0 @@
# 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

@ -1,18 +0,0 @@
# 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.

@ -1,12 +0,0 @@
# 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

@ -1,25 +0,0 @@
# 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.

@ -1,9 +0,0 @@
# 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

@ -1,14 +0,0 @@
# 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

@ -1,9 +0,0 @@
# 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

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

@ -1,14 +0,0 @@
# 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

@ -1,12 +0,0 @@
# 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

@ -1,10 +0,0 @@
# 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

@ -1,32 +0,0 @@
# 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

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

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

@ -1,6 +0,0 @@
# 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

@ -1,23 +0,0 @@
# 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

@ -1,16 +0,0 @@
# 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

@ -1,82 +0,0 @@
# 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

@ -1,19 +0,0 @@
# 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

@ -1,17 +0,0 @@
# 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

@ -1,15 +0,0 @@
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

@ -1,12 +0,0 @@
# 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

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

@ -1,7 +0,0 @@
# 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)

@ -1,19 +0,0 @@
# 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

@ -1,27 +0,0 @@
# 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)

@ -1,31 +0,0 @@
# 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

@ -1,11 +0,0 @@
# 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

@ -1,7 +0,0 @@
# 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

@ -1,45 +0,0 @@
# 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

@ -1,9 +0,0 @@
# 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

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

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

@ -1,46 +0,0 @@
<?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>

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

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

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

Some files were not shown because too many files have changed in this diff Show More

Loading…
Cancel
Save