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

Compare commits

base: chiyu1468/AirTrapMine:d15c2d0a2d2fbd14064ee424c0d6de6623fdf0de
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
d15c2d0a2d .. 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
40 changed files with 2674 additions and 4280 deletions
Expand all files Collapse all files
.gitignore
-5
View File
@@ -32,8 +32,3 @@ CLAUDE.md
my_env_notes.md
notes/
**/eeprom.bin
# MAVProxy / SITL 執行產生的 log 與參數 dump
*.tlog
*.tlog.raw
mav.parm
README.md
+19 -30
View File
@@ -2,15 +2,9 @@
===
## 功能簡介
1. mavlink 多對多串流與交換
2. 支援 xbee 與 telemetry
3. 支援 udp 管理
4. ROS2 介面監控載具
===
使用限制
1. 不允許進到 ros 系統有相同 sysid
2. 假設一台載具上所有 component 共用同一 socket
1. mavlink 多對多支援平台
2. 不允許進到 ros 系統有相同 sysid
3. 假設一台載具上所有 component 共用同一 socket
===
## 運行環境
@@ -79,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
+39 -126
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;
@@ -247,9 +174,9 @@ class CommPanel(QWidget):
self.serial_baudrate_combo = QComboBox()
self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
self.serial_baudrate_combo.setCurrentText('115200')
self.serial_baudrate_combo.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
+157 -614
View File
File diff suppressed because it is too large Load Diff
src/GUI/drone_panel.py
+32 -157
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,31 +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(" - ")
separator1.setObjectName(f"{self.drone_id}_battery_sep1")
_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(" - ")
separator2.setObjectName(f"{self.drone_id}_battery_sep2")
_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)
@@ -325,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)
@@ -345,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):
"""位置行已移除（位置座標不再顯示於面板）。"""
@@ -422,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):
# 定義信號
@@ -443,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;
""")
@@ -465,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;
@@ -493,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};
@@ -543,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):
"""
@@ -624,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
@@ -704,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
+362 -1103
View File
File diff suppressed because it is too large Load Diff
src/GUI/map_layout.py
+49 -143
View File
@@ -1,15 +1,8 @@
#!/usr/bin/env python3
import json
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 地圖顯示"""
@@ -18,9 +11,6 @@ class DroneMap:
self.map_view = QWebEngineView()
self.map_loaded = False
self.pending_map_updates = {}
self.font_scale = 1.0
self.socket_colors = {}
self.drone_group_colors = {}
# 創建橋接對象
self.bridge = MapBridge()
@@ -41,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;
@@ -76,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);
}
@@ -96,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 {
@@ -115,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;
}
@@ -142,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;
}
@@ -160,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;
}
@@ -178,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;
}
@@ -285,37 +274,38 @@ class DroneMap:
}, 100);
});
function createArrowIcon(color, groupColor) {
const outlineColor = groupColor || 'transparent';
function createArrowIcon(color) {
return L.divIcon({
className: 'drone-arrow',
html: `
<div style="
width: 34px; height: 34px;
width: 30px; height: 30px;
display: flex;
align-items: center;
justify-content: center;
">
<svg width="34" height="34" viewBox="0 0 34 34">
<polygon
points="17,3 31,31 17,26 3,31"
fill="${color || '#FF0000'}"
stroke="${outlineColor}"
stroke-width="3"
stroke-linejoin="round"
/>
<svg width="30" height="30" viewBox="0 0 30 30">
<polygon points="15,0 30,30 15,25 0,30" fill="${color || '#FF0000'}" />
</svg>
</div>
`,
iconSize: [34, 34],
iconAnchor: [17, 17]
iconSize: [30, 30],
iconAnchor: [15, 15]
});
}
function getColorBySocketId(id) {
const match = /^s([^_]+)_/.exec(id || '');
const socketId = match ? match[1] : 'default';
return socketColors[socketId] || socketColors.default || "#E0E0E0";
if (id.startsWith("s0_")) return "#00BFFF";
if (id.startsWith("s1_")) return "#FFD700";
if (id.startsWith("s2_")) return "#FF6969";
if (id.startsWith("s3_")) return "#FF69B4";
if (id.startsWith("s4_")) return "#00FA9A";
if (id.startsWith("s5_")) return "#9370DB";
if (id.startsWith("s6_")) return "#FFA500";
if (id.startsWith("s7_")) return "#20B2AA";
if (id.startsWith("s8_")) return "#7CFC00";
if (id.startsWith("s9_")) return "#FF8C00";
return "#AAAAAA";
}
function createIdIcon(id) {
@@ -332,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],
@@ -343,11 +333,7 @@ class DroneMap:
var markers = {};
var idLabels = {};
var trajectories = {};
const maxTrajectoryPoints = 2000;
const trajectoryMinDistanceMeters = 0.05;
var trajectoryLines = {};
var socketColors = {};
var droneGroupColors = {};
var focusedId = null;
var initialized = false;
var trajectoryGroup = L.layerGroup().addTo(map);
@@ -375,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 = [];
@@ -588,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>',
@@ -641,10 +617,6 @@ class DroneMap:
}
}
// ================================================================================
function setFontScale(scale) {
document.documentElement.style.setProperty('--ui-font-scale', scale);
}
// 開始任務
function startMission() {
@@ -709,7 +681,7 @@ class DroneMap:
color: color,
weight: 3,
opacity: 0.7,
smoothFactor: 0.1
smoothFactor: 1
}).addTo(trajectoryGroup);
}
}
@@ -719,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);
@@ -752,7 +716,7 @@ class DroneMap:
if (markers[id]) {
const lastPos = markers[id].getLatLng();
const distance = lastPos.distanceTo([lat, lon]);
if (distance >= trajectoryMinDistanceMeters) {
if (distance > 1) {
addTrajectoryPoint(id, lat, lon);
}
@@ -766,7 +730,7 @@ class DroneMap:
const color = getColorBySocketId(id);
markers[id] = L.marker([lat, lon], {
icon: createArrowIcon(color, droneGroupColors[id]),
icon: createArrowIcon(color),
rotationAngle: heading,
rotationOrigin: 'center'
})
@@ -804,33 +768,6 @@ class DroneMap:
}
}
function setDroneGroupColors(groupColors) {
droneGroupColors = groupColors || {};
Object.keys(markers).forEach(id => {
markers[id].setIcon(
createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
);
if (typeof markers[id].setRotationAngle === 'function') {
markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
}
});
}
function setSocketColors(colors) {
socketColors = colors || {};
Object.keys(markers).forEach(id => {
markers[id].setIcon(
createArrowIcon(getColorBySocketId(id), droneGroupColors[id])
);
if (typeof markers[id].setRotationAngle === 'function') {
markers[id].setRotationAngle(markers[id].options.rotationAngle || 0);
}
});
Object.keys(trajectoryLines).forEach(id => {
trajectoryLines[id].setStyle({ color: getColorBySocketId(id) });
});
}
function clearAllTrajectories() {
trajectories = {};
Object.values(trajectoryLines).forEach(line => {
@@ -865,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>',
@@ -885,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>',
@@ -930,17 +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)
self.set_socket_colors(self.socket_colors)
self.set_drone_group_colors(self.drone_group_colors)
else:
_log("ERROR", "地圖載入失敗")
print("⚠️ 地圖加載失敗")
def update_drone_position(self, drone_id, lat, lon, heading):
"""更新無人機位置（加入待處理隊列）"""
@@ -953,7 +887,7 @@ class DroneMap:
js_commands = []
for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
js_commands.append(f"updateDrone({lat:.8f}, {lon:.8f}, '{drone_id}', {heading:.1f});")
js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
if js_commands:
combined_js = "\n".join(js_commands)
@@ -965,31 +899,11 @@ class DroneMap:
"""清除所有軌跡"""
if self.map_loaded:
self.map_view.page().runJavaScript("clearAllTrajectories();")
def set_drone_group_colors(self, drone_group_colors):
"""設定每台無人機的 group 外框顏色；未分組者不傳入即為透明。"""
self.drone_group_colors = dict(drone_group_colors or {})
if self.map_loaded:
payload = json.dumps(self.drone_group_colors)
self.map_view.page().runJavaScript(f"setDroneGroupColors({payload});")
def set_socket_colors(self, socket_colors):
"""設定 socket 顏色，供地圖箭頭本體與軌跡線使用。"""
self.socket_colors = dict(socket_colors or {})
if self.map_loaded:
payload = json.dumps(self.socket_colors)
self.map_view.page().runJavaScript(f"setSocketColors({payload});")
def focus_on_drone(self, drone_id):
"""聚焦到指定無人機"""
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});")
# ================================================================================
# 任務規劃視覺化方法
@@ -1010,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 端切換地圖的任務模式（觸發框選/路徑標記等）"""
@@ -1123,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
+26 -87
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"
@@ -43,7 +39,6 @@ class DroneTask:
__slots__ = (
'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
'sent_current_wp', 'fail_count', 'status', 'waiting_since',
'entered_radius_at', 'last_log_at',
)
def __init__(self, drone_id, sysid, waypoints):
@@ -56,8 +51,6 @@ class DroneTask:
self.fail_count = 0
self.status = TaskStatus.NORMAL
self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
self.entered_radius_at = 0.0 # 進入 arrival_radius 的時間，0 = 尚未進入
self.last_log_at = 0.0 # 上次印 distance log 的時間
@property
def current_target(self):
@@ -92,21 +85,14 @@ class MissionExecutor(QObject):
mission_completed = pyqtSignal()
def __init__(self, sender, drone_gps, monitor=None,
arrival_radius=4.0, tick_rate_hz=2.0,
barrier_timeout_sec=20.0,
hover_stable_sec=2.0,
progress_log_interval_sec=3.0):
arrival_radius=2.0, tick_rate_hz=2.0,
barrier_timeout_sec=20.0):
super().__init__()
self.sender = sender
self.drone_gps = drone_gps
self.monitor = monitor # 用於失敗 fallback 到 LOITER
self.arrival_radius = arrival_radius
self.barrier_timeout_sec = barrier_timeout_sec
# hover-stable 判定：進入 radius 後須穩定停留 hover_stable_sec 秒才算到達，
# 容忍 GPS 抖動跨越邊界（用 radius * 1.5 作 hysteresis）
self.hover_stable_sec = hover_stable_sec
self.hysteresis_factor = 1.5
self.progress_log_interval_sec = progress_log_interval_sec
self.state = MissionState.IDLE
self.tasks = {}
self.rendezvous_indices = set()
@@ -123,7 +109,7 @@ class MissionExecutor(QObject):
def start(self, planned_waypoints):
if self.state == MissionState.RUNNING:
_log("WARN", "任務已在執行中")
print("任務已在執行中")
return
self.tasks.clear()
@@ -146,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 (hover-stable {self.hover_stable_sec}s), "
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("任務停止")
# ------------------------------------------------------------------ 控制迴圈
@@ -187,7 +170,7 @@ class MissionExecutor(QObject):
"""
now = time.monotonic()
# ---- Phase 1: 到達判定（hover-stable） ----
# ---- Phase 1: 到達判定 ----
for task in self.tasks.values():
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
continue
@@ -205,38 +188,16 @@ class MissionExecutor(QObject):
tgt_lat, tgt_lon, _ = target
distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
# hover-stable 判定：
# - 首次進入 radius → 記錄時間
# - 在 radius * hysteresis 內持續 → 保留計時
# - 離開 hysteresis → 重置
# - 在 radius 內穩定 hover_stable_sec 秒 → 視為到達
hysteresis_radius = self.arrival_radius * self.hysteresis_factor
if distance < self.arrival_radius:
if task.entered_radius_at == 0.0:
task.entered_radius_at = now
stable_for = now - task.entered_radius_at
if stable_for < self.hover_stable_sec:
self._maybe_log_progress(task, now, distance, stable_for)
continue
elif distance < hysteresis_radius and task.entered_radius_at > 0.0:
# 還在 hysteresis 內，計時不重置（容忍 GPS 抖動跨越邊界）
self._maybe_log_progress(task, now, distance, now - task.entered_radius_at)
continue
else:
# 離開 hysteresis：重置計時，繼續等待到達
task.entered_radius_at = 0.0
self._maybe_log_progress(task, now, distance, 0.0)
if distance >= self.arrival_radius:
continue
# 到達當前 wp_index
task.entered_radius_at = 0.0 # 重置給下個 wp 用
if (task.wp_index in self.rendezvous_indices
and task.wp_index < task.total_waypoints - 1):
# rendezvous 點 → 不推進，進入 barrier 等待
task.status = TaskStatus.WAITING_AT_BARRIER
task.waiting_since = now
_log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index}，等待同伴")
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}"
@@ -275,49 +236,26 @@ class MissionExecutor(QObject):
self._timer.stop()
self.state = MissionState.IDLE
self.mission_completed.emit()
_log("INFO", "任務全部完成")
def _maybe_log_progress(self, task, now, distance, stable_for):
"""週期性印出 drone 與當前 wp 的距離，方便實飛 debug 卡點問題。"""
if now - task.last_log_at < self.progress_log_interval_sec:
return
task.last_log_at = now
if stable_for > 0:
stable_info = f", 在內側 {stable_for:.1f}s/{self.hover_stable_sec:.1f}s"
else:
stable_info = ""
_log(
"INFO",
f"{task.drone_id} → WP {task.wp_index}: "
f"距離 {distance:.1f}m (半徑 {self.arrival_radius:.1f}m{stable_info})",
)
print("===== 任務全部完成 =====")
def _advance_waypoint(self, task, arrived_distance):
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
task.wp_index += 1
task.sent_current_wp = False
task.fail_count = 0
task.entered_radius_at = 0.0
task.last_log_at = 0.0
if task.wp_index >= task.total_waypoints:
task.done = True
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
_log(
"INFO",
f"{task.drone_id} 已完成所有航點 "
f"({task.total_waypoints}/{task.total_waypoints})",
)
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 能不能釋放。"""
@@ -351,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})"
@@ -385,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
@@ -400,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
+26 -110
View File
@@ -13,34 +13,16 @@ from mission_executor import MissionExecutor, MissionState
# 群組顏色（循環使用）
GROUP_COLORS = [
"#E53935", # Deep Red
"#FB8C00", # Deep Orange
"#FDD835", # Deep Yellow
"#43A047", # Deep Green
"#1E88E5", # Deep Blue
"#8E24AA", # Deep Purple
"#D81B60", # Deep Pink
'#4A9EFF', # 藍
'#FF8C42', # 橘
'#56C87A', # 綠
'#E85D75', # 紅
'#B07CED', # 紫
'#F5C542', # 黃
'#42C9C9', # 青
'#FF6B9D', # 粉
]
DEFAULT_MISSION_PARAM_VALUES = {
'spacing': '5.0',
'base_altitude': '10.0',
'altitude_diff': '2.0',
'radius': '10.0',
'altitude': '10.0',
'start_angle': '0',
'lateral_offset': '3.0',
'longitudinal_spacing': '5.0',
'line_spacing': '5.0',
# 執行參數（所有任務類型共用）
'arrival_radius': '4.0', # 到達半徑 (m)，實飛建議 3~5，SITL 可調小到 2
'hover_stable_sec': '2.0', # 在 radius 內穩定停留多久才視為到達 (s)
}
# 執行參數的 key 集合（所有 mission type 共用，不隨類型切換）
EXECUTION_PARAM_KEYS = ('arrival_radius', 'hover_stable_sec')
class MissionGroup:
"""單一任務群組的資料"""
@@ -162,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 — 清除分組
@@ -176,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):
@@ -377,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()
# ============================
# 第四欄：任務參數
# ============================
@@ -411,32 +372,26 @@ 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'),
],
}
# 所有 mission type 共用的執行參數（不隨類型切換隱藏）
self._exec_param_defs = [
('arrival_radius', '到達半徑 (m)', self.default_params['arrival_radius']),
('hover_stable_sec', '停留判定 (s)', self.default_params['hover_stable_sec']),
]
# 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
self._param_widgets = {} # key → (label_widget, input_widget)
self._param_rows = [] # 所有 row layout 對應的 container widget
@@ -460,23 +415,6 @@ class GroupPanel(QWidget):
self._param_widgets[key] = (row_w, inp)
self._param_rows.append(row_w)
# 執行參數列（所有 mission type 共用，永遠顯示）
for key, label_text, default in self._exec_param_defs:
row_w = QWidget()
row_l = QHBoxLayout(row_w)
row_l.setContentsMargins(0, 0, 0, 0)
row_l.setSpacing(3)
lbl = QLabel(label_text)
lbl.setStyleSheet(LBL)
inp = QLineEdit(default)
inp.setStyleSheet(INPUT)
inp.setFixedWidth(50)
row_l.addWidget(lbl, 1)
row_l.addWidget(inp)
param_col.addWidget(row_w)
self._param_widgets[key] = (row_w, inp)
self._param_rows.append(row_w)
# LEADER_FOLLOWER 專用：領隊下拉選單
self._leader_row = QWidget()
leader_layout = QHBoxLayout(self._leader_row)
@@ -500,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())
@@ -509,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)
@@ -576,17 +512,15 @@ class GroupPanel(QWidget):
self.delete_group_btn.setEnabled(enabled)
def _update_param_visibility(self, _=None):
"""根據當前任務類型，顯示/隱藏對應的參數列。執行參數永遠顯示。"""
"""根據當前任務類型，顯示/隱藏對應的參數列"""
mission_type = self.type_combo.currentText()
mission_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
exec_keys = {d[0] for d in self._exec_param_defs}
visible_keys = mission_keys | exec_keys
visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
for key, (row_w, _inp) in self._param_widgets.items():
row_w.setVisible(key in visible_keys)
self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
def get_mission_params(self):
"""讀取當前任務類型的規劃參數，回傳 dict（不含執行參數）"""
"""讀取當前顯示的參數值，回傳 dict"""
mission_type = self.type_combo.currentText()
params = {}
for key, _label, default in self._param_defs.get(mission_type, []):
@@ -598,25 +532,6 @@ class GroupPanel(QWidget):
params[key] = float(default)
return params
def get_execution_params(self):
"""讀取執行參數（arrival_radius, hover_stable_sec 等），回傳 dict"""
params = {}
for key, _label, default in self._exec_param_defs:
if key in self._param_widgets:
_row_w, inp = self._param_widgets[key]
try:
params[key] = float(inp.text())
except ValueError:
params[key] = float(default)
return params
def set_param_value(self, key, value):
"""更新指定參數欄位的文字值。"""
if key not in self._param_widgets:
return
_row_w, inp = self._param_widgets[key]
inp.setText(str(value))
def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
"""更新中心點 / 目標點顯示"""
info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
@@ -638,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
+199 -46
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)
@@ -193,13 +192,11 @@ class FormationPlanner:
final_y = center_y + radius * math.sin(angle_rad)
final_z = altitude
current_x, current_y, _current_z = drone_positions[i]
# stage1 高度直接用 final_z：先爬升到目標高度再水平集合，
# 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高）
current_x, current_y, current_z = drone_positions[i]
stage1_positions.append((
current_x + (final_x - current_x) * 0.5,
current_y + (final_y - current_y) * 0.5,
final_z,
current_z + (final_z - current_z) * 0.5
))
stage2_positions.append((final_x, final_y, final_z))
@@ -209,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)
@@ -228,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']
@@ -248,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:
@@ -258,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]
@@ -289,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
@@ -305,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)
@@ -326,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
-6
View File
@@ -8,17 +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"
"msg/ServiceAckResult.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/msg/ServiceAckResult.msg
-43
View File
@@ -1,43 +0,0 @@
# ServiceAckResult.msg
# ACK code definition for command-like service responses.
# 這邊只是定義常數的檔案
#
# Design:
# - 0~10 : MAVLink MAV_RESULT compatible range (autopilot/firmware ACK) 大多不會用到 直接吃飛控回傳值
# - 50~89 : System-level ACK codes (local bridge/orchestrator status)
# - 90~99 : Unknown/reserved fallback
#
# Rule:
# - If FC ACK is received, return MAV_RESULT_* code.
# - If FC ACK is not reachable / local pipeline failed, return SYS_* code.
# ---- MAVLink MAV_RESULT (standard) ---- 詳細請自行去網路找 mavlink MAV_RESULT
uint8 MAV_RESULT_ACCEPTED=0 # 封包正常接受並執行
uint8 MAV_RESULT_TEMPORARILY_REJECTED=1 # 封包正常接受 但是飛控很忙 不執行 需要等待後重試
uint8 MAV_RESULT_DENIED=2 # 封包不正常 參數可能給錯了
uint8 MAV_RESULT_UNSUPPORTED=3 # 封包不正常 根本認不出是啥
uint8 MAV_RESULT_FAILED=4 # 封包正常接受 但是載具的狀態異常 不執行
uint8 MAV_RESULT_IN_PROGRESS=5
uint8 MAV_RESULT_CANCELLED=6
uint8 MAV_RESULT_COMMAND_LONG_ONLY=7
uint8 MAV_RESULT_COMMAND_INT_ONLY=8
uint8 MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME=9
uint8 MAV_RESULT_NOT_IN_CONTROL=10
# ---- System-level result codes (custom) ----
# 50 : 有接到預期的封包 但是訊息建議再確認
# 51 : some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair
#
# 52 : mavlink communication timeout 可能是通訊丟包了
# 53 : there already has a command being execution for this sysid, try later
# 54 : this sysid is not found
uint8 FCSYS_GET_EXPECTED_FEEDBACK=50
uint8 FCSYS_NO_EXPECTED_FEEDBACK=51
uint8 FCSYS_MAVLINK_TIMEOUT=52
uint8 FCSYS_BUSY=53
uint8 FCSYS_DEVICE_NOT_FOUND=54
# ---- Fallback ----
uint8 FCSYS_UNKNOWN=90
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/ackEnum.py
+31
View File
@@ -0,0 +1,31 @@
'''
單獨出來定義回傳碼的意義
若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
所以要避開重疊的號碼
這邊備註 Mavlink 的 MAV_RESULT 參數意義
0 : 封包正常接受並執行
1 : 封包正常接受 但是正在忙不執行 需要等待後重試
4 : 封包正常接受 但是載具的狀態異常 不執行
2 : 封包不正常 參數可能給錯了
3 : 封包不正常 根本認不出是啥
'''
from enum import IntEnum
class serviceAckResult(IntEnum):
MAVLINK_TIMEOUT = 51 # mavlink communication timeout
MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair
MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
UNKNOWN = 90 # i did not capture this error, try read the logs
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
+2 -26
View File
@@ -37,7 +37,6 @@
- *self.bridge* 存放 mavlink_bridge 實例
- *self.plumber* 存放 serial_manager 實例
- *self.vehicle_registry* 存放 vehicle_registry 實例
- *self.fc_ros_manager* 存放 fc_ros_manager 實例
- *self.panel_thread* 面板的執行緒
- *self.panelState* 暫存面板與調配者互動的資料流動區
@@ -177,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()* 啟動自己
@@ -213,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,14 +224,7 @@
8. 基礎載具流量觀測
9. 載具狀態收集與彙整
10. a. ros2 topic 應用開發介面 (基礎框架)
- GNSS (alt lan fix_type epv eph)
- attitude position_ned (pitch yaw row local_position)
- battery
b. ros2 service 應用開發介面 (基礎框架)
c. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
11. ros2 topic service rtcm Node 重啟功能
12. 可以定義 ROS_DOMAIN_ID
### 待開發功能
5-1. 建立 serial 連線 並可以對接收器下達AT指令
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+15 -89
View File
@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
from .utils.acquirePort import find_available_port
logger = setup_logger(os.path.basename(__file__))
PROJECT_VER = "v1.10"
PROJECT_VER = "v0.70"
class PanelState:
def __init__(self):
@@ -177,18 +177,11 @@ class ControlPanel:
MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
]),
MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "SHOW_VEHICLES"),
MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
MenuNode("Engineer Mode", "工程模式", children=[
MenuNode("Stop Manager", "停止 Mavlink_Object 管理", "STOP_MANAGER"),
MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
MenuNode("Stop Serial M.", "停止 Serial 端口", "STOP_SERIAL_MANAGER"),
MenuNode("Edit ROS Nodes", "管理 ROS NODE", children=[
MenuNode("Reboot Topics", "重建 ROS Topics", "REBOOT_ROS_TOPICS"),
MenuNode("Reboot Services", "重建 ROS Services", "REBOOT_ROS_SERVICES"),
MenuNode("Reboot RTCM", "重建 RTK RTCM", "REBOOT_ROS_RTCM"),
MenuNode("Stop ROS Manager", "停止 ROS 功能", "STOP_ROS_MANAGER"),
MenuNode("Start ROS Manager", "啟動 ROS 功能", "START_ROS_MANAGER"),
]),
MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
]),
MenuNode("Shutdown", "關閉整個系統", children=[
MenuNode("Return", "繼續運行", "BACK"),
@@ -391,7 +384,6 @@ class ControlPanel:
state.intoTERMINATION()
pre_panel_shutdown()
# related with -> UDP mavlink Object
elif selected.action == "TEXT_UDP_IP":
result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
if result is not None:
@@ -420,8 +412,6 @@ class ControlPanel:
# menu_stack.pop()
# idx_stack.pop()
# related with -> Serial Connection
elif selected.action == "TEXT_BAUD_SERIAL":
result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
if result is not None:
@@ -487,7 +477,12 @@ class ControlPanel:
menu_stack.pop()
idx_stack.pop()
# related with -> 列表選單
elif selected.action == "LIST_MAV_OBJECT":
# 動態生成 mavlink_object 列表選單
created_list_menu = self.create_object_list_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
if hasattr(selected, 'page'):
current_list_menu = menu_stack[-1]
@@ -512,13 +507,6 @@ class ControlPanel:
menu_stack.append(created_list_menu)
idx_stack.append(0)
# related with -> mavlink object list
elif selected.action == "LIST_MAV_OBJECT":
# 動態生成 mavlink_object 列表選單
created_list_menu = self.create_object_list_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_MAV_OBJECT":
# 顯示物件詳細資訊
if hasattr(selected, 'socket_id'):
@@ -563,7 +551,6 @@ class ControlPanel:
if target_id is not None:
cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
# related with -> Engineer Mode
elif selected.action == "STOP_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
@@ -581,40 +568,8 @@ class ControlPanel:
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
elif selected.action == "STOP_ROS_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("STOP_ROS_MANAGER")
elif selected.action == "START_ROS_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("START_ROS_MANAGER")
elif selected.action == "REBOOT_ROS_TOPICS":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put(("RESTART_ROS_NODE", "status_publisher")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
elif selected.action == "REBOOT_ROS_SERVICES":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put(("RESTART_ROS_NODE", "command_service")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
elif selected.action == "REBOOT_ROS_RTCM":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put(("RESTART_ROS_NODE", "rtcm_relay")) # 參照一下 mavlinkROS2Nodes.NODE_KEYS
# related with -> 進 bridge 的設備 列表
elif selected.action == "SHOW_VEHICLES":
elif selected.action == "INSPECT_VEHICLES":
# 進入載具檢視選單
cmd_q.put("UPDATE_VEHICLES_LIST")
created_list_menu = self.create_vehicles_list_menu(state, page=0)
@@ -1190,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,
@@ -1218,8 +1173,6 @@ class Orchestrator:
def mainLoop(self):
logger.info("Main orchestrator started <-")
self.ros2_retry_time = None
self.ros2_manual_stop = False
try:
# while not self.stop_evt.is_set():
while self.panel_thread.is_alive():
@@ -1251,17 +1204,6 @@ class Orchestrator:
else:
self.panelState.ros2_manager_state = 'Stopped'
# 這邊做一個簡易的重啟 掛掉要等待一秒再重啟 避免過度頻繁 # 算是高技術債的方式構建 但是未來有需要在還吧 (煙
if not self.ros2_manual_stop:
# logger.info(f"mark A {self.ros2_retry_time}")
if self.ros2_retry_time:
if (time.time() - self.ros2_retry_time) > 1:
self.fc_ros_manager.start()
self.fc_ros_manager.schedule_restart_node("command_service")
self.ros2_retry_time = None
else:
self.ros2_retry_time = time.time()
# B. 更新載具列表（從 vehicle_registry 獲取）
self._update_vehicles_list()
@@ -1326,12 +1268,6 @@ class Orchestrator:
elif action == "UNREGISTER_VEHICLE":
sysid = cmd[1]
self.vehicle_registry.unregister(sysid)
elif action == "RESTART_ROS_NODE":
node_key = cmd[1]
if not self.fc_ros_manager.schedule_restart_node(node_key):
logger.warning(
f"RESTART_ROS_NODE failed: {node_key!r}"
)
elif cmd == "CREATE_UDP_INBOUND":
self.panelState.udp_info_temp["direction"] = "inbound"
@@ -1341,19 +1277,12 @@ class Orchestrator:
self.create_udp_object()
elif cmd == "CREATE_SERIAL_PORT":
self.create_serial_port_object()
elif cmd == "SHUTDOWN_BRIDGE":
self.bridge.stop()
elif cmd == "SHUTDOWN_MANAGER":
self.manager.shutdown()
elif cmd == "SHUTDOWN_SERIAL_MANAGER":
self.plumber.shutdown()
elif cmd == "STOP_ROS_MANAGER":
self.fc_ros_manager.stop()
self.ros2_manual_stop = True
elif cmd == "START_ROS_MANAGER":
self.fc_ros_manager.start()
self.ros2_manual_stop = False
except queue.Empty:
pass
@@ -1642,7 +1571,7 @@ def main():
if mo.MODULE_VER != "1.50":
print("Module Version Error! : mavlinkObkect")
version_check = False
if mros.MODULE_VER != "2.10":
if mros.MODULE_VER != "1.50":
print("Module Version Error! : mavlinkROS2Nodes")
version_check = False
if mvv.MODULE_VER != "1.00":
@@ -1693,7 +1622,4 @@ if __name__ == "__main__":
2025.04.13
1. 加入各項模組的版本先驗 檢驗失敗就直接離開
2025.05.28
1. 工程模式 新增 Node 重啟的功能
'''
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
+64 -357
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,8 +32,7 @@ import mavros_msgs.msg
# ROS2 Service imports
import fc_interfaces.srv as fcsrv
import fc_interfaces.msg as fcmsg
from fc_interfaces.msg import ServiceAckResult
from .ackEnum import serviceAckResult
# 自定義 imports
from . import mavlinkVehicleView as mvv
@@ -45,10 +40,7 @@ from . import mavlinkObject as mo
from .utils import setup_logger
logger = setup_logger(os.path.basename(__file__))
MODULE_VER = "2.10"
FC_ROS_DOMAIN_ID = "0"
NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay")
MODULE_VER = "1.50"
# ============================================================================
# VehicleStatusPublisher Node
@@ -68,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 未來移除)
@@ -138,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)
# 狀態標誌
@@ -178,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)
@@ -209,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
@@ -219,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
@@ -280,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)
@@ -443,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,
}
@@ -538,7 +531,7 @@ class MavlinkCommandService(Node):
def __init__(self):
super().__init__('mavlink_command_service')
# 狀態標記
# 狀態標記
self.running = True
# # mavlinkObject 的引用（將由外部設置） 用不到
@@ -584,7 +577,7 @@ class MavlinkCommandService(Node):
self.handle_set_position_target_global_int
)
# logger.info("MavlinkCommandService initialized")
logger.info("MavlinkCommandService initialized")
def _index(self, target_sysid, target_compid):
@@ -611,19 +604,20 @@ class MavlinkCommandService(Node):
藉由 event.set() 解開 service 中的 block
'''
return_tuple = mo.return_packet_ring.get()
# 確認 return_packet_ring 有資料
if return_tuple == None: return
socketid, timestamp, msg = return_tuple
sysid = msg.get_srcSystem()
_pending = self._pending_by_sysid.get(sysid)
if _pending is None:
# 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
if _pending == None:
return
if _pending.match_fn(msg):
_pending.result_mav_msg = msg
_pending.event.set()
return
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 發送 TIMESYNC 可以作為模板
@@ -708,20 +702,20 @@ class MavlinkCommandService(Node):
# 設定失效回應
response.success = False
response.message = "Unknown error"
response.ack_result = ServiceAckResult.FCSYS_UNKNOWN
response.ack_result = serviceAckResult.UNKNOWN
timeout_sec = request.timeout_sec
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
if request.target_sysid in self._pending_by_sysid:
response.message = f"sysid {request.target_sysid} already has pending request"
response.ack_result = ServiceAckResult.FCSYS_BUSY
response.ack_result = serviceAckResult.MAVLINK_BUSY
return response
# 2) 找到 socket 標地
socketObject = self._index(request.target_sysid, request.target_compid)
if socketObject is None:
response.message = "This system id not found."
response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND
response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
return response
# 3) 接收封包系統 的設定
@@ -752,13 +746,12 @@ class MavlinkCommandService(Node):
# 5) 等待回應封包
if not evt.wait(timeout_sec):
response.message = "mavlink timeout - CommLONG"
response.ack_result = ServiceAckResult.FCSYS_MAVLINK_TIMEOUT
response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
fail_skip = True
# 6) 處理回應封包
if not fail_skip:
ack_msg = _pending.result_mav_msg
# response.success = ServiceAckResult.FCSYS_MAVLINK_BREAK # dev 故意觸發錯誤用的
response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
response.message = "" # 沒有消息就是好消息
response.ack_result = ack_msg.result
@@ -782,20 +775,20 @@ class MavlinkCommandService(Node):
'''
# 設定失效回應
response.message = "Unknown error"
response.ack_result = ServiceAckResult.FCSYS_UNKNOWN
response.ack_result = serviceAckResult.UNKNOWN
timeout_sec = request.timeout_sec
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
if request.target_sysid in self._pending_by_sysid:
response.message = f"sysid {request.target_sysid} already has pending request"
response.ack_result = ServiceAckResult.FCSYS_BUSY
response.ack_result = serviceAckResult.MAVLINK_BUSY
return response
# 2) 找到 socket 標地
socketObject = self._index(request.target_sysid, request.target_compid)
if socketObject is None:
response.message = "This system id not found."
response.ack_result = ServiceAckResult.FCSYS_DEVICE_NOT_FOUND
response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
return response
# 3) 接收封包系統 的設定
@@ -830,7 +823,7 @@ class MavlinkCommandService(Node):
# 5) 等待回應封包
if not evt.wait(timeout_sec):
response.message = "mavlink timeout - SET POSITION 86"
response.ack_result = ServiceAckResult.FCSYS_NO_EXPECTED_FEEDBACK
response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
fail_skip = True
# 6) 處理回應封包
@@ -838,7 +831,7 @@ class MavlinkCommandService(Node):
ack_msg = _pending.result_mav_msg
response.message = ""
response.ack_result = ServiceAckResult.MAV_RESULT_ACCEPTED
response.ack_result = 0
response.r_time_boot_ms = ack_msg.time_boot_ms
response.r_type_mask = ack_msg.type_mask
response.r_lat_int = ack_msg.lat_int
@@ -955,183 +948,11 @@ class MavlinkCommandService(Node):
return response
def stop(self):
"""停止服務"""
self.running = False
# 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")
logger.info("MavlinkCommandService stopped")
# ============================================================================
@@ -1147,10 +968,9 @@ class fc_ros_manager:
stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
shutdown 是完全關閉 ROS2 並銷毀節點實例。
管理三個獨立的 ROS2 Node :
管理兩個獨立的 ROS2 Node：
- VehicleStatusPublisher
- MavlinkCommandService
- RtcmRelay
提供統一的啟動/停止介面給 mainOrchestrator
"""
@@ -1158,14 +978,10 @@ class fc_ros_manager:
def __init__(self):
self.initialized = False
self.running = False
self._restarting = False
self._pending_restart: Optional[str] = None
self._restart_lock = threading.Lock()
# 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
@@ -1178,20 +994,18 @@ class fc_ros_manager:
return False
try:
os.environ.setdefault("ROS_DOMAIN_ID", FC_ROS_DOMAIN_ID)
# 初始化 ROS2
if not rclpy.ok():
rclpy.init(args=None)
# 創建節點 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")
@@ -1213,10 +1027,6 @@ class fc_ros_manager:
try:
self.running = True
self.status_publisher.running = True
self.command_service.running = True
self.rtcm_relay.running = True
self.spin_thread = threading.Thread(
target=self._spin_executor,
@@ -1232,105 +1042,24 @@ class fc_ros_manager:
logger.error(f"Failed to start fc_ros_manager: {e}")
self.running = False
return False
# 給 orchestrator 呼叫的函數
def schedule_restart_node(self, node_key: str) -> bool:
"""由 orchestrator 排程單一 node 重啟（於 spin thread 下一輪執行）"""
if node_key not in NODE_KEYS:
logger.warning(f"schedule_restart_node: unknown key {node_key!r}")
return False
if not self.running:
logger.warning("schedule_restart_node: fc_ros_manager not running")
return False
if not self.spin_thread or not self.spin_thread.is_alive():
logger.warning("schedule_restart_node: spin thread not alive")
return False
with self._restart_lock:
self._pending_restart = node_key
logger.info(f"schedule_restart_node: queued {node_key}")
return True
# 循環執行的地方
def _spin_executor(self):
"""在 thread 中運行的 executor"""
try:
# logger.info("ROS2 executor spinning...")
while self.running:
self.executor.spin_once(timeout_sec=0.1)
pending = None
with self._restart_lock:
if self._pending_restart:
pending = self._pending_restart
self._pending_restart = None
if pending:
self._restart_node(pending)
elif self.command_service and not self._restarting:
self.command_service.return_router()
self.command_service.return_router()
except Exception as e:
logger.error(f"fc_ros_manager error in spinning executor: {e}")
finally:
self.running = False
def _restart_node(self, node_key: str) -> bool:
"""僅由 spin thread 呼叫 : destroy 並重建單一 node"""
old_node = getattr(self, node_key, None)
if old_node is None:
logger.error(f"_restart_node: {node_key} is None")
return False
# 重置前保留舊有或處理設定
saved_topic_intervals = None
if node_key == "status_publisher":
saved_topic_intervals = dict(old_node.rate_controller.topic_intervals)
# 如果是 command_service 清除所有等待的 pending 並且清除 return message
if node_key == "command_service":
old_node._pending_by_sysid.clear()
for sockObj in mo.mavlink_object.mavlinkObjects.values():
sockObj.set_return_message_types([])
try:
self._restarting = True
# 停止運作 移出執行迴圈 銷毀資源
old_node.stop()
self.executor.remove_node(old_node)
old_node.destroy_node()
# 建立新的 node
if node_key == "status_publisher":
new_node = VehicleStatusPublisher()
if saved_topic_intervals is not None:
new_node.rate_controller.topic_intervals = saved_topic_intervals
elif node_key == "command_service":
new_node = MavlinkCommandService()
elif node_key == "rtcm_relay":
new_node = RtcmRelay()
else:
return False
# 加回去管理系統中
setattr(self, node_key, new_node)
self.executor.add_node(new_node)
logger.info(f"_restart_node: {node_key} restarted")
return True
except Exception as e:
logger.error(f"_restart_node failed for {node_key}: {e}")
return False
finally:
self._restarting = False
def stop(self):
"""停止 ROS2 nodes"""
# if not self.running:
# logger.warning("fc_ros_manager not running")
# return False
if not self.spin_thread:
logger.warning("fc_ros_manager without thread to stop")
if not self.running:
logger.warning("fc_ros_manager not running")
return False
try:
# 標記停止
self.running = False
@@ -1340,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():
@@ -1366,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():
@@ -1379,21 +1104,14 @@ class fc_ros_manager:
except Exception as e:
logger.error(f"Error during shutdown: {e}")
def get_status(self):
def get_status(self) -> dict:
return {
'initialized': self.initialized,
'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,
}
# ============================================================================
# 全域實例
@@ -1419,21 +1137,10 @@ 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 生命週期
2026.05.22
1. initialize() 設定 ROS_DOMAIN_ID ( FC_ROS_DOMAIN_ID )
2. schedule_restart_node / _restart_node : 手動重啟單一 node (spin thread 內執行）
3. orchestrator cmd: ("RESTART_ROS_NODE", node_key), node_key 見 NODE_KEYS
TODO
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
'''
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
+13 -22
View File
@@ -1,24 +1,21 @@
備選需要的功能
- serail 對於 telemetry 的支援
- serial_manager.serial_object.transport 這些變數可能不需要
不用動
- mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async
- 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複)
這一步
RTK
跟 RTK2go 抓取列表 Done
從特定 mount point 得到數據 Done
做一個 ros2 service 接到數據並包裝
研究 ros2 service
下一步
重啟做好了 但是還沒自動化
下下一步
整合 xbee api-api 功能
後面
rssi 資訊提取s
@@ -30,20 +27,8 @@ 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
python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart
export ROS_DOMAIN_ID=13
ros2 daemon stop
ros2 daemon start
ros2 topic list
ros2 topic echo /fc_network/vehicle/sys3/battery
ros2 topic echo hz /fc_network/vehicle/sys3/battery
ros2 topic bw /fc_network/vehicle/sys3/attitude
ros2 topic hz /fc_network/vehicle/sys3/attitude
/home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
@@ -54,7 +39,7 @@ ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5
ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 10, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
@@ -63,4 +48,10 @@ sudo tcpdump -i lo 'udp dst port 14561' -X
sudo tcpdump -i lo 'udp dst port 14550' -X -vv
sudo tcpdump -i lo -X udp port 14550
幾個要討論的
1. 專案文件中 .mat 檔案是幹嘛的?
2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
3. readme 那串文字來源? 用途?
4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
5. pitch yaw roll 出來了 弄一下
6.
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