chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: chiyu1468/AirTrapMine:4e08b18a037f0e48f2e7669d1acd297a3cae4c18
Branches Tags
chiyu1468/AirTrapMine:master chiyu1468/AirTrapMine:wenchun chiyu1468/AirTrapMine:chiyu chiyu1468/AirTrapMine:chiyuTemp chiyu1468/AirTrapMine:lunu chiyu1468/AirTrapMine:ken
..
compare: chiyu1468/AirTrapMine:lunu
Branches Tags
chiyu1468/AirTrapMine:master chiyu1468/AirTrapMine:wenchun chiyu1468/AirTrapMine:chiyu chiyu1468/AirTrapMine:chiyuTemp chiyu1468/AirTrapMine:lunu chiyu1468/AirTrapMine:ken
4 Commits
4e08b18a03 .. lunu
Author SHA1 Message Date
lenting89 5075d99ad6 學長建議修改內容 2026-06-16 15:09:29 +08:00
lenting89 3e6490d07f 處理一開始disc找不到飛機的問題 2026-06-08 18:37:20 +08:00
lenting89 ea7774eb9e esp32 不寫死sysid 2026-06-08 17:16:35 +08:00
lenting89 0e7070b412 update files 2026-06-03 14:12:12 +08:00
37 changed files with 2513 additions and 3593 deletions
Expand all files Collapse all files
README.md
+16 -21
View File
@@ -73,30 +73,25 @@ python gui.py
===
## 資料夾說明
1. fc_network_adapter (核心)
建立、維護與飛控韌體的連接
構築 mavlink 封包
處理無線模組的通訊格式 (XBee)
--同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
2. fc_interfaces (重要)
自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了
3. fc_network_module (重要)
非核心 但是支援載具的重要附屬功能
需要該功能時 作為一個 ros2 節點打開
例如 : ntrip rtk 訊號轉接
4. fc_network_apps
與 fc_network_adapter 銜接做高階功能包裝的應用小程式
利於開發GUI或其他應用 使用者的外層包裝
這裡的定位是 "核心功能的高階包裝" 可以完全不去用
5. someotherpkg
如何使用 fc_network_apps 的範例檔案
6. GUI
由 PyQt6 開發的互動式介面
7. unitdev 為各自協作者做開發時的測試區
1. unitdev 為各自協作者做開發時的測試區
01 -> 晉凱(ken)
02 -> 其宇(chiyu)
03 -> 文鈞
04 -> 倫渝
8. logs 是執行時期的記錄檔
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 是執行時期的記錄檔
===
src/GUI/comm_panel.py
+38 -125
View File
@@ -1,78 +1,10 @@
#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
QPushButton, QLineEdit, QComboBox, QApplication)
from PyQt6.QtGui import QFont
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
QPushButton, QLineEdit, QComboBox)
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):
"""通讯设置面板类"""
@@ -94,7 +26,6 @@ class CommPanel(QWidget):
self.ws_connections = []
self.serial_connections = []
self._init_ui()
self.apply_font_scale()
def _init_ui(self):
"""初始化UI"""
@@ -104,7 +35,7 @@ class CommPanel(QWidget):
# ========== UDP MAVLink 區域 ==========
udp_title = QLabel("UDP")
_set_scaled_stylesheet(udp_title, """
udp_title.setStyleSheet("""
color: #DDD;
font-size: 14px;
font-weight: bold;
@@ -129,7 +60,7 @@ class CommPanel(QWidget):
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, """
self.udp_ip_input.setStyleSheet("""
QLineEdit {
background-color: #333;
color: #DDD;
@@ -143,7 +74,7 @@ class CommPanel(QWidget):
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, """
self.udp_port_input.setStyleSheet("""
QLineEdit {
background-color: #333;
color: #DDD;
@@ -155,7 +86,7 @@ class CommPanel(QWidget):
add_udp_btn = QPushButton("添加")
add_udp_btn.clicked.connect(self._handle_add_udp)
_set_scaled_stylesheet(add_udp_btn, """
add_udp_btn.setStyleSheet("""
QPushButton {
background-color: #4CAF50;
color: white;
@@ -167,13 +98,9 @@ class CommPanel(QWidget):
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(QLabel("IP:", styleSheet="color: #DDD;"))
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(QLabel("Port:", styleSheet="color: #DDD;"))
add_udp_layout.addWidget(self.udp_port_input)
add_udp_layout.addWidget(add_udp_btn)
@@ -186,7 +113,7 @@ class CommPanel(QWidget):
# ========== Serial 區域 ==========
serial_title = QLabel("Serial")
_set_scaled_stylesheet(serial_title, """
serial_title.setStyleSheet("""
color: #DDD;
font-size: 14px;
font-weight: bold;
@@ -209,7 +136,7 @@ class CommPanel(QWidget):
add_serial_layout.setContentsMargins(0, 0, 0, 0)
self.serial_port_combo = QComboBox()
_set_scaled_stylesheet(self.serial_port_combo, """
self.serial_port_combo.setStyleSheet("""
QComboBox {
background-color: #333;
color: #DDD;
@@ -233,7 +160,7 @@ class CommPanel(QWidget):
refresh_ports_btn.setFixedWidth(35)
refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
refresh_ports_btn.setToolTip("重新掃描串口")
_set_scaled_stylesheet(refresh_ports_btn, """
refresh_ports_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
@@ -249,7 +176,7 @@ class CommPanel(QWidget):
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, """
self.serial_baudrate_combo.setStyleSheet("""
QComboBox {
background-color: #333;
color: #DDD;
@@ -270,7 +197,7 @@ class CommPanel(QWidget):
add_serial_btn = QPushButton("添加")
add_serial_btn.clicked.connect(self._handle_add_serial)
_set_scaled_stylesheet(add_serial_btn, """
add_serial_btn.setStyleSheet("""
QPushButton {
background-color: #4CAF50;
color: white;
@@ -282,14 +209,10 @@ class CommPanel(QWidget):
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(QLabel("Port:", styleSheet="color: #DDD;"))
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(QLabel("Baud:", styleSheet="color: #DDD;"))
add_serial_layout.addWidget(self.serial_baudrate_combo)
add_serial_layout.addWidget(add_serial_btn)
@@ -302,7 +225,7 @@ class CommPanel(QWidget):
# ========== WebSocket 區域 ==========
ws_title = QLabel("WebSocket")
_set_scaled_stylesheet(ws_title, """
ws_title.setStyleSheet("""
color: #DDD;
font-size: 14px;
font-weight: bold;
@@ -326,7 +249,7 @@ class CommPanel(QWidget):
self.ws_url_input = QLineEdit()
self.ws_url_input.setPlaceholderText("host")
_set_scaled_stylesheet(self.ws_url_input, """
self.ws_url_input.setStyleSheet("""
QLineEdit {
background-color: #333;
color: #DDD;
@@ -338,7 +261,7 @@ class CommPanel(QWidget):
add_ws_btn = QPushButton("添加")
add_ws_btn.clicked.connect(self._handle_add_ws)
_set_scaled_stylesheet(add_ws_btn, """
add_ws_btn.setStyleSheet("""
QPushButton {
background-color: #4CAF50;
color: white;
@@ -350,9 +273,7 @@ class CommPanel(QWidget):
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(QLabel("URL:", styleSheet="color: #DDD;"))
add_ws_layout.addWidget(self.ws_url_input)
add_ws_layout.addWidget(add_ws_btn)
@@ -516,7 +437,7 @@ class CommPanel(QWidget):
def create_udp_connection_panel(self, conn):
"""創建 UDP 連接面板"""
panel = QWidget()
_set_scaled_stylesheet(panel, """
panel.setStyleSheet("""
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
@@ -530,22 +451,22 @@ class CommPanel(QWidget):
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
_set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
info_label.setStyleSheet("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.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
_set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
status_label.setStyleSheet("color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
# 控制按鈕
toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
toggle_btn.setFixedWidth(60)
toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label))
_set_scaled_stylesheet(toggle_btn, """
toggle_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
@@ -560,7 +481,7 @@ class CommPanel(QWidget):
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
_set_scaled_stylesheet(remove_btn, """
remove_btn.setStyleSheet("""
QPushButton {
background-color: #d32f2f;
color: white;
@@ -588,7 +509,7 @@ class CommPanel(QWidget):
def create_ws_connection_panel(self, conn):
"""創建 WebSocket 連接面板"""
panel = QWidget()
_set_scaled_stylesheet(panel, """
panel.setStyleSheet("""
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
@@ -602,22 +523,22 @@ class CommPanel(QWidget):
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['url']}")
_set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
info_label.setStyleSheet("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.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
_set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
status_label.setStyleSheet("color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
# 控制按鈕
toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
toggle_btn.setFixedWidth(60)
toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label))
_set_scaled_stylesheet(toggle_btn, """
toggle_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
@@ -632,7 +553,7 @@ class CommPanel(QWidget):
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
_set_scaled_stylesheet(remove_btn, """
remove_btn.setStyleSheet("""
QPushButton {
background-color: #d32f2f;
color: white;
@@ -684,7 +605,7 @@ class CommPanel(QWidget):
def create_serial_connection_panel(self, conn):
"""創建 Serial 連接面板"""
panel = QWidget()
_set_scaled_stylesheet(panel, """
panel.setStyleSheet("""
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
@@ -698,22 +619,22 @@ class CommPanel(QWidget):
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
_set_scaled_stylesheet(info_label, "color: #DDD; font-size: 12px;")
info_label.setStyleSheet("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.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
_set_scaled_stylesheet(status_label, "color: #888; font-size: 16px;")
status_label.setStyleSheet("color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
# 控制按鈕
toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
toggle_btn.setFixedWidth(60)
toggle_btn.clicked.connect(lambda: self.serial_connection_toggled.emit(conn, toggle_btn, status_label))
_set_scaled_stylesheet(toggle_btn, """
toggle_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
@@ -728,7 +649,7 @@ class CommPanel(QWidget):
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
_set_scaled_stylesheet(remove_btn, """
remove_btn.setStyleSheet("""
QPushButton {
background-color: #d32f2f;
color: white;
@@ -763,12 +684,4 @@ class CommPanel(QWidget):
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)
self.serial_connections.remove(conn)
src/GUI/command_sender.py
+3 -7
View File
@@ -16,10 +16,6 @@ from PyQt6.QtCore import QObject, pyqtSignal
from pymavlink import mavutil
def _log(level, message):
print(f"[{level}] {message}")
class CommandSender(ABC):
"""指令發送抽象介面"""
@@ -67,7 +63,7 @@ class MavlinkSender(CommandSender):
"""
self.connection_string = connection_string
self.mav = mavutil.mavlink_connection(connection_string)
_log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
print(f"MavlinkSender 已建立連線: {connection_string}")
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
@@ -90,7 +86,7 @@ class MavlinkSender(CommandSender):
if self.mav:
self.mav.close()
self.mav = None
_log("INFO", "MavlinkSender 已關閉")
print("MavlinkSender 已關閉")
class Ros2CommandSender(QObject):
@@ -166,4 +162,4 @@ class Ros2CommandSender(QObject):
if not task.done():
task.cancel()
self._pending.clear()
_log("INFO", "Ros2CommandSender 已關閉")
print("Ros2CommandSender 已關閉")
src/GUI/communication.py
+144 -472
View File
@@ -12,10 +12,6 @@ import socket
import sys
import os
import traceback
try:
import serial
except ImportError:
serial = None
from pymavlink import mavutil
from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -24,11 +20,6 @@ from mavros_msgs.msg import State, VfrHud
from nav_msgs.msg import Odometry
from mavros_msgs.srv import CommandBool, CommandTOL
def _log(level, message):
print(f"[{level}] {message}")
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _src_path not in sys.path:
@@ -39,9 +30,10 @@ try:
from fc_network_apps.longCommand import CommandLongClient
except ImportError as e:
import traceback
_log("WARN", "無法導入 CommandLongClient")
_log("ERROR", f"錯誤: {e}")
_log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
print(f"⚠️ 警告: 無法導入 CommandLongClient")
print(f" 错误: {e}")
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
print(f" 完整堆栈跟踪:")
traceback.print_exc()
CommandLongClient = None
@@ -50,175 +42,14 @@ try:
from fc_network_apps.navigation import PositionTargetGlobalIntClient
except ImportError as e:
import traceback
_log("WARN", "無法導入 PositionTargetGlobalIntClient")
_log("ERROR", f"錯誤: {e}")
print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient")
print(f" 错误: {e}")
traceback.print_exc()
PositionTargetGlobalIntClient = None
try:
from fc_interfaces.msg import AttitudeRaw
except ImportError as e:
_log("WARN", "無法導入 AttitudeRaw")
_log("ERROR", f"錯誤: {e}")
AttitudeRaw = None
try:
from fc_interfaces.msg import GnssRaw
except ImportError as e:
_log("WARN", "無法導入 GnssRaw")
_log("ERROR", f"錯誤: {e}")
GnssRaw = None
class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
class JsonTelemetryProcessor:
"""共用 WebSocket JSON telemetry 轉換器。
Canonical JSON fields:
{
"system_id": 1,
"mode": "GUIDED",
"battery": 85,
"position": {"lat": 24.0, "lon": 120.0},
"heading": 90
}
Serial JSON uses this same shape; only the transport/framing differs.
"""
def _emit_json_connection_type(self, drone_id):
self.signals.update_signal.emit('connection_type', drone_id, {
'type': self.source_type
})
def process_json_telemetry_message(self, data):
"""處理 WebSocket JSON 格式的遙測資料。"""
try:
if isinstance(data, list):
for item in data:
if isinstance(item, dict):
self.process_json_telemetry_message(item)
return
if not isinstance(data, dict):
return
system_id = data.get('system_id', data.get('sysid'))
if system_id is None:
return
drone_id = f"s{self.socket_id}_{system_id}"
self._emit_json_connection_type(drone_id)
mode = data.get('mode', data.get('mode_name'))
state = {}
if mode is not None:
state['mode'] = mode
if 'armed' in data:
state['armed'] = data.get('armed')
if state:
self.signals.update_signal.emit('state', drone_id, state)
if 'battery' in data:
battery = data['battery']
if isinstance(battery, dict):
battery_data = {}
if 'percentage' in battery:
battery_data['percentage'] = battery.get('percentage')
elif 'percent' in battery:
battery_data['percentage'] = battery.get('percent')
if 'voltage' in battery:
battery_data['voltage'] = battery.get('voltage')
elif 'voltage_v' in battery:
battery_data['voltage'] = battery.get('voltage_v')
if battery_data:
self.signals.update_signal.emit('battery', drone_id, battery_data)
else:
self.signals.update_signal.emit('battery', drone_id, {
'percentage': battery
})
elif 'battery_percentage' in data or 'battery_voltage' in data:
battery_data = {}
if 'battery_percentage' in data:
battery_data['percentage'] = data.get('battery_percentage')
if 'battery_voltage' in data:
battery_data['voltage'] = data.get('battery_voltage')
self.signals.update_signal.emit('battery', drone_id, battery_data)
pos = data.get('position')
if isinstance(pos, dict):
gps_data = {
'lat': pos.get('lat', pos.get('latitude', 0)),
'lon': pos.get('lon', pos.get('longitude', 0)),
'alt': pos.get('alt', pos.get('altitude', 0))
}
self.signals.update_signal.emit('gps', drone_id, gps_data)
elif 'lat' in data or 'latitude' in data:
self.signals.update_signal.emit('gps', drone_id, {
'lat': data.get('lat', data.get('latitude', 0)),
'lon': data.get('lon', data.get('longitude', 0)),
'alt': data.get('alt', data.get('altitude', 0))
})
local = data.get('local_position', data.get('local_pose', data.get('local')))
if isinstance(local, dict):
x = local.get('x', 0.0)
y = local.get('y', 0.0)
z = local.get('z', 0.0)
self.signals.update_signal.emit('local_pose', drone_id, {
'x': x,
'y': y,
'z': z
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': z
})
elif isinstance(pos, dict):
alt = pos.get('alt', pos.get('altitude', 0.0))
self.signals.update_signal.emit('local_pose', drone_id, {
'x': 0.0,
'y': 0.0,
'z': alt
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': alt
})
velocity = data.get('velocity')
if isinstance(velocity, dict):
self.signals.update_signal.emit('velocity', drone_id, {
'vx': velocity.get('vx', velocity.get('x', 0.0)),
'vy': velocity.get('vy', velocity.get('y', 0.0)),
'vz': velocity.get('vz', velocity.get('z', 0.0))
})
attitude = data.get('attitude')
if isinstance(attitude, dict):
self.signals.update_signal.emit('attitude', drone_id, {
'roll': attitude.get('roll', 0.0),
'pitch': attitude.get('pitch', 0.0),
'yaw': attitude.get('yaw', 0.0),
'rates': attitude.get('rates', (0.0, 0.0, 0.0))
})
hud = data.get('hud', {})
if not isinstance(hud, dict):
hud = {}
if 'heading' in data:
hud['heading'] = data.get('heading')
if hud:
self.signals.update_signal.emit('hud', drone_id, {
'heading': hud.get('heading', 0.0),
'groundspeed': hud.get('groundspeed', 0.0),
'airspeed': hud.get('airspeed', 0.0),
'throttle': hud.get('throttle', 0.0),
'alt': hud.get('alt', hud.get('altitude', 0.0)),
'climb': hud.get('climb', 0.0)
})
except Exception as e:
print(f"{self.source_type} JSON telemetry processing error: {e}")
class UDPMavlinkReceiver(threading.Thread):
"""UDP MAVLink 接收器"""
def __init__(self, ip, port, signals, connection_name, monitor=None):
@@ -287,11 +118,9 @@ class UDPMavlinkReceiver(threading.Thread):
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
longitude = msg.lon / 1e7
relative_alt = msg.relative_alt / 1000.0
self.signals.update_signal.emit('gps', drone_id, {
'lat': latitude,
'lon': longitude,
'alt': relative_alt,
'lon': longitude
})
elif msg_type == "GPS_RAW_INT":
@@ -344,8 +173,8 @@ class UDPMavlinkReceiver(threading.Thread):
"""停止接收器"""
self.running = False
class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
"""串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。"""
class SerialMavlinkReceiver(threading.Thread):
"""串口 MAVLink 接收器"""
def __init__(self, port, baudrate, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.port = port
@@ -354,125 +183,47 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用
self.socket_id = monitor.get_next_socket_id() if monitor else 0
self.source_type = 'Serial'
self.running = False
self.serial_conn = None
self.mav_parser = mavutil.mavlink.MAVLink(None)
self.mav_parser.robust_parsing = True
self.json_buffer = []
self.json_depth = 0
self.json_in_string = False
self.json_escape = False
self._detected_protocols = set()
self.mav = None
def run(self):
"""執行串口接收循環。"""
"""執行串口接收循環"""
self.running = True
try:
print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)")
if serial is None:
raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線")
self.serial_conn = serial.Serial(
print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
# 創建 MAVLink 串口連接
self.mav = mavutil.mavlink_connection(
self.port,
self.baudrate,
timeout=0.2
baud=self.baudrate,
source_system=255
)
print(f"Waiting for heartbeat from {self.port}...")
self.mav.wait_heartbeat()
print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
while self.running:
try:
chunk = self.serial_conn.read(256)
if not chunk:
msg = self.mav.recv_match(blocking=True, timeout=1.0)
if msg is None:
continue
for raw_byte in chunk:
byte = bytes([raw_byte])
self._process_json_byte(byte)
self._process_mavlink_byte(byte)
self.process_mavlink_message(msg)
except Exception as e:
if self.running:
print(f"Error receiving serial telemetry: {e}")
print(f"Error receiving MAVLink message from serial: {e}")
except Exception as e:
print(f"Serial receiver error: {e}")
finally:
if self.serial_conn:
if self.mav:
try:
self.serial_conn.close()
except Exception:
self.mav.close()
except:
pass
def _process_mavlink_byte(self, byte):
try:
msg = self.mav_parser.parse_char(byte)
if msg is None:
return
if 'MAVLink' not in self._detected_protocols:
print(f"Serial {self.connection_name} detected MAVLink")
self._detected_protocols.add('MAVLink')
self.process_mavlink_message(msg)
except Exception:
# MAVLink parser is deliberately fed the whole stream, including JSON bytes.
return
def _process_json_byte(self, byte):
try:
char = byte.decode('utf-8')
except UnicodeDecodeError:
if self.json_buffer:
self._reset_json_framing()
return
if not self.json_buffer:
if char.isspace():
return
if char not in ('{', '['):
return
self.json_depth = 1
self.json_in_string = False
self.json_escape = False
self.json_buffer = [char]
return
self.json_buffer.append(char)
if self.json_in_string:
if self.json_escape:
self.json_escape = False
elif char == '\\':
self.json_escape = True
elif char == '"':
self.json_in_string = False
return
if char == '"':
self.json_in_string = True
elif char in ('{', '['):
self.json_depth += 1
elif char in ('}', ']'):
self.json_depth -= 1
if self.json_depth > 0:
return
payload = ''.join(self.json_buffer)
self._reset_json_framing()
try:
data = json.loads(payload)
if 'JSON' not in self._detected_protocols:
print(f"Serial {self.connection_name} detected JSON")
self._detected_protocols.add('JSON')
self.process_json_telemetry_message(data)
except json.JSONDecodeError as e:
print(f"Serial {self.connection_name} JSON decode error: {e}")
def _reset_json_framing(self):
self.json_buffer = []
self.json_depth = 0
self.json_in_string = False
self.json_escape = False
def process_mavlink_message(self, msg):
"""處理 MAVLink 訊息"""
try:
@@ -501,11 +252,9 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
longitude = msg.lon / 1e7
relative_alt = msg.relative_alt / 1000.0
self.signals.update_signal.emit('gps', drone_id, {
'lat': latitude,
'lon': longitude,
'alt': relative_alt,
'lon': longitude
})
elif msg_type == "GPS_RAW_INT":
@@ -557,23 +306,15 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def stop(self):
"""停止接收器"""
self.running = False
if self.serial_conn:
try:
self.serial_conn.close()
except Exception:
pass
class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
class WebSocketMavlinkReceiver(threading.Thread):
"""WebSocket MAVLink 接收器"""
def __init__(self, url, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.url = url
self.signals = signals
self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用
self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id
self.source_type = 'WS'
self.running = False
self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False
self.max_retries = 5
self.base_delay = 1.0
@@ -601,8 +342,7 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
try:
data = json.loads(message)
if isinstance(data, dict):
data['_connection_source'] = self.connection_name
data['_connection_source'] = self.connection_name
self.process_websocket_message(data)
except json.JSONDecodeError as e:
print(f"WebSocket {self.connection_name} JSON decode error: {e}")
@@ -632,7 +372,57 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def process_websocket_message(self, data):
"""處理 WebSocket 訊息"""
self.process_json_telemetry_message(data)
try:
system_id = data.get('system_id')
if not system_id:
return
drone_id = f"s{self.socket_id}_{system_id}"
# 模式
if 'mode' in data:
# 先發送連接類型資訊
self.signals.update_signal.emit('connection_type', drone_id, {
'type': 'WS'
})
self.signals.update_signal.emit('state', drone_id, {
'mode': data['mode'],
})
# 電池
if 'battery' in data:
self.signals.update_signal.emit('battery', drone_id, {
'percentage': data['battery']
})
# 位置
if 'position' in data:
pos = data['position']
self.signals.update_signal.emit('gps', drone_id, {
'lat': pos.get('lat', 0),
'lon': pos.get('lon', 0)
})
# Local position - 設定 x, y 為 0.0
self.signals.update_signal.emit('local_pose', drone_id, {
'x': 0.0,
'y': 0.0,
'z': 0.0
})
# Altitude - 設定為 0.0
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': 0.0
})
# 航向
if 'heading' in data:
self.signals.update_signal.emit('hud', drone_id, {
'heading': data['heading'],
'groundspeed': 0.0
})
except Exception as e:
print(f"WebSocket message processing error: {e}")
def stop(self):
"""停止接收器"""
@@ -738,25 +528,25 @@ class DroneMonitor(Node):
unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
client = CommandLongClient(node_name=unique_name)
self.command_long_clients[drone_id] = client
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})")
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
# 將新 client 添加到主執行器（這樣它的回調才能被處理）
if self.executor:
self.executor.add_node(client)
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
except TypeError:
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設
client = CommandLongClient()
self.command_long_clients[drone_id] = client
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
if self.executor:
self.executor.add_node(client)
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
except Exception as e:
_log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}")
return None
return self.command_long_clients[drone_id]
@@ -771,12 +561,12 @@ class DroneMonitor(Node):
unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
client = PositionTargetGlobalIntClient(node_name=unique_name)
self.position_target_clients[drone_id] = client
_log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})")
print(f" ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
if self.executor:
self.executor.add_node(client)
_log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器")
except Exception as e:
_log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
return None
return self.position_target_clients[drone_id]
@@ -803,21 +593,8 @@ class DroneMonitor(Node):
if not hasattr(self, subs_attr):
self.setup_drone(sys_id)
else:
# 檢查既有訂閱是否包含 position_gnss / position_ned / attitude，如果不包含就添加（兼容舊訂閱）
# 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
subs = getattr(self, subs_attr, {})
if isinstance(subs, dict) and 'position_gnss' not in subs and GnssRaw is not None:
base_topic = f'/fc_network/vehicle/{sys_id}'
try:
position_gnss_sub = self.create_subscription(
GnssRaw,
f'{base_topic}/position_gnss',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
)
subs['position_gnss'] = position_gnss_sub
setattr(self, subs_attr, subs)
except Exception:
pass
if isinstance(subs, dict) and 'position_ned' not in subs:
base_topic = f'/fc_network/vehicle/{sys_id}'
try:
@@ -831,19 +608,6 @@ class DroneMonitor(Node):
setattr(self, subs_attr, subs) # 明確保存更新後的字典
except Exception as e:
pass
if isinstance(subs, dict) and 'attitude' not in subs and AttitudeRaw is not None:
base_topic = f'/fc_network/vehicle/{sys_id}'
try:
attitude_sub = self.create_subscription(
AttitudeRaw,
f'{base_topic}/attitude',
lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
10
)
subs['attitude'] = attitude_sub
setattr(self, subs_attr, subs)
except Exception:
pass
def setup_drone(self, sys_id):
# sys_id 格式： sys11, sys12, ...
@@ -873,6 +637,12 @@ class DroneMonitor(Node):
lambda msg, sid=sys_id: self.battery_callback(sid, msg),
10
),
'position': self.create_subscription(
NavSatFix,
f'{base_topic}/position',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
),
'summary': self.create_subscription(
String,
f'{base_topic}/summary',
@@ -892,29 +662,6 @@ class DroneMonitor(Node):
10
)
}
if GnssRaw is not None:
subs['position_gnss'] = self.create_subscription(
GnssRaw,
f'{base_topic}/position_gnss',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
)
else:
subs['position'] = self.create_subscription(
NavSatFix,
f'{base_topic}/position',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
)
if AttitudeRaw is not None:
subs['attitude'] = self.create_subscription(
AttitudeRaw,
f'{base_topic}/attitude',
lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
10
)
setattr(self, f'drone_{sys_id}_subs', subs)
@@ -952,22 +699,22 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
_log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
# 獲取模式對應的 custom_mode 值
custom_mode = self.MODE_MAPPING.get(mode_name)
if custom_mode is None:
_log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
print(f"❌ [SET_MODE] 未知模式: {mode_name}")
return False
_log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id)
if not client:
_log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -980,13 +727,13 @@ class DroneMonitor(Node):
)
if result and result.success:
_log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
print(f"✅ [SET_MODE] 模式切換成功")
return True
else:
_log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
_log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
print(f"❌ [SET_MODE] 例外錯誤: {e}")
traceback.print_exc()
return False
@@ -996,17 +743,17 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
_log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
action_name = "解鎖" if arm else "上鎖"
_log("INFO", f"[ARM] {drone_id} -> {action_name}")
print(f"\n📢 [ARM] {drone_id} → {action_name}")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id)
if not client:
_log("ERROR", "[ARM] CommandLongClient 無法初始化")
print(f"❌ [ARM] CommandLongClient 無法初始化")
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -1018,13 +765,13 @@ class DroneMonitor(Node):
)
if result and result.success:
_log("INFO", f"[ARM] {drone_id} {action_name}成功")
print(f"✅ [ARM] {action_name}成功")
return True
else:
_log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})")
print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
_log("ERROR", f"[ARM] 例外錯誤: {e}")
print(f"❌ [ARM] 例外錯誤: {e}")
traceback.print_exc()
return False
@@ -1034,16 +781,16 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
_log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
_log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id)
if not client:
_log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -1055,46 +802,13 @@ class DroneMonitor(Node):
)
if result and result.success:
_log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
print(f"✅ [TAKEOFF] 起飛成功")
return True
else:
_log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
_log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
traceback.print_exc()
return False
async def reboot_drone(self, drone_id):
"""使用 CommandLongClient 執行飛控重啟。"""
try:
parts = drone_id.split('_')
if len(parts) < 2:
_log("ERROR", f"[REBOOT] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
_log("INFO", f"[REBOOT] {drone_id} -> 飛控重啟")
client = self.get_or_create_client(drone_id)
if not client:
_log("ERROR", "[REBOOT] CommandLongClient 無法初始化")
return False
result = await client.reboot_autopilot_async(
target_sysid=sysid,
target_compid=0,
timeout_sec=5.0,
)
if result and result.success:
_log("INFO", f"[REBOOT] {drone_id} 重啟命令已送出")
return True
_log("ERROR", f"[REBOOT] 重啟失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
_log("ERROR", f"[REBOOT] 例外錯誤: {e}")
print(f"❌ [TAKEOFF] 例外錯誤: {e}")
traceback.print_exc()
return False
@@ -1126,42 +840,17 @@ class DroneMonitor(Node):
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
# callbacks
def attitude_callback(self, sys_id, msg):
"""處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。"""
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
if actual_drone_id is None:
return
try:
if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'):
data = {
'roll': math.degrees(msg.roll),
'pitch': math.degrees(msg.pitch),
'yaw': math.degrees(msg.yaw),
'rates': (
getattr(msg, 'rollspeed', 0.0),
getattr(msg, 'pitchspeed', 0.0),
getattr(msg, 'yawspeed', 0.0),
)
}
elif hasattr(msg, 'orientation'):
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
data = {
'roll': roll,
'pitch': pitch,
'yaw': yaw,
'rates': (
msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z,
)
}
else:
return
self.latest_data[(actual_drone_id, 'attitude')] = data
except Exception as e:
print(f"Error parsing attitude msg for {sys_id}: {e}")
def attitude_callback(self, drone_id, msg):
if hasattr(msg, 'orientation'):
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
self.latest_data[(drone_id, 'attitude')] = {
'roll': roll,
'pitch': pitch,
'yaw': yaw,
'rates': (msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z)
}
def battery_callback(self, sys_id, msg):
# 使用映射獲取實際的 drone_id
@@ -1214,7 +903,7 @@ class DroneMonitor(Node):
sys_num = sys_id.replace('sys', '')
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
self.sys_to_actual_id[sys_id] = actual_drone_id
_log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
# 先發送連接類型資訊
self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -1243,28 +932,21 @@ class DroneMonitor(Node):
# 如果還沒有從 summary 獲取到映射，則不處理
if actual_drone_id is None:
return
gps_data = {
self.latest_data[(actual_drone_id, 'gps')] = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
}
if hasattr(msg, 'fix_type'):
gps_data['fix_type'] = msg.fix_type
if hasattr(msg, 'satellites_visible'):
gps_data['satellites_visible'] = msg.satellites_visible
if hasattr(msg, 'eph'):
gps_data['eph'] = msg.eph
if hasattr(msg, 'epv'):
gps_data['epv'] = msg.epv
self.latest_data[(actual_drone_id, 'gps')] = gps_data
# ================================================================================
# 【新增】儲存 GPS 資料到 drone_gps 字典
# ================================================================================
self.drone_gps[actual_drone_id] = gps_data.copy()
self.drone_gps[actual_drone_id] = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
}
# ================================================================================
def local_vel_callback(self, drone_id, msg):
@@ -1321,9 +1003,6 @@ class DroneMonitor(Node):
x = msg.pose.pose.position.y # NED 座標系中交換 x/y（與 local_pose 對齐）
y = msg.pose.pose.position.x
z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
vx = msg.twist.twist.linear.y
vy = msg.twist.twist.linear.x
vz = -msg.twist.twist.linear.z
# 儲存高度信息
self.latest_data[(actual_drone_id, 'altitude')] = {
@@ -1336,13 +1015,6 @@ class DroneMonitor(Node):
'y': y,
'z': z
}
# 儲存速度資訊，供總覽頁「XY速度」欄位顯示
self.latest_data[(actual_drone_id, 'velocity')] = {
'vx': vx,
'vy': vy,
'vz': vz
}
# 發送信號給 GUI 更新高度顯示
self.signals.update_signal.emit('altitude', actual_drone_id, {
@@ -1362,10 +1034,10 @@ class DroneMonitor(Node):
}
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
"""啟動串口遙測連接（自動辨識 MAVLink / JSON）"""
"""啟動串口 MAVLink 連接"""
connection_name = f"Serial_{port.replace('/', '_')}"
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
receiver.start()
self.serial_receivers.append(receiver)
_log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)")
return receiver
print(f"Started serial connection on {port} at {baudrate} baud")
return receiver
src/GUI/drone_panel.py
+32 -155
View File
@@ -1,77 +1,10 @@
#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox, QApplication)
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
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):
"""單個無人機面板類別"""
@@ -99,13 +32,12 @@ class DronePanel(QWidget):
self.attitude_indicator = None
self._init_ui()
self.apply_font_scale()
def _init_ui(self):
"""初始化UI"""
self.setObjectName(f"panel_{self.drone_id}")
self.setFixedHeight(160)
_set_scaled_stylesheet(self, """
self.setFixedHeight(140)
self.setStyleSheet("""
background-color: #2A2A2A;
border-radius: 8px;
""")
@@ -117,7 +49,7 @@ class DronePanel(QWidget):
# 創建內容容器（包含 info 和 control）
content_widget = QWidget()
_set_scaled_stylesheet(content_widget, "background-color: #333; border-radius: 6px;")
content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
content_layout = QHBoxLayout(content_widget)
content_layout.setContentsMargins(8, 8, 8, 8)
content_layout.setSpacing(8)
@@ -150,7 +82,7 @@ class DronePanel(QWidget):
# 勾選框
self.checkbox = QCheckBox()
self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
_set_scaled_stylesheet(self.checkbox, """
self.checkbox.setStyleSheet("""
QCheckBox {
color: #DDD;
}
@@ -171,8 +103,8 @@ class DronePanel(QWidget):
)
# ID 顯示
self.id_label = QLabel(self.display_id)
_set_scaled_stylesheet(self.id_label, """
id_label = QLabel(self.display_id)
id_label.setStyleSheet("""
font-weight: bold;
font-size: 14px;
color: #7FFFD4;
@@ -180,7 +112,7 @@ class DronePanel(QWidget):
""")
header_layout.addWidget(self.checkbox)
header_layout.addWidget(self.id_label)
header_layout.addWidget(id_label)
header_layout.addStretch()
info_layout.addWidget(header)
@@ -196,12 +128,8 @@ class DronePanel(QWidget):
# 第三行：高度
altitude_row = self._create_altitude_row()
info_layout.addWidget(altitude_row)
# 第四行：GNSS 定位狀態
gnss_row = self._create_gnss_row()
info_layout.addWidget(gnss_row)
# 第五行：航向 + 速度
# 第四行：航向 + 速度
nav_row = self._create_nav_row()
info_layout.addWidget(nav_row)
@@ -220,15 +148,15 @@ class DronePanel(QWidget):
status_layout.setContentsMargins(0, 0, 0, 0)
status_title = QLabel("狀態：")
_set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;")
status_title.setStyleSheet("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.mode_label.setStyleSheet("color: #DDD;")
self.armed_label = QLabel("--")
self.armed_label.setObjectName(f"{self.drone_id}_armed")
_set_scaled_stylesheet(self.armed_label, "color: #DDD;")
self.armed_label.setStyleSheet("color: #DDD;")
status_layout.addWidget(status_title)
status_layout.addWidget(self.mode_label)
@@ -244,15 +172,15 @@ class DronePanel(QWidget):
connection_layout.setContentsMargins(0, 0, 0, 0)
connection_title = QLabel("Socket：")
_set_scaled_stylesheet(connection_title, "color: #888; min-width: 50px;")
connection_title.setStyleSheet("color: #888; min-width: 50px;")
# 根據解析的 drone_id 資訊設定初始值
self.socket_seq_label = QLabel(self.socket_seq)
self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq")
_set_scaled_stylesheet(self.socket_seq_label, "color: #DDD;")
self.socket_seq_label.setStyleSheet("color: #DDD;")
connection_sep = QLabel(" - ")
_set_scaled_stylesheet(connection_sep, "color: #DDD;")
connection_sep.setStyleSheet("color: #DDD;")
# 設定連接方式顯示
connection_type_map = {
@@ -265,7 +193,7 @@ class DronePanel(QWidget):
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;")
self.connection_type_label.setStyleSheet("color: #DDD;")
connection_layout.addWidget(connection_title)
connection_layout.addWidget(self.socket_seq_label)
@@ -282,29 +210,29 @@ class DronePanel(QWidget):
battery_layout.setContentsMargins(0, 0, 0, 0)
# 顯示百分比
battery_title = QLabel("電池：")
_set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;")
battery_title.setStyleSheet("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;")
self.battery_pct_label.setStyleSheet("color: #DDD;")
# 分隔符
separator1 = QLabel(" - ")
_set_scaled_stylesheet(separator1, "color: #DDD;")
separator1.setStyleSheet("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;")
self.battery_vol_label.setStyleSheet("color: #DDD;")
# 分隔符
separator2 = QLabel(" - ")
_set_scaled_stylesheet(separator2, "color: #DDD;")
separator2.setStyleSheet("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;")
self.battery_cells_label.setStyleSheet("color: #DDD;")
battery_layout.addWidget(battery_title)
battery_layout.addWidget(self.battery_pct_label)
@@ -323,18 +251,18 @@ class DronePanel(QWidget):
altitude_layout.setContentsMargins(0, 0, 0, 0)
altitude_title = QLabel("高度：")
_set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;")
altitude_title.setStyleSheet("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;")
self.altitude_label.setStyleSheet("color: #DDD;")
speed_title = QLabel("速度：")
_set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;")
speed_title.setStyleSheet("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;")
self.speed_label.setStyleSheet("color: #DDD;")
altitude_layout.addWidget(altitude_title)
altitude_layout.addWidget(self.altitude_label)
@@ -343,34 +271,6 @@ class DronePanel(QWidget):
altitude_layout.addStretch()
return altitude_row
def _create_gnss_row(self):
"""創建 GNSS 定位狀態行。"""
gnss_row = QWidget()
gnss_layout = QHBoxLayout(gnss_row)
gnss_layout.setContentsMargins(0, 0, 0, 0)
fix_title = QLabel("定位：")
_set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;")
self.fix_type_label = QLabel("--")
self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label")
_set_scaled_stylesheet(self.fix_type_label, "color: #DDD;")
error_title = QLabel("誤差(H/V)：")
_set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;")
self.gnss_error_label = QLabel("--")
self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error")
_set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;")
gnss_layout.addWidget(fix_title)
gnss_layout.addWidget(self.fix_type_label)
gnss_layout.addWidget(error_title)
gnss_layout.addWidget(self.gnss_error_label)
gnss_layout.addStretch()
return gnss_row
def _create_position_row(self):
"""位置行已移除（位置座標不再顯示於面板）。"""
@@ -420,16 +320,6 @@ class DronePanel(QWidget):
def is_checked(self):
"""獲取勾選狀態"""
return self.checkbox.isChecked()
def apply_font_scale(self):
"""重新套用目前字體倍率到 panel 內所有元件。"""
_apply_scaled_font(self)
_reapply_scaled_stylesheet(self)
for child in self.findChildren(QWidget):
_apply_scaled_font(child)
_reapply_scaled_stylesheet(child)
if self.attitude_indicator:
self.attitude_indicator.update()
class SocketGroupPanel(QWidget):
# 定義信號
@@ -441,12 +331,11 @@ class SocketGroupPanel(QWidget):
self.color = color
self.socket_type = socket_type
self._init_ui()
self.apply_font_scale()
def _init_ui(self):
"""初始化UI"""
self.setObjectName(f"socket_group_{self.socket_id}")
_set_scaled_stylesheet(self, """
self.setStyleSheet("""
background-color: #1E1E1E;
border-radius: 12px;
""")
@@ -463,7 +352,7 @@ class SocketGroupPanel(QWidget):
# 分組勾選框
self.group_checkbox = QCheckBox()
self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
_set_scaled_stylesheet(self.group_checkbox, f"""
self.group_checkbox.setStyleSheet(f"""
QCheckBox {{ color: #DDD; }}
QCheckBox::indicator {{
width: 14px;
@@ -491,7 +380,7 @@ class SocketGroupPanel(QWidget):
else:
title_text = f"Socket {self.socket_id}"
self.title_label = QLabel(title_text)
_set_scaled_stylesheet(self.title_label, f"""
self.title_label.setStyleSheet(f"""
font-weight: bold;
font-size: 16px;
color: {self.color};
@@ -541,14 +430,6 @@ class SocketGroupPanel(QWidget):
"""設置分組勾選狀態（支持半選）"""
self.group_checkbox.setCheckState(state)
def apply_font_scale(self):
"""重新套用目前字體倍率到 socket 分組面板。"""
_apply_scaled_font(self)
_reapply_scaled_stylesheet(self)
for child in self.findChildren(QWidget):
_apply_scaled_font(child)
_reapply_scaled_stylesheet(child)
class AttitudeIndicator(QWidget):
"""
@@ -622,9 +503,7 @@ class AttitudeIndicator(QWidget):
# 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)
p.setFont(QFont("Arial", 6))
for deg in range(-30, 31, 10):
if deg == 0:
continue
@@ -702,8 +581,6 @@ class AttitudeIndicator(QWidget):
# 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)
p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
hdg_str = f"{int(self.heading)}°"
p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)
p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)
src/GUI/gui.py
+339 -1031
View File
File diff suppressed because it is too large Load Diff
src/GUI/map_layout.py
+28 -73
View File
@@ -3,11 +3,6 @@ from PyQt6.QtWebEngineWidgets import QWebEngineView
from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
from PyQt6.QtWebChannel import QWebChannel
def _log(level, message):
print(f"[{level}] {message}")
class DroneMap:
"""無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
@@ -16,7 +11,6 @@ class DroneMap:
self.map_view = QWebEngineView()
self.map_loaded = False
self.pending_map_updates = {}
self.font_scale = 1.0
# 創建橋接對象
self.bridge = MapBridge()
@@ -37,7 +31,6 @@ class DroneMap:
<script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
<script src="qrc:///qtwebchannel/qwebchannel.js"></script>
<style>
:root { --ui-font-scale: 1; }
html, body, #map { height: 100%; margin: 0; }
#map {
user-select: none;
@@ -72,7 +65,7 @@ class DroneMap:
border: none;
border-radius: 4px;
cursor: pointer;
font-size: calc(13px * var(--ui-font-scale));
font-size: 13px;
font-weight: bold;
box-shadow: 0 2px 5px rgba(0,0,0,0.2);
}
@@ -92,7 +85,7 @@ class DroneMap:
}
.mission-info-row {
margin-bottom: 8px;
font-size: calc(12px * var(--ui-font-scale));
font-size: 12px;
color: #333;
}
.mission-info-label {
@@ -111,7 +104,7 @@ class DroneMap:
border: none;
border-radius: 4px;
cursor: pointer;
font-size: calc(13px * var(--ui-font-scale));
font-size: 13px;
font-weight: bold;
margin-top: 8px;
}
@@ -138,7 +131,7 @@ class DroneMap:
border: none;
border-radius: 4px;
cursor: pointer;
font-size: calc(13px * var(--ui-font-scale));
font-size: 13px;
font-weight: bold;
transition: background-color 0.2s;
}
@@ -156,7 +149,7 @@ class DroneMap:
border: none;
border-radius: 4px;
cursor: pointer;
font-size: calc(13px * var(--ui-font-scale));
font-size: 13px;
font-weight: bold;
transition: background-color 0.2s;
}
@@ -174,7 +167,7 @@ class DroneMap:
border: none;
border-radius: 4px;
cursor: pointer;
font-size: calc(13px * var(--ui-font-scale));
font-size: 13px;
font-weight: bold;
transition: background-color 0.2s;
}
@@ -329,7 +322,7 @@ class DroneMap:
align-items: center;
justify-content: center;
font-weight: bold;
font-size: calc(12px * var(--ui-font-scale));
font-size: 12px;
text-shadow: 1px 1px 2px rgba(255,255,255,0.8), -1px -1px 2px rgba(255,255,255,0.8);
">${sysid}</div>`,
iconSize: [16, 16],
@@ -340,7 +333,6 @@ class DroneMap:
var markers = {};
var idLabels = {};
var trajectories = {};
const maxTrajectoryPoints = 300;
var trajectoryLines = {};
var focusedId = null;
var initialized = false;
@@ -369,16 +361,6 @@ class DroneMap:
var drawStartPoint = null;
var mapDragStarted = false;
var clickStartPos = null;
var lastManualMapMoveAt = 0;
var autoCenteringMap = false;
function markManualMapMove() {
if (!autoCenteringMap) {
lastManualMapMoveAt = Date.now();
}
}
map.on('dragstart drag dragend zoomstart zoomend', markManualMapMove);
// 路徑標記變量 (跟隨模式用)
var routePoints = [];
@@ -582,7 +564,7 @@ class DroneMap:
'align-items: center;' +
'justify-content: center;' +
'font-weight: bold;' +
'font-size: calc(11px * var(--ui-font-scale));' +
'font-size: 11px;' +
'border: 2px solid white;' +
'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
'">' + idx + '</div>',
@@ -635,10 +617,6 @@ class DroneMap:
}
}
// ================================================================================
function setFontScale(scale) {
document.documentElement.style.setProperty('--ui-font-scale', scale);
}
// 開始任務
function startMission() {
@@ -713,32 +691,24 @@ class DroneMap:
const point = [lat, lon];
trajectories[id].push(point);
if (trajectories[id].length > maxTrajectoryPoints) {
if (trajectories[id].length > 1000) {
trajectories[id].shift();
trajectoryLines[id].setLatLngs(trajectories[id]);
} else {
trajectoryLines[id].addLatLng(point);
}
trajectoryLines[id].setLatLngs([...trajectories[id]]);
}
function focusOn(id) {
if (!markers[id]) return;
focusedId = id;
var latlng = markers[id].getLatLng();
autoCenteringMap = true;
map.flyTo(latlng, map.getZoom());
setTimeout(() => { autoCenteringMap = false; }, 500);
}
setInterval(() => {
if (focusedId && markers[focusedId]) {
if (Date.now() - lastManualMapMoveAt < 1000) {
return;
}
var latlng = markers[focusedId].getLatLng();
autoCenteringMap = true;
map.panTo(latlng);
setTimeout(() => { autoCenteringMap = false; }, 300);
}
}, 1000);
@@ -832,7 +802,7 @@ class DroneMap:
'width: 22px; height: 22px;' +
'border-radius: 50%;' +
'display: flex; align-items: center; justify-content: center;' +
'font-weight: bold; font-size: calc(10px * var(--ui-font-scale));' +
'font-weight: bold; font-size: 10px;' +
'border: 2px solid white;' +
'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
'">' + groupId + '</div>',
@@ -852,7 +822,7 @@ class DroneMap:
'width: 22px; height: 22px;' +
'border-radius: 50%;' +
'display: flex; align-items: center; justify-content: center;' +
'font-weight: bold; font-size: calc(14px * var(--ui-font-scale));' +
'font-weight: bold; font-size: 14px;' +
'border: 2px solid white;' +
'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
'">★</div>',
@@ -897,15 +867,14 @@ class DroneMap:
# 設置地圖更新計時器
self.map_update_timer = QTimer()
self.map_update_timer.timeout.connect(self.update_map_positions)
self.map_update_timer.start(100) # 每 100ms 更新一次（10Hz）
self.map_update_timer.start(200) # 每 200ms 更新一次
def _on_map_loaded(self, ok: bool):
"""地圖加載完成回調"""
if ok:
self.map_loaded = True
self.set_font_scale(self.font_scale)
else:
_log("ERROR", "地圖載入失敗")
print("⚠️ 地圖加載失敗")
def update_drone_position(self, drone_id, lat, lon, heading):
"""更新無人機位置（加入待處理隊列）"""
@@ -935,12 +904,6 @@ class DroneMap:
"""聚焦到指定無人機"""
if self.map_loaded:
self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
def set_font_scale(self, scale):
"""設定地圖 HTML 控制項的字體倍率。"""
self.font_scale = scale
if self.map_loaded:
self.map_view.page().runJavaScript(f"setFontScale({scale:.3f});")
# ================================================================================
# 任務規劃視覺化方法
@@ -961,24 +924,20 @@ class DroneMap:
f"{target_lat:.6f}, {target_lon:.6f});"
)
self.map_view.page().runJavaScript(js_code)
_log(
"INFO",
f"地圖已繪製 Group {group_id} 任務規劃: "
f"C({center_lat:.6f}, {center_lon:.6f}) -> "
f"T({target_lat:.6f}, {target_lon:.6f})",
)
print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
def clear_mission_plan(self):
"""清除地圖上所有任務規劃標記"""
if self.map_loaded:
self.map_view.page().runJavaScript("clearAllMissionPlans();")
_log("INFO", "地圖已清除所有任務規劃")
print("🗑️ 地圖已清除所有任務規劃")
def clear_mission_plan_for_group(self, group_id):
"""清除指定群組的任務規劃標記"""
if self.map_loaded:
self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
_log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
def set_mission_mode(self, mode):
"""從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
@@ -1074,56 +1033,52 @@ class MapBridge(QObject):
def clearAllDroneSelection(self):
"""供 JavaScript 調用的方法 - 清除所有無人機選擇"""
self.clear_all_drone_selection.emit()
_log("INFO", "已清除所有無人機選擇")
print("🗑️ 清除所有無人機選擇")
@pyqtSlot()
def toggleSelectAllDrones(self):
"""供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
self.select_all_drones.emit()
_log("INFO", "已切換全選無人機")
print("🔄 切換全選無人機")
@pyqtSlot(float, float, float, float)
def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
"""供 JavaScript 調用的方法 - 開始任務"""
self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon)
_log(
"INFO",
f"已發出開始任務信號: "
f"C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})",
)
print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
@pyqtSlot()
def pauseMissionSignal(self):
"""供 JavaScript 調用的方法 - 暫停任務"""
self.pause_mission_signal.emit()
_log("INFO", "已發出暫停任務信號")
print("⏸️ 暫停任務信號已發出")
@pyqtSlot(str)
def rectangleSelected(self, points_json):
"""供 JavaScript 調用的方法 - 矩形選擇完成"""
self.rectangle_selected.emit(points_json)
_log("INFO", f"矩形區域已選擇: {points_json}")
print(f"📦 矩形區域已選擇: {points_json}")
@pyqtSlot(str)
def polygonSelected(self, points_json):
"""供 JavaScript 調用的方法 - 多邊形選擇完成"""
self.polygon_selected.emit(points_json)
_log("INFO", f"多邊形區域已選擇: {points_json}")
print(f"🔷 多邊形區域已選擇: {points_json}")
@pyqtSlot(str)
def missionModeChanged(self, mode):
"""供 JavaScript 調用的方法 - 任務模式切換"""
self.mission_mode_changed.emit(mode)
_log("INFO", f"任務模式已切換: {mode}")
print(f"🔄 任務模式切換: {mode}")
@pyqtSlot(str)
def routeConfirmed(self, points_json):
"""供 JavaScript 調用的方法 - 路徑確認"""
self.route_confirmed.emit(points_json)
_log("INFO", f"路徑已確認: {points_json}")
print(f"📍 路徑已確認: {points_json}")
@pyqtSlot(str)
def droneBoxSelected(self, drone_ids_json):
"""供 JavaScript 調用的方法 - 框選無人機完成"""
self.drone_box_selected.emit(drone_ids_json)
_log("INFO", f"框選無人機完成: {drone_ids_json}")
print(f"📦 框選無人機: {drone_ids_json}")
src/GUI/mission_executor.py
+22 -34
View File
@@ -19,10 +19,6 @@ from enum import Enum
from PyQt6.QtCore import QObject, QTimer, pyqtSignal
def _log(level, message):
print(f"[{level}] {message}")
class MissionState(Enum):
"""整體任務狀態"""
IDLE = "idle"
@@ -113,7 +109,7 @@ class MissionExecutor(QObject):
def start(self, planned_waypoints):
if self.state == MissionState.RUNNING:
_log("WARN", "任務已在執行中")
print("任務已在執行中")
return
self.tasks.clear()
@@ -136,34 +132,31 @@ class MissionExecutor(QObject):
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}",
)
print(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", "任務已暫停")
print("任務暫停")
def resume(self):
if self.state == MissionState.PAUSED:
self._timer.start(self._interval_ms)
self.state = MissionState.RUNNING
_log("INFO", "任務已恢復")
print("任務恢復")
def stop(self):
self._timer.stop()
self.tasks.clear()
self.rendezvous_indices = set()
self.state = MissionState.IDLE
_log("INFO", "任務已停止")
print("任務停止")
# ------------------------------------------------------------------ 控制迴圈
@@ -204,7 +197,7 @@ class MissionExecutor(QObject):
# rendezvous 點 → 不推進，進入 barrier 等待
task.status = TaskStatus.WAITING_AT_BARRIER
task.waiting_since = now
_log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index}，等待同伴")
print(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}"
@@ -243,7 +236,7 @@ class MissionExecutor(QObject):
self._timer.stop()
self.state = MissionState.IDLE
self.mission_completed.emit()
_log("INFO", "任務全部完成")
print("===== 任務全部完成 =====")
def _advance_waypoint(self, task, arrived_distance):
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
@@ -255,20 +248,14 @@ class MissionExecutor(QObject):
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})",
)
print(f" {task.drone_id} → DONE "
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)",
)
print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
f"(到達距離: {arrived_distance:.1f}m)")
def _check_barriers(self, now):
"""檢查每個有人在等的 barrier 能不能釋放。"""
@@ -302,13 +289,14 @@ class MissionExecutor(QObject):
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}，強制放行")
print(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)} 架")
print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
f"推進 {len(waiting_tasks)} 架")
for task in waiting_tasks:
task.status = TaskStatus.NORMAL
msg = f"barrier WP {barrier_idx} released ({tag})"
@@ -336,7 +324,7 @@ class MissionExecutor(QObject):
return
task.fail_count += 1
_log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
print(f"⚠️ {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
if task.fail_count < self.MAX_RETRY:
task.status = TaskStatus.RETRYING
@@ -351,13 +339,13 @@ class MissionExecutor(QObject):
drone_id, task.status.value,
f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
)
_log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER")
print(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")
print(f"⚠️ 無 monitor，無法將 {drone_id} 切到 LOITER")
return
try:
loop = asyncio.get_event_loop()
src/GUI/mission_group.py
+15 -56
View File
@@ -23,18 +23,6 @@ GROUP_COLORS = [
'#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:
"""單一任務群組的資料"""
@@ -156,7 +144,6 @@ class GroupPanel(QWidget):
mode_change_requested = pyqtSignal(str, str) # group_id, mode
arm_requested = pyqtSignal(str) # group_id
takeoff_requested = pyqtSignal(str, float) # group_id, altitude
reboot_requested = pyqtSignal(str) # group_id
box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
clear_group_requested = pyqtSignal(str) # group_id — 清除分組
@@ -170,13 +157,10 @@ class GroupPanel(QWidget):
QPushButton:disabled {{ background-color: #444; color: #666; }}
"""
def __init__(self, group: MissionGroup, parent=None, default_params=None):
def __init__(self, group: MissionGroup, parent=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):
@@ -371,23 +355,6 @@ class GroupPanel(QWidget):
right.addStretch()
# ============================
# 飛控操作
# ============================
reboot_col = QVBoxLayout()
reboot_col.setSpacing(3)
reboot_title = QLabel("飛控操作")
reboot_title.setStyleSheet(TITLE)
reboot_col.addWidget(reboot_title)
reboot_btn = QPushButton("重啟飛控")
reboot_btn.setStyleSheet(BTN.format(bg='#8A3A3A', fg='#FFF', hover='#A84646'))
reboot_btn.clicked.connect(
lambda: self.reboot_requested.emit(self.group.group_id))
reboot_col.addWidget(reboot_btn)
reboot_col.addStretch()
# ============================
# 第四欄：任務參數
# ============================
@@ -405,23 +372,23 @@ class GroupPanel(QWidget):
# 每種任務類型的參數定義: (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']),
('spacing', '間距 (m)', '5.0'),
('base_altitude', '基準高度 (m)', '10.0'),
('altitude_diff', '高低差 (m)', '2.0'),
],
'CIRCLE_FORMATION': [
('radius', '半徑 (m)', self.default_params['radius']),
('altitude', '高度 (m)', self.default_params['altitude']),
('start_angle', '起始角 (°)', self.default_params['start_angle']),
('radius', '半徑 (m)', '10.0'),
('altitude', '高度 (m)', '10.0'),
('start_angle', '起始角 (°)', '0'),
],
'LEADER_FOLLOWER': [
('lateral_offset', '橫向偏移 (m)', self.default_params['lateral_offset']),
('longitudinal_spacing', '縱向間距 (m)', self.default_params['longitudinal_spacing']),
('altitude', '高度 (m)', self.default_params['altitude']),
('lateral_offset', '橫向偏移 (m)', '3.0'),
('longitudinal_spacing', '縱向間距 (m)', '5.0'),
('altitude', '高度 (m)', '10.0'),
],
'GRID_SWEEP': [
('line_spacing', '掃描線距 (m)', self.default_params['line_spacing']),
('altitude', '高度 (m)', self.default_params['altitude']),
('line_spacing', '掃描線距 (m)', '5.0'),
('altitude', '高度 (m)', '10.0'),
],
}
@@ -471,7 +438,7 @@ class GroupPanel(QWidget):
# 當任務類型切換時更新參數顯示
self.type_combo.currentTextChanged.connect(self._update_param_visibility)
# ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ──
# ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
# 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
cols.addLayout(left, 0)
cols.addWidget(self._make_sep())
@@ -480,9 +447,7 @@ class GroupPanel(QWidget):
cols.addLayout(param_col, 0)
cols.addWidget(self._make_sep())
cols.addLayout(right, 0)
cols.addWidget(self._make_sep())
cols.addLayout(reboot_col, 0)
cols.addStretch() # 填充剩餘空間，使欄位置左
cols.addStretch() # 填充剩餘空間，使四列置左
layout.addLayout(cols)
@@ -567,13 +532,6 @@ class GroupPanel(QWidget):
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;"
@@ -595,3 +553,4 @@ class GroupPanel(QWidget):
except ValueError:
alt = 10.0
self.takeoff_requested.emit(self.group.group_id, alt)
src/GUI/mission_planner.py
+197 -42
View File
@@ -83,11 +83,10 @@ class FormationPlanner:
center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
# origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
# 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
self.current_origin = (center_lat, center_lon, 0.0)
self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
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)
@@ -207,17 +206,29 @@ class FormationPlanner:
def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
"""
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
center_path = 使用者畫的路徑點（直線段連接，不插弧），
每架 drone 沿弧長 s 參數化做隊形偏移：
s_follower = s_leader - lon_k
position = lookup(s_follower) + lat_k × perp(tangent)
s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
核心想法：
- 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 處理，避免倒退或衝出
Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
隊形 (俯視):
D0 (lat=-L, lon=0) ← leader
解決前版「空間偏移」的三個 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)
@@ -226,16 +237,29 @@ class FormationPlanner:
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: 建立中心路徑（直線段連接，每個中間航點設 barrier）
center_path, barrier_indices = self._build_center_path(route_wps_local)
# 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)。超出範圍時沿端點方向外推。"""
"""
在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
s<0 → clip 到 start；s>s_max → clip 到 end。
tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
"""
if n_pts == 1:
pt = center_path[0]
return pt['x'], pt['y'], pt['dir']
@@ -246,8 +270,7 @@ class FormationPlanner:
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
dx, dy = sdx / slen, sdy / slen
return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
return a['x'], a['y'], (sdx / slen, sdy / slen)
return a['x'], a['y'], a['dir']
if s_target >= s_max:
@@ -256,9 +279,7 @@ class FormationPlanner:
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
dx, dy = sdx / slen, sdy / slen
overshoot = s_target - s_max
return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
return b['x'], b['y'], (sdx / slen, sdy / slen)
return b['x'], b['y'], b['dir']
# Binary search：s_list[lo] <= s_target < s_list[hi]
@@ -287,6 +308,16 @@ class FormationPlanner:
# 避免浮點噪音在投影值相近時翻轉 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
@@ -303,16 +334,46 @@ class FormationPlanner:
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):
def _build_center_path(self, waypoints,
formation_max_lateral,
min_inner_radius,
arc_resolution,
sharp_turn_cos_threshold):
"""
建立中心路徑 — 直接用路徑點連接，不插弧。
建立以「圓弧」連接直線段的中心路徑。
每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。
每個中間 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)
@@ -324,35 +385,129 @@ class FormationPlanner:
'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': (0.0, 1.0),
's': 0.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 = []
for i in range(num_wps):
if i < num_wps - 1:
dx = waypoints[i + 1][0] - waypoints[i][0]
dy = waypoints[i + 1][1] - waypoints[i][1]
else:
dx = waypoints[i][0] - waypoints[i - 1][0]
dy = waypoints[i][1] - waypoints[i - 1][1]
# 起點
path.append({
'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': segments[0][0],
})
length = math.hypot(dx, dy)
if length < 0.01:
direction = (0.0, 1.0)
else:
direction = (dx / length, dy / length)
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': waypoints[i][0], 'y': waypoints[i][1],
'dir': direction,
'x': T_in[0], 'y': T_in[1],
'dir': u_in,
})
if 0 < i < num_wps - 1:
barrier_indices.append(len(path) - 1)
# 弧段採樣 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]
src/GUI/overview_table.py
+16 -21
View File
@@ -6,9 +6,8 @@ class OverviewTable(QTableWidget):
"""總覽表格，顯示所有無人機的狀態資訊"""
# 默認的資訊類型和映射
DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV",
"高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度",
"爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
"空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
DEFAULT_INFO_TYPE_MAP = {
"mode": 0,
@@ -16,24 +15,20 @@ class OverviewTable(QTableWidget):
"battery": 2,
"longitude": 3,
"latitude": 4,
"fix_type": 5,
"satellites_visible": 6,
"eph": 7,
"epv": 8,
"altitude": 9,
"local": 10,
"velocity": 11,
"groundspeed": 12,
"heading": 13,
"airspeed": 14,
"throttle": 15,
"hud_alt": 16,
"climb": 17,
"roll": 18,
"pitch": 19,
"yaw": 20,
"loss_rate": 21,
"ping": 22
"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):
src/fc_interfaces/CMakeLists.txt
-5
View File
@@ -8,16 +8,11 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AttitudeRaw.msg"
"msg/GnssRaw.msg"
"srv/MavPing.srv"
"srv/MavCommandLong.srv"
"srv/MavPositionTargetGlobalInt.srv"
DEPENDENCIES std_msgs builtin_interfaces
)
ament_package()
src/fc_interfaces/msg/AttitudeRaw.msg
-9
View File
@@ -1,9 +0,0 @@
uint32 seq
builtin_interfaces/Time stamp
string frame_id
float32 roll
float32 pitch
float32 yaw
float32 rollspeed
float32 pitchspeed
float32 yawspeed
src/fc_interfaces/msg/GnssRaw.msg
-8
View File
@@ -1,8 +0,0 @@
builtin_interfaces/Time stamp
float64 latitude
float64 longitude
float64 altitude
uint8 fix_type
uint8 satellites_visible
float32 eph
float32 epv
src/fc_interfaces/package.xml
-5
View File
@@ -15,11 +15,6 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<build_depend>std_msgs</build_depend>
<exec_depend>std_msgs</exec_depend>
<build_depend>builtin_interfaces</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
+2 -19
View File
@@ -176,15 +176,13 @@
這裡支持了所有 ROS2 框架的對應功能 包涵
1. 將載具的串流更新的狀態更新到對應的 topic
2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
3. 訂閱外部 RTCM 資料並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
### **[Class]** fc_ros_manager
MAVLink ROS2 節點管理器 管理三個獨立的 Node
會開一個執行緒 讓三個 Node 都跑在裡面
MAVLink ROS2 節點管理器 管理兩個獨立的 Node
會開一個執行緒 讓兩個 Node 都跑在裡面
然後用 spin_once 保持 Node 的活性
- *self.status_publisher* 物件實例
- *self.command_service* 物件實例
- *self.rtcm_relay* 物件實例
- *self.spin_thread* 執行緒
---
- *start()* 啟動自己
@@ -212,20 +210,6 @@
- *__init__()* 這邊登入要創建的 service 到 ros2 系統
- *handle_XXX()* 這邊是實現 service 的具體方法
### **[Class]** RtcmRelay
訂閱 /fc_network/rtcm_in (mavros_msgs/msg/RTCM) 並轉發為 MAVLink GPS_RTCM_DATA
topic 的 QoS：BEST_EFFORT / KEEP_LAST depth=1 / VOLATILE（低延遲優先，丟舊包不排隊）
處理順序：過期丟棄(1s) → blake2s hash 去重 → 最短轉發間隔節流 → 分片轉發
分片：GPS_RTCM_DATA 每片 180 bytes，超過自動分片，最多 4 片 (720 bytes)，超長直接丟棄
失敗語意：不做舊包重送，送失敗就放棄等下一包
- *self.counters* 觀測用計數器 (received/forwarded/dropped_stale/dropped_duplicate/dropped_throttle/dropped_oversize/no_route/send_fail)
- *self.clock_offset_sec* 與 RTCM 來源的時鐘偏移量（秒）
---
- *_rtcm_callback()* 收到 RTCM 封包時的處理流程
- *_send_rtcm()* 對單一載具編碼並送出 GPS_RTCM_DATA（含分片邏輯）
- *set_clock_offset()* 設定時鐘偏移量
- *get_counters()* 取得觀測計數器
# 開發記錄
@@ -241,7 +225,6 @@
9. 載具狀態收集與彙整
10. a. ros2 topic 應用開發介面 (基礎框架)
b. ros2 service 應用開發介面 (基礎框架)
11. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
### 待開發功能
5-1. 建立 serial 連線 並可以對接收器下達AT指令
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+3 -3
View File
@@ -1145,9 +1145,9 @@ class Orchestrator:
self.fc_ros_manager = mros.ros2_manager
self.fc_ros_manager.initialize()
self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
'position_gnss': 0.2,
'position_ned': 0.5,
'attitude': 0.1,
'position': 1.0,
'position_ned': 1.0,
'attitude': 1.0,
'velocity': 0.0,
'battery': 1.0,
'vfr_hud': 1.0,
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
+1 -12
View File
@@ -109,7 +109,6 @@ class mavlink_bridge:
"""初始化訊息處理器映射表，提高處理效率"""
self.message_handlers = {
0: self._handle_heartbeat, # HEARTBEAT
24: self._handle_gps_raw_int, # GPS_RAW_INT
30: self._handle_attitude, # ATTITUDE
32: self._handle_local_position, # LOCAL_POSITION_NED
33: self._handle_global_position, # GLOBAL_POSITION_INT
@@ -240,14 +239,6 @@ class mavlink_bridge:
component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺
component.status.position.timestamp = timestamp
def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
"""處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
component.status.gps.fix_type = msg.fix_type
component.status.gps.satellites_visible = msg.satellites_visible
component.status.gps.eph = None if msg.eph == 65535 else msg.eph
component.status.gps.epv = None if msg.epv == 65535 else msg.epv
component.status.gps.timestamp = timestamp
def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
"""處理 VFR_HUD 訊息 (msg_id: 74)"""
component.status.vfr.airspeed = msg.airspeed
@@ -395,9 +386,7 @@ class mavlink_object:
self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
# 記錄訊息過濾類型 (可選)
# 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE,
# 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])
self.bridge_msg_types = set([0, 30, 32, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED
self.return_msg_types = set([])
# 轉發到別的 mavlink object 作為目標端口 的列表
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+36 -228
View File
@@ -1,13 +1,11 @@
"""
MAVLink ROS2 Nodes
主要包含三個獨立的 ROS2 Node :
主要包含兩個獨立的 ROS2 Node :
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
以最基礎的 mavlink microservice 會實現目標
並不會包含額外的功能
3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
過期丟棄、去重、節流、分片
與一個節點管理器
- fc_ros_manager
@@ -17,7 +15,6 @@ MAVLink ROS2 Nodes
import os
import time
import math
import hashlib
import threading
from typing import Dict, Optional
@@ -25,7 +22,6 @@ from typing import Dict, Optional
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
# ROS2 Message imports
import std_msgs.msg
@@ -36,7 +32,6 @@ import mavros_msgs.msg
# ROS2 Service imports
import fc_interfaces.srv as fcsrv
import fc_interfaces.msg as fcmsg
from .ackEnum import serviceAckResult
# 自定義 imports
@@ -65,7 +60,7 @@ class PublishRateController:
# 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
# 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
self.topic_intervals = {
'position_gnss': 0.0, # GNSS位置
'position': 0.0, # GNSS位置
'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度)
'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態)
'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除)
@@ -135,8 +130,8 @@ class VehicleStatusPublisher(Node):
self.fc_publishers: Dict[tuple, any] = {}
# 定時器：以較高頻率檢查 vehicle_registry 並發布
# 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
self.timer_period = 0.05 # 50ms
# 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
self.timer_period = 0.1 # 100ms
self.timer = self.create_timer(self.timer_period, self.timer_callback)
# 狀態標誌
@@ -175,7 +170,7 @@ class VehicleStatusPublisher(Node):
# 例如：self._publish_ekf_status(sysid, status)
# ═══════════════════════════════════════════════════════════════
# 發布各種狀態（通過頻率控制器判斷是否發布）
self._publish_position_gnss(sysid, status)
self._publish_position(sysid, status)
self._publish_position_ned(sysid, status)
self._publish_attitude(sysid, status)
self._publish_velocity(sysid, status)
@@ -206,9 +201,9 @@ class VehicleStatusPublisher(Node):
logger.info(f"Created publisher: {topic_name}")
return self.fc_publishers[key]
def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
"""發布 GPS 位置"""
if not self.rate_controller.should_publish(sysid, 'position_gnss'):
if not self.rate_controller.should_publish(sysid, 'position'):
return
pos = status.position
@@ -216,32 +211,27 @@ class VehicleStatusPublisher(Node):
return
publisher = self._get_or_create_publisher(
sysid, 'position_gnss', fcmsg.GnssRaw
sysid, 'position', sensor_msgs.msg.NavSatFix
)
# 檢查是否有訂閱者
if publisher.get_subscription_count() == 0:
return
msg = fcmsg.GnssRaw()
msg.stamp = self.get_clock().now().to_msg()
msg.latitude = float(pos.latitude)
msg.longitude = float(pos.longitude)
msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
msg = sensor_msgs.msg.NavSatFix()
msg.latitude = pos.latitude
msg.longitude = pos.longitude
msg.altitude = pos.altitude if pos.altitude is not None else 0.0
# GPS 狀態資訊
gps = status.gps
if gps.fix_type is not None:
msg.fix_type = int(gps.fix_type)
if gps.satellites_visible is not None:
msg.satellites_visible = int(gps.satellites_visible)
msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus
publisher.publish(msg)
def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
"""發布 LOCAL_POSITION_NED (NED 座標，含位置與速度）"""
"""發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
if not self.rate_controller.should_publish(sysid, 'position_ned'):
return
@@ -277,28 +267,34 @@ class VehicleStatusPublisher(Node):
publisher.publish(msg)
def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
"""發布原始姿態（roll/pitch/yaw 與角速度）"""
"""發布姿態（IMU）"""
if not self.rate_controller.should_publish(sysid, 'attitude'):
return
att = status.attitude
if att.roll is None or att.pitch is None or att.yaw is None:
if att.roll is None:
return
publisher = self._get_or_create_publisher(
sysid, 'attitude', fcmsg.AttitudeRaw
)
publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
if publisher.get_subscription_count() == 0:
return
msg = fcmsg.AttitudeRaw()
msg.stamp = self.get_clock().now().to_msg()
msg.roll = float(att.roll)
msg.pitch = float(att.pitch)
msg.yaw = float(att.yaw)
msg.rollspeed = float(att.rollspeed) if att.rollspeed is not None else 0.0
msg.pitchspeed = float(att.pitchspeed) if att.pitchspeed is not None else 0.0
msg.yawspeed = float(att.yawspeed) if att.yawspeed is not None else 0.0
msg = sensor_msgs.msg.Imu()
# 歐拉角轉四元數
qx, qy, qz, qw = self._euler_to_quaternion(
att.roll, att.pitch, att.yaw
)
msg.orientation.x = qx
msg.orientation.y = qy
msg.orientation.z = qz
msg.orientation.w = qw
# 角速度
if att.rollspeed is not None:
msg.angular_velocity.x = att.rollspeed
msg.angular_velocity.y = att.pitchspeed
msg.angular_velocity.z = att.yawspeed
publisher.publish(msg)
@@ -440,7 +436,7 @@ class VehicleStatusPublisher(Node):
# 'longitude': status.position.longitude if status.position.longitude else 0.0,
# 'altitude': status.position.altitude if status.position.altitude else 0.0,
# 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
# 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
'connection_type': vehicle.connected_via.value,
'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
}
@@ -959,179 +955,6 @@ class MavlinkCommandService(Node):
logger.info("MavlinkCommandService stopped")
# ============================================================================
# RtcmRelay Node
# ============================================================================
class RtcmRelay(Node):
"""
RTCM 轉發節點 - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
職責:
- 訂閱外部 RTCM 資料（mavros_msgs/msg/RTCM）
- 過期丟棄：封包 Header.stamp 超過 1 秒則丟棄
- 去重：與上一筆 payload blake2s hash 相同則丟棄
- 節流：最短轉發間隔保護（可參數化）
- 對所有在線載具直接編碼並送出 MAVLink GPS_RTCM_DATA（含分片）
MAVLink GPS_RTCM_DATA (msg_id=233) 每片 payload 上限 180 bytes
超過 180 bytes 需分片，最多 4 片 = 720 bytes
"""
FRAGMENT_MAX_LEN = 180
MAX_TOTAL_LEN = 4 * 180 # 720 bytes
STALE_THRESHOLD_SEC = 1.0
def __init__(self, min_forward_interval_ms: float = 0.0, clock_offset_ms: float = 0.0):
super().__init__('rtcm_relay')
self.running = True
self.min_forward_interval_sec = min_forward_interval_ms / 1000.0
self.clock_offset_sec = clock_offset_ms / 1000.0
# 去重：上一筆 payload 的短 hash
self._last_hash: Optional[bytes] = None
# 節流：上次成功轉發的時間
self._last_forward_time: float = 0.0
# MAVLink GPS_RTCM_DATA 分片序號 (0~31 循環，同一筆 RTCM 的所有分片共用)
self._seq: int = 0
# 觀測 counters
self.counters = {
'received': 0,
'forwarded': 0,
'dropped_stale': 0,
'dropped_duplicate': 0,
'dropped_throttle': 0,
'dropped_oversize': 0,
'no_route': 0,
'send_fail': 0,
}
# QoS: 低延遲、弱可靠 — 寧可掉舊包也不排隊
rtcm_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
)
self.subscription = self.create_subscription(
mavros_msgs.msg.RTCM,
'/fc_network/rtcm_input',
self._rtcm_callback,
rtcm_qos,
)
logger.info("RtcmRelay initialized")
# ── callback ──────────────────────────────────────────────────────
def _rtcm_callback(self, msg):
"""處理順序：過期檢查 → 去重 → 節流 → 轉發"""
if not self.running:
return
self.counters['received'] += 1
data = bytes(msg.data)
now = time.time()
# 1) 過期丟棄
stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
age = (now + self.clock_offset_sec) - stamp_sec
if age > self.STALE_THRESHOLD_SEC:
self.counters['dropped_stale'] += 1
return
# 2) 去重：blake2s 短 hash 與上一筆比對
h = hashlib.blake2s(data, digest_size=8).digest()
if h == self._last_hash:
self.counters['dropped_duplicate'] += 1
return
self._last_hash = h
# 3) 節流：最短轉發間隔
if self.min_forward_interval_sec > 0:
if (now - self._last_forward_time) < self.min_forward_interval_sec:
self.counters['dropped_throttle'] += 1
return
# 4) 超長丟棄
if len(data) > self.MAX_TOTAL_LEN:
self.counters['dropped_oversize'] += 1
logger.warning(f"RTCM data too large ({len(data)} bytes), max {self.MAX_TOTAL_LEN}")
return
# 5) 取得本次的分片序號，然後遞增給下一筆
seq = self._seq
self._seq = (self._seq + 1) & 0x1F
# 6) 轉發給所有載具
forwarded_any = False
for sysid, vehicle in mvv.vehicle_registry.read_all():
socket_id = vehicle.custom_meta.get("socket_id")
if socket_id is None:
self.counters['no_route'] += 1
continue
mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
if mav_obj is None:
self.counters['no_route'] += 1
continue
try:
self._send_rtcm(mav_obj, data, seq)
forwarded_any = True
except Exception as e:
self.counters['send_fail'] += 1
logger.debug(f"RTCM send fail sysid={sysid}: {e}")
if forwarded_any:
self.counters['forwarded'] += 1
self._last_forward_time = now
# ── MAVLink 編碼與分片 ────────────────────────────────────────────
def _send_rtcm(self, mav_obj, data: bytes, seq: int):
"""
對單一載具送出 GPS_RTCM_DATA 必要時做分片
GPS_RTCM_DATA flags 欄位:
bit 0 : 1=fragmented
bit 1-2 : fragment_id (0~3)
bit 3-7 : sequence number (0~31)
"""
if len(data) <= self.FRAGMENT_MAX_LEN:
flags = (seq & 0x1F) << 3
padded = data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(data))
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(data), padded)
else:
fragments = [
data[i:i + self.FRAGMENT_MAX_LEN]
for i in range(0, len(data), self.FRAGMENT_MAX_LEN)
]
for frag_id, frag_data in enumerate(fragments):
flags = 1 | ((frag_id & 0x03) << 1) | ((seq & 0x1F) << 3)
padded = frag_data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(frag_data))
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(frag_data), padded)
# ── 外部介面 ──────────────────────────────────────────────────────
def set_clock_offset(self, offset_ms: float):
"""更新時鐘偏移量（毫秒），用於與 RTCM 來源的時間對齊"""
self.clock_offset_sec = offset_ms / 1000.0
def get_counters(self) -> dict:
return dict(self.counters)
def stop(self):
"""停止轉發"""
self.running = False
logger.info("RtcmRelay stopped")
# ============================================================================
# ROS2 節點管理器
# ============================================================================
@@ -1145,10 +968,9 @@ class fc_ros_manager:
stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
shutdown 是完全關閉 ROS2 並銷毀節點實例。
管理三個獨立的 ROS2 Node：
管理兩個獨立的 ROS2 Node：
- VehicleStatusPublisher
- MavlinkCommandService
- RtcmRelay
提供統一的啟動/停止介面給 mainOrchestrator
"""
@@ -1157,10 +979,9 @@ class fc_ros_manager:
self.initialized = False
self.running = False
# node 實例
# 两个 node 實例
self.status_publisher: Optional[VehicleStatusPublisher] = None
self.command_service: Optional[MavlinkCommandService] = None
self.rtcm_relay: Optional[RtcmRelay] = None
# Executor & Thread
self.spin_thread: Optional[threading.Thread] = None
@@ -1180,13 +1001,11 @@ class fc_ros_manager:
# 創建節點 node
self.status_publisher = VehicleStatusPublisher()
self.command_service = MavlinkCommandService()
self.rtcm_relay = RtcmRelay()
# 創建執行者 MultiThreadedExecutor 並把 node 加入其中
self.executor = MultiThreadedExecutor()
self.executor.add_node(self.status_publisher)
self.executor.add_node(self.command_service)
self.executor.add_node(self.rtcm_relay)
self.initialized = True
# logger.info("fc_ros_manager initialized")
@@ -1250,8 +1069,6 @@ class fc_ros_manager:
self.status_publisher.stop()
if self.command_service:
self.command_service.stop()
if self.rtcm_relay:
self.rtcm_relay.stop()
# 等待 spin 執行緒結束
if self.spin_thread and self.spin_thread.is_alive():
@@ -1276,8 +1093,6 @@ class fc_ros_manager:
self.status_publisher.destroy_node()
if self.command_service:
self.command_service.destroy_node()
if self.rtcm_relay:
self.rtcm_relay.destroy_node()
# 關閉 ROS2
if rclpy.ok():
@@ -1295,7 +1110,6 @@ class fc_ros_manager:
'running': self.running,
'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
'command_service_active': self.command_service is not None,
'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
}
@@ -1323,13 +1137,7 @@ ros2_manager = fc_ros_manager()
2026.04.07
1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
2. 優化 response.ack_result 回傳值的資訊
3. 新增 _publish_position_ned() 發布剛體座標的訊息
4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
2026.05.11
1. 新增 RtcmRelay node - 訂閱 mavros_msgs/RTCM topic 轉發為 MAVLink GPS_RTCM_DATA
2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
TODO
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
src/fc_network_adapter/fc_network_adapter/serialManager.py
+4 -10
View File
@@ -38,7 +38,6 @@ class SerialMode(Enum):
"""連接類型"""
STRAIGHT = auto() # 原始數據直通
XBEEAPI2AT = auto() # XBee API 模式
XBEEAPI_POLL = auto()
NOT_USE = auto() # 不使用
@@ -177,10 +176,10 @@ class XBeeFrameProcessor(FrameProcessor):
"""根據 frame type 分派；若是 RX payload 回傳 bytes，其餘回傳 None"""
frame_type = frame[3]
if frame_type == self.FRAME_TYPE_RX_PACKET: # mavlink
if frame_type == self.FRAME_TYPE_RX_PACKET:
return self._decapsulate(frame)
if frame_type == self.FRAME_TYPE_AT_RESPONSE: # AT command
if frame_type == self.FRAME_TYPE_AT_RESPONSE:
if self.at_handler is not None:
self.at_handler.handle_frame(frame)
return None
@@ -336,10 +335,9 @@ class ATCommandHandler:
def _handle_rssi(self, data: bytes):
"""處理 DB (RSSI) 回應：單 byte 無號值，單位 dBm"""
pass
if data:
print(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
# if data:
# print(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
# logger.debug(f"[{self.serial_port}] RSSI = -{data[0]} dBm") # dev
pass
def _handle_serial_high(self, data: bytes):
"""處理 SH (Serial Number High)"""
@@ -372,10 +370,6 @@ class SerialHandler(asyncio.Protocol):
at_handler = ATCommandHandler(self.serial_port_str)
return XBeeFrameProcessor(at_handler=at_handler)
# if self.serial_mode == SerialMode.XBEEAPI_POLL:
# at_handler = ATCommandHandler_new(self.serial_port_str)
# return XBeeFrameProcessor(at_handler=at_handler)
logger.warning(f"Unknown serial mode: {self.serial_mode}, using Raw")
return RawFrameProcessor()
src/fc_network_adapter/package.xml
-7
View File
@@ -7,13 +7,6 @@
<maintainer email="chiyu1468@hotmail.com">picars</maintainer>
<license>TODO: License declaration</license>
<exec_depend>fc_interfaces</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>mavros_msgs</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
+8 -9
View File
@@ -17,7 +17,6 @@ import std_msgs.msg
import sensor_msgs.msg
import geometry_msgs.msg
import mavros_msgs.msg
import fc_interfaces.msg
# 專案 imports
from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -40,7 +39,7 @@ class TestSubscriber(Node):
self.sysid = sysid
self.received_messages = {
'position_gnss': [],
'position': [],
'attitude': [],
'velocity': [],
'battery': [],
@@ -61,9 +60,9 @@ class TestSubscriber(Node):
# Position
self.create_subscription(
fc_interfaces.msg.GnssRaw,
f'{base_topic}/position_gnss',
lambda msg: self._on_message('position_gnss', msg),
sensor_msgs.msg.NavSatFix,
f'{base_topic}/position',
lambda msg: self._on_message('position', msg),
10
)
@@ -122,7 +121,7 @@ class TestSubscriber(Node):
def _format_msg(self, topic_name: str, msg) -> str:
"""格式化消息以便顯示"""
if topic_name == 'position_gnss':
if topic_name == 'position':
return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
elif topic_name == 'attitude':
return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -265,7 +264,7 @@ def test_basic_publishing():
# 檢查收到的消息
print("\n--- 消息統計 ---")
total_messages = 0
for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
count = test_sub.get_message_count(topic)
total_messages += count
print(f" {topic:15s}: {count:3d} 條消息")
@@ -305,7 +304,7 @@ def test_frequency_control():
# 修改頻率設定
publisher_node = ros2_manager.status_publisher
publisher_node.rate_controller.topic_intervals = {
'position_gnss': 1.5,
'position': 1.5,
'attitude': 1.0,
'velocity': 1.0,
'battery': 1.0,
@@ -330,7 +329,7 @@ def test_frequency_control():
print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
print("2Hz Topics (預期 ~6 條):")
for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
count = test_sub.get_message_count(topic)
print(f" {topic:15s}: {count:3d} 條")
for topic in ['battery', 'mode', 'summary']:
src/fc_network_apps/longCommand.py
-46
View File
@@ -14,7 +14,6 @@ from fc_interfaces.srv import MavCommandLong
COMMAND_DO_SET_MODE = 176
COMMAND_NAV_LAND = 21
COMMAND_NAV_TAKEOFF = 22
COMMAND_PREFLIGHT_REBOOT_SHUTDOWN = 246
COMMAND_COMPONENT_ARM_DISARM = 400
DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
@@ -192,28 +191,6 @@ class CommandLongClient(Node):
timeout_sec=timeout_sec,
)
def reboot_autopilot(
self,
*,
target_sysid: int,
target_compid: int = 0,
timeout_sec: float = DEFAULT_TIMEOUT_SEC,
) -> CommandLongResult:
return self._send_command_long(
target_sysid=target_sysid,
target_compid=target_compid,
command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
confirmation=0,
param1=1.0,
param2=0.0,
param3=0.0,
param4=0.0,
param5=0.0,
param6=0.0,
param7=0.0,
timeout_sec=timeout_sec,
)
# ============================================================================
# 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
# 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
@@ -376,29 +353,6 @@ class CommandLongClient(Node):
timeout_sec=timeout_sec,
)
async def reboot_autopilot_async(
self,
*,
target_sysid: int,
target_compid: int = 0,
timeout_sec: float = DEFAULT_TIMEOUT_SEC,
) -> CommandLongResult:
"""非阻塞 async 版本的 autopilot reboot."""
return await self._send_command_long_async(
target_sysid=target_sysid,
target_compid=target_compid,
command=COMMAND_PREFLIGHT_REBOOT_SHUTDOWN,
confirmation=0,
param1=1.0,
param2=0.0,
param3=0.0,
param4=0.0,
param5=0.0,
param6=0.0,
param7=0.0,
timeout_sec=timeout_sec,
)
async def land_async(
self,
*,
src/fc_network_module/fc_network_module/__init__.py
View File
src/fc_network_module/fc_network_module/ntrip_client.py
-146
View File
@@ -1,146 +0,0 @@
import socket
import base64
import threading
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
from mavros_msgs.msg import RTCM
class NtripClientNode(Node):
"""連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
RECONNECT_BASE_SEC = 2.0
RECONNECT_MAX_SEC = 60.0
def __init__(self):
super().__init__('ntrip_client')
self.declare_parameter('host', 'rtk2go.com')
self.declare_parameter('port', 2101)
self.declare_parameter('mountpoint', '')
self.declare_parameter('username', '')
self.declare_parameter('password', '')
rtcm_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
)
self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
self._running = True
self._thread = threading.Thread(target=self._receive_loop, daemon=True)
self._thread.start()
self.get_logger().info('NtripClientNode 已啟動')
# ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
def _receive_loop(self):
backoff = self.RECONNECT_BASE_SEC
while self._running:
host = self.get_parameter('host').value
port = self.get_parameter('port').value
mount = self.get_parameter('mountpoint').value
user = self.get_parameter('username').value
pwd = self.get_parameter('password').value
if not mount:
self.get_logger().error('mountpoint 參數未設定，等待重試...')
time.sleep(backoff)
continue
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.settimeout(10)
try:
self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
sock.connect((host, port))
auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
request = (
f'GET /{mount} HTTP/1.0\r\n'
f'User-Agent: NTRIP PythonClient\r\n'
f'Authorization: Basic {auth}\r\n'
f'Connection: close\r\n\r\n'
)
sock.sendall(request.encode())
resp = sock.recv(4096)
if b'ICY 200 OK' not in resp:
self.get_logger().error(
f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
)
raise ConnectionError('handshake failed')
self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
backoff = self.RECONNECT_BASE_SEC
self._read_stream(sock)
except Exception as e:
if self._running:
self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
time.sleep(backoff)
backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
finally:
sock.close()
def _read_stream(self, sock: socket.socket):
buf = b''
while self._running:
try:
chunk = sock.recv(4096)
except socket.timeout:
continue
if not chunk:
break
buf += chunk
while len(buf) >= 6:
if buf[0] != 0xD3:
buf = buf[1:]
continue
payload_len = ((buf[1] & 0x03) << 8) | buf[2]
frame_len = payload_len + 6 # header(3) + payload + CRC(3)
if len(buf) < frame_len:
break
packet = buf[:frame_len]
buf = buf[frame_len:]
self._publish_rtcm(packet)
def _publish_rtcm(self, raw_packet: bytes):
msg = RTCM()
msg.header.stamp = self.get_clock().now().to_msg()
msg.data = list(raw_packet)
self.publisher_.publish(msg)
def destroy_node(self):
self._running = False
self._thread.join(timeout=3.0)
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = NtripClientNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/fc_network_module/package.xml
-22
View File
@@ -1,22 +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>fc_network_module</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="chiyu1468@hotmail.com">picars</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>mavros_msgs</depend>
<depend>std_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/fc_network_module/resource/fc_network_module
View File
src/fc_network_module/setup.cfg
-4
View File
@@ -1,4 +0,0 @@
[develop]
script_dir=$base/lib/fc_network_module
[install]
install_scripts=$base/lib/fc_network_module
src/fc_network_module/setup.py
-26
View File
@@ -1,26 +0,0 @@
from setuptools import find_packages, setup
package_name = 'fc_network_module'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='picars',
maintainer_email='chiyu1468@hotmail.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'ntrip_client = fc_network_module.ntrip_client:main',
],
},
)
src/fc_network_module/test/test_copyright.py
-25
View File
@@ -1,25 +0,0 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
src/fc_network_module/test/test_flake8.py
-25
View File
@@ -1,25 +0,0 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
src/fc_network_module/test/test_pep257.py
-23
View File
@@ -1,23 +0,0 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
src/unitdev02/unitdev02/devnote.txt
+1 -13
View File
@@ -7,17 +7,9 @@
- 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複)
這一步
希望 ackEnum 可以擺到正確的位置
node 增加 GNSS 訊息 (2d fix 3d fix rtk float...)
RTK
跟 RTK2go 抓取列表
從特定 mount point 得到數據
做一個 ros2 service(action?) 接到數據並包裝
研究 ros2 service
下一步
NODE 重啟
可以 refresh node topic
下下一步
@@ -26,8 +18,6 @@
rssi 資訊提取s
mavlink timeout - CommLONG 這個是 mavlink bus 沒有回應 可能是丟包了
自己的常用指令
python -m fc_network_adapter.tests.test_vehicleStatusPublisher
@@ -37,8 +27,6 @@ python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
python -m someotherpkg.src.example_takeoff_land
python -m someotherpkg.src.example_change_mode
python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com
ros2 topic list
ros2 topic echo /fc_network/vehicle/sys3/battery
src/unitdev04/esp32-final.py
-151
View File
@@ -1,151 +0,0 @@
from machine import UART
import time
import struct
import gc
# ================= 設定區 =================
FC_BAUDRATE = 115200
XB_BAUDRATE = 115200
DEST_64 = b'\x00\x13\xA2\x00\x42\x5B\x9D\xC8'
XBEE_MAX_PAYLOAD = 100
MY_SYSID = 15 # 第二台就改成 10，第三台 15
POLL_MAGIC = b'POLL'
tx_buf = bytearray()
MAX_BUF_SIZE = 6144 # 保留安全餘裕的緩衝區大小
# =========================================
uart_fc = UART(1, baudrate=FC_BAUDRATE, tx=32, rx=33, rxbuf=4096)
uart_xb = UART(2, baudrate=XB_BAUDRATE, tx=25, rx=26, rxbuf=4096)
rx_buf = bytearray()
def get_checksum(data):
return 0xFF - (sum(data) & 0xFF)
def send_to_xbee_chunked(payload):
total_len = len(payload)
sent_len = 0
while sent_len < total_len:
end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
chunk = payload[sent_len : end_len]
sent_len = end_len
frame_content = bytearray()
frame_content.append(0x10)
frame_content.append(0x00) # 0x00 代表不要求 XBee ACK，極大加快發送速度
frame_content.extend(DEST_64)
frame_content.extend(b'\xFF\xFE')
frame_content.append(0x00)
frame_content.append(0x00)
frame_content.extend(chunk)
length = len(frame_content)
checksum = get_checksum(frame_content)
packet = bytearray()
packet.append(0x7E)
packet.append((length >> 8) & 0xFF)
packet.append(length & 0xFF)
packet.extend(frame_content)
packet.append(checksum)
uart_xb.write(packet)
# 僅需極短暫延遲，讓硬體發送緩衝區消化即可
time.sleep_ms(1)
def process_xbee_buffer():
global rx_buf, tx_buf
while True:
start_pos = rx_buf.find(b'\x7E')
if start_pos == -1:
rx_buf = bytearray() # 避免用 clear()
return
if start_pos > 0:
rx_buf = rx_buf[start_pos:]
if len(rx_buf) < 3: return
pkt_len = (rx_buf[1] << 8) | rx_buf[2]
total_len = pkt_len + 4
if pkt_len > 300:
rx_buf = rx_buf[1:]
continue
if len(rx_buf) < total_len: return
packet = rx_buf[3 : 3+pkt_len]
checksum_recv = rx_buf[3+pkt_len]
if get_checksum(packet) == checksum_recv:
if packet[0] == 0x90:
real_data = packet[12:]
if len(real_data) == 5 and real_data.startswith(POLL_MAGIC):
if real_data[4] == MY_SYSID:
flush_tx_buffer()
else:
uart_fc.write(real_data)
rx_buf = rx_buf[total_len:]
else:
rx_buf = rx_buf[1:]
def flush_tx_buffer():
global tx_buf
if len(tx_buf) == 0:
return
# 單次最大發送量設為 1200 bytes，避免在空中飛行超過時槽導致碰撞
send_limit = min(len(tx_buf), 1200)
cut_idx = send_limit
if send_limit < len(tx_buf):
last_fd_pos = tx_buf[:send_limit].rfind(b'\xFD')
if last_fd_pos > 0:
cut_idx = last_fd_pos
data_to_send = tx_buf[:cut_idx]
tx_buf = tx_buf[cut_idx:]
send_to_xbee_chunked(data_to_send)
# 初始清理
gc.collect()
while True:
try:
# FC -> 緩衝區
if uart_fc.any():
data = uart_fc.read(250)
if data:
tx_buf.extend(data)
# 聰明的溫和防爆機制 (砍掉舊的 2/3，保留最新 1/3)
if len(tx_buf) > MAX_BUF_SIZE:
safe_cut = tx_buf.find(b'\xFD', (MAX_BUF_SIZE // 3) * 2)
if safe_cut != -1:
tx_buf = tx_buf[safe_cut:]
else:
tx_buf = tx_buf[(MAX_BUF_SIZE // 2):]
# XBee -> FC
if uart_xb.any():
chunk = uart_xb.read()
if chunk:
rx_buf.extend(chunk)
process_xbee_buffer()
time.sleep_ms(1)
except MemoryError:
# 發生 OOM 時的「斷尾求生」法，強制捨棄舊變數以釋放空間，防止死機
tx_buf = bytearray()
rx_buf = bytearray()
gc.collect()
time.sleep_ms(10)
except Exception as e:
time.sleep_ms(2)
src/unitdev04/esp32.py
+635
View File
@@ -0,0 +1,635 @@
from machine import UART
import time
import struct
import gc
# =========================================================
# ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge
# Packet-size TDMA + DONE + SYSID / GCS address 自動學習版
#
# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
#
# 重要：
# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。
# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。
# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING；
# 之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。
#
# 封包格式：
# GCS -> UAV:
# DISC
# POLL + target_sysid(1) + grant_bytes(2)
# COMM / RTK broadcast downlink bytes
#
# UAV -> GCS:
# HELO + sysid(1)
# DONE + sysid(1) + sent_len(2) + remain_len(2)
#
# 注意：
# - HELO 不帶 remain。
# - DONE 保留 remain。
# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
# - DEST_64 不寫死，由 DISC 的 XBee 0x90 src64 自動學。
# - POLL 只負責 TDMA 授權；COMM / RTK 直接 forward。
# =========================================================
# ================= 設定區 =================
FC_BAUDRATE = 115200
XB_BAUDRATE = 115200
DEST_64 = None
MY_SYSID = None
SYSID_LEARN_CONFIRM_COUNT = 3
_sysid_candidate = None
_sysid_candidate_count = 0
# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。
# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。
DISC_PENDING = False
XBEE_MAX_PAYLOAD = 100
# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。
DISC_MAGIC = b'DISC'
# UAV -> GCS hello：
# HELO + sysid(1)
HELLO_MAGIC = b'HELO'
HELLO_MIN_INTERVAL_MS = 1000
_last_hello_ms = 0
# 保留此變數只作狀態記錄；本版 DISC 會 force=True，所以不會被 HELLO_SENT 擋掉。
HELLO_SENT = False
POLL_MAGIC = b'POLL'
DEFAULT_GRANT_BYTES = 600
DONE_MAGIC = b'DONE'
MAX_BUF_SIZE = 6144
READ_FC_BYTES = 250
FC_UART_ID = 1
FC_TX_PIN = 32
FC_RX_PIN = 33
XB_UART_ID = 2
XB_TX_PIN = 25
XB_RX_PIN = 26
# =========================================
uart_fc = UART(FC_UART_ID, baudrate=FC_BAUDRATE, tx=FC_TX_PIN, rx=FC_RX_PIN, rxbuf=4096)
uart_xb = UART(XB_UART_ID, baudrate=XB_BAUDRATE, tx=XB_TX_PIN, rx=XB_RX_PIN, rxbuf=4096)
tx_buf = bytearray() # FC -> GCS 等待 TDMA poll 的 MAVLink stream
rx_buf = bytearray() # XBee API frame parser buffer
# =========================================================
# 基本工具函式
# =========================================================
def get_checksum(data):
return 0xFF - (sum(data) & 0xFF)
def is_valid_addr64(addr):
if addr is None:
return False
if len(addr) != 8:
return False
if addr == b'\x00\x00\x00\x00\x00\x00\x00\x00':
return False
return True
def learn_dest64_from_src64(src64):
"""
UAV 收到 GCS 的 XBee 0x90 frame 時，
0x90 裡面的 src64 就是 GCS / coordinator XBee 的 64-bit address。
"""
global DEST_64
global HELLO_SENT
global DISC_PENDING
if not is_valid_addr64(src64):
return False
src64 = bytes(src64)
if DEST_64 is None:
DEST_64 = src64
HELLO_SENT = False
return True
if DEST_64 != src64:
DEST_64 = src64
HELLO_SENT = False
DISC_PENDING = False
return True
return False
def build_api_tx_frame(payload):
"""
建立 XBee API frame type 0x10，把 payload 送到 DEST_64。
若 DEST_64 尚未學到，回傳 None。
"""
if DEST_64 is None:
return None
frame_content = bytearray()
frame_content.append(0x10)
frame_content.append(0x00)
frame_content.extend(DEST_64)
frame_content.extend(b'\xFF\xFE')
frame_content.append(0x00)
frame_content.append(0x00)
frame_content.extend(payload)
length = len(frame_content)
packet = bytearray()
packet.append(0x7E)
packet.append((length >> 8) & 0xFF)
packet.append(length & 0xFF)
packet.extend(frame_content)
packet.append(get_checksum(frame_content))
return packet
def send_to_xbee_chunked(payload):
"""
把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。
"""
if DEST_64 is None:
return False
total_len = len(payload)
sent_len = 0
while sent_len < total_len:
end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
chunk = payload[sent_len:end_len]
sent_len = end_len
pkt = build_api_tx_frame(chunk)
if pkt is None:
return False
uart_xb.write(pkt)
time.sleep_ms(1)
return True
# =========================================================
# HELO / DONE 回報
# =========================================================
def send_hello_report(force=False):
"""
UAV 回報自己存在：
HELO + MY_SYSID
本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。
"""
global _last_hello_ms
global HELLO_SENT
if MY_SYSID is None:
return False
if DEST_64 is None:
return False
if HELLO_SENT and not force:
return False
now = time.ticks_ms()
if not force:
if time.ticks_diff(now, _last_hello_ms) < HELLO_MIN_INTERVAL_MS:
return False
payload = HELLO_MAGIC + struct.pack('>B', MY_SYSID)
pkt = build_api_tx_frame(payload)
if pkt is None:
return False
uart_xb.write(pkt)
_last_hello_ms = now
HELLO_SENT = True
time.sleep_ms(1)
return True
def try_send_pending_hello():
"""
若先收到 DISC、後學到 MY_SYSID，這裡會自動補送 HELO。
"""
global DISC_PENDING
if DISC_PENDING and MY_SYSID is not None and DEST_64 is not None:
delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
time.sleep_ms(delay_ms)
if send_hello_report(force=True):
DISC_PENDING = False
def send_done_report(sent_len, remain_len):
"""
DONE payload:
b'DONE' + sysid(1) + sent_len(2) + remain_len(2)
"""
if MY_SYSID is None:
return False
if DEST_64 is None:
return False
if sent_len > 65535:
sent_len = 65535
if remain_len > 65535:
remain_len = 65535
payload = DONE_MAGIC + struct.pack('>BHH', MY_SYSID, sent_len, remain_len)
pkt = build_api_tx_frame(payload)
if pkt is None:
return False
uart_xb.write(pkt)
time.sleep_ms(1)
return True
# =========================================================
# MAVLink frame 解析與 MY_SYSID 自動學習
# =========================================================
def find_first_mavlink_magic(buf):
pos_fe = buf.find(b'\xFE')
pos_fd = buf.find(b'\xFD')
if pos_fe == -1:
return pos_fd
if pos_fd == -1:
return pos_fe
return pos_fe if pos_fe < pos_fd else pos_fd
def mavlink_frame_length(buf, start_idx):
if start_idx >= len(buf):
return None
magic = buf[start_idx]
if magic == 0xFE:
if len(buf) - start_idx < 2:
return None
payload_len = buf[start_idx + 1]
total_len = payload_len + 8
if len(buf) - start_idx < total_len:
return None
return total_len
if magic == 0xFD:
if len(buf) - start_idx < 3:
return None
payload_len = buf[start_idx + 1]
incompat_flags = buf[start_idx + 2]
signed = (incompat_flags & 0x01) != 0
total_len = payload_len + 12 + (13 if signed else 0)
if len(buf) - start_idx < total_len:
return None
return total_len
return None
def get_mavlink_sysid(buf, start_idx):
if start_idx >= len(buf):
return None
magic = buf[start_idx]
if magic == 0xFE:
if len(buf) - start_idx >= 6:
return buf[start_idx + 3]
if magic == 0xFD:
if len(buf) - start_idx >= 10:
return buf[start_idx + 5]
return None
def learn_my_sysid_from_tx_buf():
"""
從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame，
並自動學習飛控的 MAVLink SYSID。
若更改飛控 SYSID，建議重新上電 ESP32，讓 MY_SYSID 重新學習。
"""
global MY_SYSID
global _sysid_candidate
global _sysid_candidate_count
global HELLO_SENT
global tx_buf
# 已經學到 SYSID 後，不再掃 tx_buf，減少 ESP32 運算量
if MY_SYSID is not None:
return
if len(tx_buf) == 0:
return
idx = 0
checked = 0
while idx < len(tx_buf) and checked < 8:
sub = tx_buf[idx:]
rel_start = find_first_mavlink_magic(sub)
if rel_start == -1:
return
start = idx + rel_start
frame_len = mavlink_frame_length(tx_buf, start)
if frame_len is None:
return
sysid = get_mavlink_sysid(tx_buf, start)
if sysid is not None and sysid > 0:
if _sysid_candidate == sysid:
_sysid_candidate_count += 1
else:
_sysid_candidate = sysid
_sysid_candidate_count = 1
if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT:
MY_SYSID = _sysid_candidate
HELLO_SENT = False
return
idx = start + frame_len
checked += 1
# =========================================================
# tx_buf MAVLink frame 取出與裁切
# =========================================================
def pop_mavlink_frames_by_quota(quota_bytes):
global tx_buf
if quota_bytes <= 0 or len(tx_buf) == 0:
return b''
start = find_first_mavlink_magic(tx_buf)
if start == -1:
tx_buf = bytearray()
return b''
if start > 0:
tx_buf = tx_buf[start:]
out = bytearray()
while len(tx_buf) > 0:
if tx_buf[0] not in (0xFE, 0xFD):
start = find_first_mavlink_magic(tx_buf)
if start == -1:
tx_buf = bytearray()
break
tx_buf = tx_buf[start:]
frame_len = mavlink_frame_length(tx_buf, 0)
if frame_len is None:
break
if len(out) > 0 and (len(out) + frame_len) > quota_bytes:
break
if len(out) == 0 and frame_len > quota_bytes:
out.extend(tx_buf[:frame_len])
tx_buf = tx_buf[frame_len:]
break
out.extend(tx_buf[:frame_len])
tx_buf = tx_buf[frame_len:]
if len(out) >= quota_bytes:
break
return bytes(out)
def trim_tx_buffer_if_needed():
global tx_buf
if len(tx_buf) <= MAX_BUF_SIZE:
return
target_size = MAX_BUF_SIZE // 2
while len(tx_buf) > target_size:
start = find_first_mavlink_magic(tx_buf)
if start == -1:
tx_buf = bytearray()
return
if start > 0:
tx_buf = tx_buf[start:]
continue
frame_len = mavlink_frame_length(tx_buf, 0)
if frame_len is None:
if len(tx_buf) > target_size:
tx_buf = tx_buf[-target_size:]
return
tx_buf = tx_buf[frame_len:]
def flush_tx_buffer(grant_bytes):
global tx_buf
# DEST_64 未學到時不能送資料，保留防護避免先 pop tx_buf 造成資料遺失
if DEST_64 is None:
return
if grant_bytes <= 0:
send_done_report(0, len(tx_buf))
return
data_to_send = pop_mavlink_frames_by_quota(grant_bytes)
sent_len = len(data_to_send)
if sent_len > 0:
send_to_xbee_chunked(data_to_send)
send_done_report(sent_len, len(tx_buf))
# =========================================================
# GCS control payload 解析
# =========================================================
def parse_poll_payload(real_data):
if not real_data.startswith(POLL_MAGIC):
return None, None
if len(real_data) == 5:
return real_data[4], DEFAULT_GRANT_BYTES
if len(real_data) == 7:
target_sysid = real_data[4]
grant_bytes = (real_data[5] << 8) | real_data[6]
return target_sysid, grant_bytes
return None, None
def is_discovery_payload(real_data):
return real_data.startswith(DISC_MAGIC)
# =========================================================
# XBee API RX parser
# =========================================================
def process_xbee_buffer():
global rx_buf
global DISC_PENDING
while True:
start_pos = rx_buf.find(b'\x7E')
if start_pos == -1:
rx_buf = bytearray()
return
if start_pos > 0:
rx_buf = rx_buf[start_pos:]
if len(rx_buf) < 3:
return
pkt_len = (rx_buf[1] << 8) | rx_buf[2]
total_len = pkt_len + 4
if pkt_len > 300:
rx_buf = rx_buf[1:]
continue
if len(rx_buf) < total_len:
return
packet = rx_buf[3:3 + pkt_len]
checksum_recv = rx_buf[3 + pkt_len]
if get_checksum(packet) == checksum_recv:
if len(packet) > 0 and packet[0] == 0x90:
# XBee Receive Packet 0x90:
# packet = 90 | src64(8) | src16(2) | options(1) | RF data
if len(packet) >= 12:
src64 = bytes(packet[1:9])
real_data = packet[12:]
# -------------------------------------------------
# DISC：本版唯一 discovery 封包。
# 功能等同原本 DSCF：收到後強制回 HELO。
# -------------------------------------------------
if is_discovery_payload(real_data):
learn_dest64_from_src64(src64)
learn_my_sysid_from_tx_buf()
if MY_SYSID is not None and DEST_64 is not None:
delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
time.sleep_ms(delay_ms)
send_hello_report(force=True)
DISC_PENDING = False
else:
DISC_PENDING = True
else:
# -------------------------------------------------
# 判斷是不是 POLL
# -------------------------------------------------
target_sysid, grant_bytes = parse_poll_payload(real_data)
if target_sysid is not None:
# POLL 只負責 TDMA 授權；DISC 已負責 GCS address / SYSID 學習
if MY_SYSID is not None and target_sysid == MY_SYSID:
flush_tx_buffer(grant_bytes)
else:
# COMM / RTK / 一般 GCS broadcast 下行資料
# DISC 與 POLL 已經在前面處理掉，剩下資料直接轉給 FC / RTK module
uart_fc.write(real_data)
rx_buf = rx_buf[total_len:]
else:
rx_buf = rx_buf[1:]
# =========================================================
# 初始清理
# =========================================================
try:
gc.collect()
except Exception:
pass
# =========================================================
# 主迴圈
# =========================================================
while True:
try:
# -------------------------------------------------
# FC -> ESP32 tx buffer
# -------------------------------------------------
if uart_fc.any():
data = uart_fc.read(READ_FC_BYTES)
if data:
tx_buf.extend(data)
learn_my_sysid_from_tx_buf()
try_send_pending_hello()
trim_tx_buffer_if_needed()
# -------------------------------------------------
# XBee -> ESP32
# -------------------------------------------------
if uart_xb.any():
chunk = uart_xb.read()
if chunk:
rx_buf.extend(chunk)
process_xbee_buffer()
time.sleep_ms(1)
except MemoryError:
tx_buf = bytearray()
rx_buf = bytearray()
try:
gc.collect()
except Exception:
pass
time.sleep_ms(10)
except Exception:
time.sleep_ms(2)
src/unitdev04/udptest8-2.py
-500
View File
@@ -1,500 +0,0 @@
import asyncio
import serial_asyncio
import struct
import serial
import time
import threading
import tkinter as tk
from tkinter import ttk
from collections import deque, defaultdict
from pymavlink import mavutil
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
# === 多組設備設定 ===
CONFIGS = [
{"serial_port": "/dev/ttyUSB0", "udp_port": 14551},
{"serial_port": "COM15", "udp_port": 14570},
{"serial_port": "/dev/ttyUSB2", "udp_port": 14553},
{"serial_port": "/dev/ttyUSB3", "udp_port": 14554},
]
SERIAL_BAUDRATE = 115200
UDP_REMOTE_IP = '127.0.0.1'
TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
ACTIVE_SYSIDS = [3, 10, 15]
POLL_MAGIC = b'POLL'
# === 全域變數與狀態追蹤 ===
current_base_slot_ms = 250 # 預設 TDMA 時槽
LOSS_TIME_WINDOW_SEC = 5.0 # 丟包率計算的時間視窗 (5秒)
uav_states = {
sysid: {
"mode": "NORMAL",
} for sysid in ACTIVE_SYSIDS
}
rssi_history = defaultdict(lambda: deque(maxlen=5000))
time_history = defaultdict(lambda: deque(maxlen=5000))
packet_loss_history = defaultdict(lambda: deque(maxlen=1000))
packet_loss_time_history = defaultdict(lambda: deque(maxlen=1000))
mavlink_sequence_tracker = defaultdict(dict)
packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0})
serial_to_sysid = {}
serial_last_mavlink_time = {}
last_db_query_time = {}
# === 核心邏輯區 ===
def calculate_packet_loss(sysid, compid, current_seq):
global mavlink_sequence_tracker, packet_loss_stats
tracker = mavlink_sequence_tracker[sysid]
now = time.time()
if compid not in tracker:
tracker[compid] = {
'last_seq': current_seq,
'history': deque()
}
return 0.0
comp_tracker = tracker[compid]
last_seq = comp_tracker['last_seq']
if current_seq > last_seq:
expected = current_seq - last_seq
elif current_seq < last_seq:
expected = (255 - last_seq) + current_seq + 1
else:
return packet_loss_history[sysid][-1] if packet_loss_history[sysid] else 0.0
lost = max(0, expected - 1)
comp_tracker['history'].append((now, expected, lost))
comp_tracker['last_seq'] = current_seq
total_expected_all = 0
total_lost_all = 0
for c_id, c_data in tracker.items():
if 'history' in c_data:
while c_data['history'] and (now - c_data['history'][0][0]) > LOSS_TIME_WINDOW_SEC:
c_data['history'].popleft()
total_expected_all += sum(item[1] for item in c_data['history'])
total_lost_all += sum(item[2] for item in c_data['history'])
overall_loss_rate = (total_lost_all / total_expected_all) * 100.0 if total_expected_all > 0 else 0.0
packet_loss_stats[sysid] = {
'loss_rate': overall_loss_rate,
'total_received': total_expected_all - total_lost_all,
'total_lost': total_lost_all
}
packet_loss_history[sysid].append(overall_loss_rate)
packet_loss_time_history[sysid].append(now)
return overall_loss_rate
def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x00) -> bytes:
frame = b'\x10' + struct.pack(">B", frame_id) + dest_addr64 + b'\xFF\xFE\x00\x00' + data
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", 0xFF - (sum(frame) & 0xFF))
class SerialToUDP(asyncio.Protocol):
def send_poll(self, target_sysid):
poll_payload = POLL_MAGIC + struct.pack("B", target_sysid)
api_frame = build_api_tx_frame(poll_payload, TARGET_ADDR64, 0x00)
self.transport.write(api_frame)
def __init__(self, udp_protocol, serial_port):
self.udp_protocol = udp_protocol
self.serial_port = serial_port
self.buffer = bytearray()
self.gcs_tx_queue = bytearray()
def connection_made(self, transport):
self.transport = transport
if hasattr(self.udp_protocol, 'set_serial_transport'):
self.udp_protocol.set_serial_transport(self)
print(f"[{self.serial_port}] Serial connection established.")
def data_received(self, data):
self.buffer.extend(data)
while True:
try:
start_idx = self.buffer.index(0x7E)
if start_idx > 0:
del self.buffer[:start_idx]
except ValueError:
self.buffer.clear()
return
if len(self.buffer) < 3: return
length = (self.buffer[1] << 8) | self.buffer[2]
full_length = 3 + length + 1
if length > 300:
self.buffer.pop(0)
continue
if len(self.buffer) < full_length: return
frame = self.buffer[:full_length]
checksum = 0xFF - (sum(frame[3:-1]) & 0xFF)
if checksum != frame[-1]:
self.buffer.pop(0)
continue
del self.buffer[:full_length]
if hasattr(self.udp_protocol, 'send_udp'):
self.udp_protocol.send_udp(bytes(frame))
if frame[3] == 0x88 and frame[5:7] == b'DB':
status = frame[7]
if status == 0x00 and len(frame) > 8:
rssi_value = frame[8]
now = time.time()
last_time = serial_last_mavlink_time.get(self.serial_port, 0)
if now - last_time <= 0.5:
sysid = serial_to_sysid.get(self.serial_port, None)
if sysid is not None:
rssi_history[sysid].append(-rssi_value)
time_history[sysid].append(now)
def write_to_serial(self, data):
self.gcs_tx_queue.extend(data)
def flush_gcs_queue(self):
if not self.gcs_tx_queue: return
send_limit = min(len(self.gcs_tx_queue), 150)
data_to_send = self.gcs_tx_queue[:send_limit]
self.gcs_tx_queue = self.gcs_tx_queue[send_limit:]
asyncio.create_task(self._async_send_chunks(data_to_send))
async def _async_send_chunks(self, data):
try:
MAX_PAYLOAD = 80
sent_len = 0
while sent_len < len(data):
end_len = min(sent_len + MAX_PAYLOAD, len(data))
chunk = data[sent_len:end_len]
sent_len = end_len
api_frame = build_api_tx_frame(chunk, TARGET_ADDR64, 0x00)
self.transport.write(api_frame)
await asyncio.sleep(0.01)
except Exception:
pass
def send_at_command_db(self):
try:
frame_type = 0x08
frame_id = 0x52
at_command = b'DB'
parameter = b''
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + at_command + parameter
checksum = 0xFF - (sum(frame) & 0xFF)
api_frame = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
self.transport.write(api_frame)
except Exception:
pass
class UDPHandler(asyncio.DatagramProtocol):
def __init__(self, udp_port):
self.udp_port = udp_port
self.serial_transport = None
self.transport = None
self.mav_decoder = mavutil.mavlink.MAVLink(None)
def connection_made(self, transport):
self.transport = transport
def set_serial_transport(self, serial_transport):
self.serial_transport = serial_transport
def datagram_received(self, data, addr):
if self.serial_transport:
self.serial_transport.write_to_serial(data)
def decapsulate_data(self, data):
try:
if not data or data[0] != 0x7E:
return None
length = (data[1] << 8) | data[2]
if len(data) < length + 4:
return None
frame_type = data[3]
if frame_type == 0x90:
rf_data_start = 3 + 12
return data[rf_data_start:3 + length]
else:
return None
except Exception:
return None
def send_udp(self, data):
decoded_data = self.decapsulate_data(data)
if decoded_data is None: return
try:
for byte in decoded_data:
msg = self.mav_decoder.parse_char(bytes([byte]))
if msg:
sysid = msg.get_srcSystem()
compid = msg.get_srcComponent()
seq = msg.get_seq()
if sysid == 0: continue
if self.serial_transport:
port_name = self.serial_transport.serial_port
serial_to_sysid[port_name] = sysid
serial_last_mavlink_time[port_name] = time.time()
now = time.time()
last_query = last_db_query_time.get(port_name, 0)
if now - last_query >= 0.5:
self.serial_transport.send_at_command_db()
last_db_query_time[port_name] = now
calculate_packet_loss(sysid, compid, seq)
except Exception:
pass
if self.transport:
self.transport.sendto(decoded_data, (UDP_REMOTE_IP, self.udp_port))
async def setup_bridge(config):
port, udp = config["serial_port"], config["udp_port"]
try:
ser = serial.Serial(port, SERIAL_BAUDRATE)
ser.close()
except: return None
loop = asyncio.get_running_loop()
udp_handler = UDPHandler(udp)
await loop.create_datagram_endpoint(lambda: udp_handler, local_addr=('0.0.0.0', 0))
serial_proto = SerialToUDP(udp_handler, port)
await serial_asyncio.create_serial_connection(loop, lambda: serial_proto, port, baudrate=SERIAL_BAUDRATE)
return serial_proto
async def tdma_scheduler(serial_protocols):
print(f"TDMA Scheduler Started... 找到 {len(serial_protocols)} 個可用端口")
while True:
# GCS 下行時槽
for sp in serial_protocols:
if hasattr(sp, 'flush_gcs_queue'):
sp.flush_gcs_queue()
await asyncio.sleep(0.1)
# UAV 上行時槽
for sysid in ACTIVE_SYSIDS:
state = uav_states[sysid]
if state["mode"] == "INITIALIZING":
# VIP 模式：0.8秒內連發4次點名，暴力獲取參數
for _ in range(4):
for sp in serial_protocols:
if hasattr(sp, 'send_poll'):
sp.send_poll(sysid)
await asyncio.sleep(0.2)
else:
# NORMAL 模式：聽從滑桿設定的時間
slot_time = current_base_slot_ms / 1000.0
for sp in serial_protocols:
if hasattr(sp, 'send_poll'):
sp.send_poll(sysid)
await asyncio.sleep(slot_time)
async def async_main():
protocols = await asyncio.gather(*(setup_bridge(cfg) for cfg in CONFIGS))
valid_protocols = [p for p in protocols if p is not None]
if valid_protocols:
asyncio.create_task(tdma_scheduler(valid_protocols))
await asyncio.Future()
# === GUI 介面區 ===
def start_gui():
root = tk.Tk()
root.title("UAV TDMA Control Station")
root.geometry("1200x800")
# --- 左側控制面板 ---
control_frame = tk.Frame(root, width=300, bg="#f0f0f0", padx=20, pady=20)
control_frame.pack(side=tk.LEFT, fill=tk.Y)
tk.Label(control_frame, text="TDMA 基礎時槽控制", font=("Arial", 14, "bold"), bg="#f0f0f0").pack(pady=10)
slider_val = tk.IntVar(value=current_base_slot_ms)
def on_slider_change(val):
global current_base_slot_ms
current_base_slot_ms = int(float(val))
val_label.config(text=f"當前設定: {current_base_slot_ms} ms")
slider = ttk.Scale(control_frame, from_=50, to_=1000, orient='horizontal', variable=slider_val, command=on_slider_change)
slider.pack(fill=tk.X, pady=10)
val_label = tk.Label(control_frame, text=f"當前設定: {current_base_slot_ms} ms", bg="#f0f0f0")
val_label.pack()
tk.Label(control_frame, text="群機初始化控制 (INIT)", font=("Arial", 14, "bold"), bg="#f0f0f0").pack(pady=(30, 10))
# === 1. 改為單選按鈕 (Radiobutton) ===
init_var = tk.IntVar(value=0) # 0 代表全體 NORMAL
radios = {}
status_labels = {}
def on_radio_change():
selected = init_var.get()
for sysid in ACTIVE_SYSIDS:
if sysid == selected:
uav_states[sysid]["mode"] = "INITIALIZING"
else:
uav_states[sysid]["mode"] = "NORMAL"
# 新增一個「取消特權」的選項
# tk.Radiobutton(control_frame, text="全體 NORMAL (無特權)", variable=init_var, value=0, command=on_radio_change, bg="#f0f0f0", font=("Arial", 11, "bold"), fg="blue").pack(anchor=tk.W, pady=(0, 10))
for sysid in ACTIVE_SYSIDS:
frame = tk.Frame(control_frame, bg="#f0f0f0")
frame.pack(fill=tk.X, pady=5)
rb = tk.Radiobutton(frame, text=f"SYSID {sysid} 專屬載入", variable=init_var, value=sysid,
command=on_radio_change, bg="#f0f0f0", font=("Arial", 11))
rb.pack(side=tk.LEFT)
radios[sysid] = rb
lbl = tk.Label(frame, text="NORMAL", font=("Arial", 10, "bold"), fg="green", bg="#f0f0f0")
lbl.pack(side=tk.RIGHT, padx=5)
status_labels[sysid] = lbl
# === 2. 改為開關式的鎖定按鈕 (Toggle) ===
is_locked = False
def toggle_tdma_mode():
nonlocal is_locked
is_locked = not is_locked
if is_locked:
# 鎖定：強制切回 0 (全體 NORMAL)，並禁用單選按鈕
init_var.set(0)
on_radio_change()
for rb in radios.values():
rb.config(state=tk.DISABLED)
# 改變按鈕外觀為「解鎖」狀態
lock_btn.config(text="點擊解鎖 (允許重新下載參數)", bg="orange")
print("已鎖定進入純 TDMA 模式")
else:
# 解鎖：恢復單選按鈕功能
for rb in radios.values():
rb.config(state=tk.NORMAL)
# 改變按鈕外觀為「鎖定」狀態
lock_btn.config(text="參數載入完畢，鎖定進入 TDMA", bg="#4CAF50")
print("已解除鎖定，可重新分配特權")
lock_btn = tk.Button(control_frame, text="參數載入完畢，鎖定進入 TDMA",
font=("Arial", 12, "bold"), bg="#4CAF50", fg="white",
command=toggle_tdma_mode)
lock_btn.pack(pady=30, fill=tk.X)
# 動態更新狀態標籤
def update_status_gui():
for sysid, lbl in status_labels.items():
mode = uav_states[sysid]["mode"]
if mode == "INITIALIZING":
lbl.config(text="INIT (特權下載中)", fg="orange")
else:
lbl.config(text="NORMAL", fg="green")
root.after(500, update_status_gui)
update_status_gui()
# --- 右側圖表區 ---
plot_frame = tk.Frame(root)
plot_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True)
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(9, 8), dpi=100)
canvas = FigureCanvasTkAgg(fig, master=plot_frame)
canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
def update_plot(frame):
ax1.clear()
ax2.clear()
ax1.set_title("RSSI", fontsize=12); ax1.set_xlim(10, 0); ax1.set_ylim(-100, -10); ax1.grid(True, alpha=0.3)
ax2.set_title("Packet Loss Rate (5s Window)", fontsize=12); ax2.set_xlim(10, 0); ax2.set_ylim(0, 100); ax2.grid(True, alpha=0.3)
now = time.time()
colors = ['blue', 'red', 'green', 'orange', 'purple']
try: sysids = sorted(list(rssi_history.keys()))
except RuntimeError: return
loss_labels = []
for i, sysid in enumerate(sysids):
color = colors[i % len(colors)]
try:
t_hist, r_hist = list(time_history[sysid]), list(rssi_history[sysid])
lt_hist, l_hist = list(packet_loss_time_history.get(sysid, [])), list(packet_loss_history.get(sysid, []))
except RuntimeError: continue
rssi_recent = [idx for idx, ts in enumerate(t_hist) if now - ts <= 10]
if rssi_recent:
ax1.plot([now - t_hist[idx] for idx in rssi_recent], [r_hist[idx] for idx in rssi_recent], label=f"SYSID:{sysid}", color=color)
loss_recent = [idx for idx, ts in enumerate(lt_hist) if now - ts <= 10]
if loss_recent:
loss_t = [now - lt_hist[idx] for idx in loss_recent]
loss_r = [l_hist[idx] for idx in loss_recent]
ax2.plot(loss_t, loss_r, label=f"SYSID:{sysid}", color=color, marker='o', markersize=3)
if loss_r:
loss_labels.append({
'sysid': sysid, 'y_real': loss_r[-1], 'x_real': loss_t[-1], 'color': color
})
if loss_labels:
loss_labels = sorted(loss_labels, key=lambda k: k['y_real'])
min_gap = 12.0
y_positions = [lbl['y_real'] for lbl in loss_labels]
for j in range(1, len(y_positions)):
if y_positions[j] - y_positions[j-1] < min_gap:
y_positions[j] = y_positions[j-1] + min_gap
if y_positions[-1] > 90:
shift = y_positions[-1] - 90
y_positions = [y - shift for y in y_positions]
for j, lbl in enumerate(loss_labels):
sysid = lbl['sysid']
color = lbl['color']
real_y = lbl['y_real']
text_y = y_positions[j]
ax2.text(0.5, text_y, f'ID:{sysid} ({real_y:.1f}%)',
bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.8),
fontsize=10, fontweight='bold', color='white',
horizontalalignment='right', verticalalignment='center')
if abs(real_y - text_y) > 1.0:
ax2.plot([lbl['x_real'], 0.5], [real_y, text_y], color=color, linestyle=':', alpha=0.6)
ax1.legend(loc="upper left")
ax2.legend(loc="upper left")
ani = animation.FuncAnimation(fig, update_plot, interval=1000)
def on_closing():
root.quit()
root.destroy()
root.protocol("WM_DELETE_WINDOW", on_closing)
root.mainloop()
if __name__ == '__main__':
threading.Thread(target=lambda: asyncio.run(async_main()), daemon=True).start()
start_gui()
src/unitdev04/udptest8.py
+973 -260
View File
File diff suppressed because it is too large Load Diff