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

Merge remote-tracking branch 'origin/ken' into wenchun

Browse Source
chiyu
wenchun 2 months ago
parent 317483a5c0 4f49ff815d
commit 28810e5113
32 changed files with 10466 additions and 2009 deletions
Show Stats Download Patch File Download Diff File

294
src/GUI/comm_panel.py
Unescape Escape View File

@ -1,7 +1,9 @@
#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
QPushButton, QLineEdit)
QPushButton, QLineEdit, QComboBox)
from PyQt6.QtCore import pyqtSignal
import glob
import os
class CommPanel(QWidget):
"""通讯设置面板类"""
@ -13,12 +15,16 @@ class CommPanel(QWidget):
ws_connection_added = pyqtSignal(str) # url
ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
serial_connection_added = pyqtSignal(str, int) # port, baudrate
serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
serial_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
status_message = pyqtSignal(str, int) # message, timeout
def __init__(self, parent=None):
super().__init__(parent)
self.udp_connections = []
self.ws_connections = []
self.serial_connections = []
self._init_ui()
def _init_ui(self):
@ -65,7 +71,7 @@ class CommPanel(QWidget):
""")
self.udp_port_input = QLineEdit()
self.udp_port_input.setText("14540")
self.udp_port_input.setText("14550")
self.udp_port_input.setPlaceholderText("Port")
self.udp_port_input.setFixedWidth(80)
self.udp_port_input.setStyleSheet("""
@ -105,6 +111,118 @@ class CommPanel(QWidget):
separator.setFixedHeight(20)
layout.addWidget(separator)
# ========== Serial 區域 ==========
serial_title = QLabel("Serial")
serial_title.setStyleSheet("""
color: #DDD;
font-size: 14px;
font-weight: bold;
padding: 5px;
background-color: #333;
border-radius: 4px;
""")
layout.addWidget(serial_title)
# Serial 連接列表容器
self.serial_list_widget = QWidget()
self.serial_list_layout = QVBoxLayout(self.serial_list_widget)
self.serial_list_layout.setContentsMargins(0, 0, 0, 0)
self.serial_list_layout.setSpacing(5)
layout.addWidget(self.serial_list_widget)
# Serial 添加新連接區域
add_serial_widget = QWidget()
add_serial_layout = QHBoxLayout(add_serial_widget)
add_serial_layout.setContentsMargins(0, 0, 0, 0)
self.serial_port_combo = QComboBox()
self.serial_port_combo.setStyleSheet("""
QComboBox {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
QComboBox::drop-down {
border: none;
}
QComboBox::down-arrow {
image: none;
border-left: 5px solid transparent;
border-right: 5px solid transparent;
border-top: 5px solid #DDD;
}
""")
self._refresh_serial_ports()
refresh_ports_btn = QPushButton("↻")
refresh_ports_btn.setFixedWidth(35)
refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
refresh_ports_btn.setToolTip("重新掃描串口")
refresh_ports_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 6px;
border-radius: 4px;
font-size: 16px;
}
QPushButton:hover { background-color: #555; }
""")
self.serial_baudrate_combo = QComboBox()
self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
self.serial_baudrate_combo.setCurrentText('57600')
self.serial_baudrate_combo.setFixedWidth(100)
self.serial_baudrate_combo.setStyleSheet("""
QComboBox {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
QComboBox::drop-down {
border: none;
}
QComboBox::down-arrow {
image: none;
border-left: 5px solid transparent;
border-right: 5px solid transparent;
border-top: 5px solid #DDD;
}
""")
add_serial_btn = QPushButton("添加")
add_serial_btn.clicked.connect(self._handle_add_serial)
add_serial_btn.setStyleSheet("""
QPushButton {
background-color: #4CAF50;
color: white;
border: none;
padding: 6px 12px;
border-radius: 4px;
min-width: 30px;
}
QPushButton:hover { background-color: #45a049; }
""")
add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
add_serial_layout.addWidget(self.serial_port_combo)
add_serial_layout.addWidget(refresh_ports_btn)
add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;"))
add_serial_layout.addWidget(self.serial_baudrate_combo)
add_serial_layout.addWidget(add_serial_btn)
layout.addWidget(add_serial_widget)
# 分隔線
separator = QWidget()
separator.setFixedHeight(20)
layout.addWidget(separator)
# ========== WebSocket 區域 ==========
ws_title = QLabel("WebSocket")
ws_title.setStyleSheet("""
@ -229,6 +347,94 @@ class CommPanel(QWidget):
# 清空輸入框
self.ws_url_input.clear()
def _refresh_serial_ports(self):
"""重新掃描可用的串口"""
self.serial_port_combo.clear()
# 掃描 Linux 下的串口設備
ports = []
# 掃描 USB 串口
usb_ports = glob.glob('/dev/ttyUSB*')
ports.extend(usb_ports)
# 掃描 ACM 串口
acm_ports = glob.glob('/dev/ttyACM*')
ports.extend(acm_ports)
# 排序
ports.sort()
if ports:
self.serial_port_combo.addItems(ports)
else:
self.serial_port_combo.addItem("沒有找到串口")
self.serial_port_combo.setEnabled(False)
return
self.serial_port_combo.setEnabled(True)
def _handle_add_serial(self):
"""處理添加 Serial 連接"""
port = self.serial_port_combo.currentText()
baudrate_text = self.serial_baudrate_combo.currentText()
if not port or port == "沒有找到串口":
self.status_message.emit("請選擇有效的串口", 3000)
return
try:
baudrate = int(baudrate_text)
except ValueError:
self.status_message.emit("波特率格式錯誤", 3000)
return
# 檢查是否已存在相同連接
for conn in self.serial_connections:
if conn['port'] == port:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.serial_connection_added.emit(port, baudrate)
def _handle_add_ws(self):
"""處理添加 WebSocket 連接"""
input_url = self.ws_url_input.text().strip()
if not input_url:
self.status_message.emit("請輸入 WebSocket URL", 3000)
return
# 自動添加 ws:// 前綴
if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
url = f'ws://{input_url}'
else:
url = input_url
# 基本 URL 格式驗證
try:
if '://' in url:
parts = url.split('://', 1)
if len(parts) == 2 and ':' not in parts[1]:
self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
return
except:
self.status_message.emit("URL 格式錯誤", 3000)
return
# 檢查是否已存在相同連接
for conn in self.ws_connections:
if conn['url'] == url:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.ws_connection_added.emit(url)
# 清空輸入框
self.ws_url_input.clear()
def create_udp_connection_panel(self, conn):
"""創建 UDP 連接面板"""
panel = QWidget()
@ -396,3 +602,87 @@ class CommPanel(QWidget):
"""從列表中移除 WebSocket 連接"""
if conn in self.ws_connections:
self.ws_connections.remove(conn)
def create_serial_connection_panel(self, conn):
"""創建 Serial 連接面板"""
panel = QWidget()
panel.setStyleSheet("""
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
padding: 8px;
border: 1px solid #444;
}
""")
layout = QHBoxLayout(panel)
layout.setContentsMargins(8, 8, 8, 8)
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
info_label.setStyleSheet("color: #DDD; font-size: 12px;")
# 狀態指示器
status_label = QLabel("●")
if conn.get('enabled', False):
status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
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))
toggle_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #555; }
""")
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
remove_btn.setStyleSheet("""
QPushButton {
background-color: #d32f2f;
color: white;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #b71c1c; }
""")
layout.addWidget(status_label)
layout.addWidget(info_label)
layout.addStretch()
layout.addWidget(toggle_btn)
layout.addWidget(remove_btn)
# 儲存引用
panel.connection = conn
panel.toggle_btn = toggle_btn
panel.status_label = status_label
return panel
def add_serial_panel(self, conn):
"""添加 Serial 連接面板到列表"""
panel = self.create_serial_connection_panel(conn)
self.serial_list_layout.addWidget(panel)
self.serial_connections.append(conn)
return panel
def remove_serial_connection_from_list(self, conn):
"""從列表中移除 Serial 連接"""
if conn in self.serial_connections:
self.serial_connections.remove(conn)

248
src/GUI/communication.py
Unescape Escape View File

@ -11,7 +11,7 @@ import socket
from pymavlink import mavutil
from geometry_msgs.msg import Point, Vector3
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
from std_msgs.msg import Float64
from std_msgs.msg import Float64, String
from mavros_msgs.msg import State, VfrHud
from mavros_msgs.srv import CommandBool, CommandTOL
@ -20,12 +20,15 @@ class DroneSignals(QObject):
class UDPMavlinkReceiver(threading.Thread):
"""UDP MAVLink 接收器"""
def __init__(self, ip, port, signals, connection_name):
def __init__(self, ip, port, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.ip = ip
self.port = port
self.signals = signals
self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用
# UDP 使用原始 socket_id = 8
self.socket_id = monitor.get_or_assign_socket_id('8') if monitor else 0
self.running = False
self.sock = None
@ -61,7 +64,7 @@ class UDPMavlinkReceiver(threading.Thread):
try:
msg_type = msg.get_type()
system_id = msg.get_srcSystem()
drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源
drone_id = f"s{self.socket_id}_{system_id}"
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
@ -116,11 +119,13 @@ class UDPMavlinkReceiver(threading.Thread):
})
elif msg_type == "VFR_HUD":
groundspeed = msg.groundspeed
heading = msg.heading
self.signals.update_signal.emit('hud', drone_id, {
'heading': heading,
'groundspeed': groundspeed
'airspeed': msg.airspeed,
'groundspeed': msg.groundspeed,
'heading': msg.heading,
'throttle': msg.throttle,
'alt': msg.alt,
'climb': msg.climb
})
except Exception as e:
@ -132,12 +137,15 @@ class UDPMavlinkReceiver(threading.Thread):
class SerialMavlinkReceiver(threading.Thread):
"""串口 MAVLink 接收器"""
def __init__(self, port, baudrate, signals, connection_name):
def __init__(self, port, baudrate, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.port = port
self.baudrate = baudrate
self.signals = signals
self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用
# Serial 使用原始 socket_id = 5
self.socket_id = monitor.get_or_assign_socket_id('5') if monitor else 0
self.running = False
self.mav = None
@ -184,7 +192,7 @@ class SerialMavlinkReceiver(threading.Thread):
try:
msg_type = msg.get_type()
system_id = msg.get_srcSystem()
drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源
drone_id = f"s{self.socket_id}_{system_id}"
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
@ -239,11 +247,13 @@ class SerialMavlinkReceiver(threading.Thread):
})
elif msg_type == "VFR_HUD":
groundspeed = msg.groundspeed
heading = msg.heading
self.signals.update_signal.emit('hud', drone_id, {
'heading': heading,
'groundspeed': groundspeed
'airspeed': msg.airspeed,
'groundspeed': msg.groundspeed,
'heading': msg.heading,
'throttle': msg.throttle,
'alt': msg.alt,
'climb': msg.climb
})
except Exception as e:
@ -255,12 +265,12 @@ class SerialMavlinkReceiver(threading.Thread):
class WebSocketMavlinkReceiver(threading.Thread):
"""WebSocket MAVLink 接收器"""
def __init__(self, url, signals, connection_name):
def __init__(self, url, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.url = url
self.signals = signals
self.connection_name = connection_name
self.running = False
self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False
self.max_retries = 5
self.base_delay = 1.0
@ -319,10 +329,10 @@ class WebSocketMavlinkReceiver(threading.Thread):
def process_websocket_message(self, data):
"""處理 WebSocket 訊息"""
try:
drone_id = data.get('system_id')
drone_id = f"s9_{drone_id}" if drone_id else None
if not drone_id:
system_id = data.get('system_id')
if not system_id:
return
drone_id = f"s{self.socket_id}_{system_id}"
# 模式
if 'mode' in data:
@ -371,6 +381,7 @@ class WebSocketMavlinkReceiver(threading.Thread):
self.running = False
class DroneMonitor(Node):
# Subscribe to drone ROS2 topics
def __init__(self):
super().__init__('drone_monitor')
self.signals = DroneSignals()
@ -396,111 +407,112 @@ class DroneMonitor(Node):
# ================================================================================
self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
# ================================================================================
# ================================================================================
# 【新增】Socket ID 重新分配機制 (從 0 開始)
# ================================================================================
self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id}
self.socket_id_counter = 0 # 當前分配到的最大socket_id
self.socket_id_lock = Lock() # 線程安全鎖
# ================================================================================
# ================================================================================
# 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取)
# ================================================================================
self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'}
self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0}
# ================================================================================
self.serial_receivers = []
# 主题检测定时器
self.create_timer(1.0, self.scan_topics)
def get_or_assign_socket_id(self, original_socket_id):
"""根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配）
同一個原始 socket_id 會得到同一個分配的 ID
"""
original_socket_id = str(original_socket_id)
with self.socket_id_lock:
if original_socket_id not in self.socket_id_mapping:
# 分配新的 socket_id
self.socket_id_mapping[original_socket_id] = self.socket_id_counter
print(f"🆕 Socket ID 映射: 原始 {original_socket_id} -> 分配 {self.socket_id_counter}")
self.socket_id_counter += 1
return self.socket_id_mapping[original_socket_id]
def scan_topics(self):
topics = self.get_topic_names_and_types()
drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
found_drones = set()
for topic_name, _ in topics:
if match := drone_pattern.match(topic_name):
drone_id, topic_type = match.groups()
found_drones.add(drone_id)
sys_id, topic_type = match.groups()
found_drones.add(sys_id)
with self.lock:
self.drone_topics.setdefault(drone_id, set()).add(topic_type)
self.drone_topics.setdefault(sys_id, set()).add(topic_type)
for sys_id in found_drones:
# 為每個 sys_id 分配 socket_id（如果還沒有分配）
# 注意：如果後續 summary 提供了 socket_id，會使用 summary 的映射覆蓋
if sys_id not in self.sys_to_socket_id:
# 暫時所有 ROS2 topic 共享同一個原始 socket_id，等 summary 提供實際的 socket_id
self.sys_to_socket_id[sys_id] = self.get_or_assign_socket_id('0')
for drone_id in found_drones:
if not hasattr(self, f'drone_{drone_id}_subs'):
self.setup_drone(drone_id)
if not hasattr(self, f'drone_{sys_id}_subs'):
self.setup_drone(sys_id)
def setup_drone(self, drone_id):
base_topic = f'/MavLinkBus/{drone_id}'
def setup_drone(self, sys_id):
# sys_id 格式： sys11, sys12, ...
base_topic = f'/fc_network/vehicle/{sys_id}'
# Add service clients
self.arm_clients[drone_id] = self.create_client(
# Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
self.arm_clients[sys_id] = self.create_client(
CommandBool,
f'{base_topic}/cmd/arming'
)
self.takeoff_clients[drone_id] = self.create_client(
self.takeoff_clients[sys_id] = self.create_client(
CommandTOL,
f'{base_topic}/cmd/takeoff'
)
# Add setpoint publisher
self.setpoint_pubs[drone_id] = self.create_publisher(
self.setpoint_pubs[sys_id] = self.create_publisher(
Point,
f'{base_topic}/setpoint_position/local',
10
)
subs = {
'attitude': self.create_subscription(
Imu,
f'{base_topic}/attitude',
lambda msg, did=drone_id: self.attitude_callback(did, msg),
10
),
'battery': self.create_subscription(
BatteryState,
f'{base_topic}/battery',
lambda msg, did=drone_id: self.battery_callback(did, msg),
lambda msg, sid=sys_id: self.battery_callback(sid, msg),
10
),
'global': self.create_subscription(
'position': self.create_subscription(
NavSatFix,
f'{base_topic}/global_position/global',
lambda msg, did=drone_id: self.gps_callback(did, msg),
10
),
'rel_alt': self.create_subscription(
Float64,
f'{base_topic}/global_position/rel_alt',
lambda msg, did=drone_id: self.altitude_callback(did, msg),
10
),
'local_pose': self.create_subscription(
Point,
f'{base_topic}/local_position/pose',
lambda msg, did=drone_id: self.local_pose_callback(did, msg),
f'{base_topic}/position',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
),
'local_vel': self.create_subscription(
Vector3,
f'{base_topic}/local_position/velocity',
lambda msg, did=drone_id: self.local_vel_callback(did, msg),
10
),
'loss_rate': self.create_subscription(
Float64,
f'{base_topic}/packet_loss_rate',
lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
10
),
'state': self.create_subscription(
State,
f'{base_topic}/state',
lambda msg, did=drone_id: self.state_callback(did, msg),
10
),
'ping': self.create_subscription(
Float64,
f'{base_topic}/ping',
lambda msg, did=drone_id: self.ping_callback(did, msg),
'summary': self.create_subscription(
String,
f'{base_topic}/summary',
lambda msg, sid=sys_id: self.summary_callback(sid, msg),
10
),
'vfr_hud': self.create_subscription(
VfrHud,
f'{base_topic}/vfr_hud',
lambda msg, did=drone_id: self.hud_callback(did, msg),
lambda msg, sid=sys_id: self.hud_callback(sid, msg),
10
)
}
setattr(self, f'drone_{drone_id}_subs', subs)
setattr(self, f'drone_{sys_id}_subs', subs)
async def arm_drone(self, drone_id, arm):
if drone_id not in self.arm_clients:
@ -582,8 +594,14 @@ class DroneMonitor(Node):
msg.angular_velocity.z)
}
def battery_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'battery')] = {
def battery_callback(self, sys_id, msg):
# 使用映射獲取實際的 drone_id
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
# 如果還沒有從 summary 獲取到映射，則不處理
if actual_drone_id is None:
return
self.latest_data[(actual_drone_id, 'battery')] = {
'voltage': msg.voltage
}
@ -596,8 +614,62 @@ class DroneMonitor(Node):
'armed': msg.armed
}
def gps_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'gps')] = {
def summary_callback(self, sys_id, msg):
"""處理 summary topic (JSON 格式)"""
try:
data = json.loads(msg.data)
mode = data.get('mode_name', 'UNKNOWN')
if mode in self.filtered_modes:
return
# 從 summary 獲取原始 socket_id，並映射到分配的 socket_id
original_socket_id = data.get('socket_id')
if original_socket_id is not None:
# 使用原始 socket_id 獲取或分配統一的 socket_id
assigned_socket_id = self.get_or_assign_socket_id(original_socket_id)
else:
# 如果沒有 socket_id，使用 sys_to_socket_id 映射
assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0)
sysid = data.get('sysid')
if sysid is not None:
actual_drone_id = f's{assigned_socket_id}_{sysid}'
# ================================================================================
# 【關鍵】保存 sys_id 到 actual_drone_id 的映射
# ================================================================================
self.sys_to_actual_id[sys_id] = actual_drone_id
# ================================================================================
else:
# 如果沒有 sysid，使用 sys_id 中的數字
sys_num = sys_id.replace('sys', '')
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
self.sys_to_actual_id[sys_id] = actual_drone_id
self.latest_data[(actual_drone_id, 'state')] = {
'mode': mode,
'armed': data.get('armed', False),
'socket_id': original_socket_id,
'sysid': sysid,
'vehicle_type': data.get('vehicle_type'),
'autopilot': data.get('autopilot'),
'gps_fix': data.get('gps_fix'),
'gps_fix_type': data.get('gps_fix'),
'connected': data.get('connected')
}
except json.JSONDecodeError as e:
print(f"Error parsing summary JSON for {sys_id}: {e}")
except Exception as e:
print(f"Error in summary_callback for {sys_id}: {e}")
def gps_callback(self, sys_id, msg):
# 使用映射獲取實際的 drone_id
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
# 如果還沒有從 summary 獲取到映射，則不處理
if actual_drone_id is None:
return
self.latest_data[(actual_drone_id, 'gps')] = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
@ -606,7 +678,7 @@ class DroneMonitor(Node):
# ================================================================================
# 【新增】儲存 GPS 資料到 drone_gps 字典
# ================================================================================
self.drone_gps[drone_id] = {
self.drone_gps[actual_drone_id] = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
@ -632,8 +704,14 @@ class DroneMonitor(Node):
'z': msg.z
}
def hud_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'hud')] = {
def hud_callback(self, sys_id, msg):
# 使用映射獲取實際的 drone_id
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
# 如果還沒有從 summary 獲取到映射，則不處理
if actual_drone_id is None:
return
self.latest_data[(actual_drone_id, 'hud')] = {
'airspeed': msg.airspeed,
'groundspeed': msg.groundspeed,
'heading': msg.heading,
@ -655,7 +733,7 @@ class DroneMonitor(Node):
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
"""啟動串口 MAVLink 連接"""
connection_name = f"Serial_{port.replace('/', '_')}"
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name)
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
receiver.start()
self.serial_receivers.append(receiver)
print(f"Started serial connection on {port} at {baudrate} baud")

224
src/GUI/gui.py
Unescape Escape View File

@ -13,6 +13,7 @@ from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkRece
from map_layout import DroneMap
from drone_panel import DronePanel, SocketGroupPanel
from comm_panel import CommPanel
from overview_table import OverviewTable
# ================================================================================
# 【新增】導入任務規劃器
@ -43,23 +44,6 @@ class ControlStationUI(QMainWindow):
# 初始化UI
self.drones = {}
self.socket_groups = {}
self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向",
"Roll", "Pitch", "Yaw", "丟包", "延遲"]
self.info_type_map = {
"mode": 0,
"armed": 1,
"battery": 2,
"altitude": 3,
"local": 4,
"velocity": 5,
"groundspeed": 6,
"heading": 7,
"roll": 8,
"pitch": 9,
"yaw": 10,
"loss_rate": 11,
"ping": 12
}
self.socket_colors = {
'0': '#00BFFF', # 天藍色 (DeepSkyBlue)
@ -83,7 +67,8 @@ class ControlStationUI(QMainWindow):
self.udp_receivers = []
self.udp_connections = []
self.ws_connections = []
self.monitor.start_serial_connection('/dev/ttyUSB0', 57600)
self.serial_receivers = []
self.serial_connections = []
# ================================================================================
# 【新增】初始化任務規劃器
@ -119,29 +104,20 @@ class ControlStationUI(QMainWindow):
self.left_tab.addTab(scroll, "無人載具")
# — 分頁 2：Overview Table
self.overview_table = QTableWidget()
self.overview_table.setColumnCount(1)
self.overview_table.setRowCount(len(self.info_types))
self.overview_table.setHorizontalHeaderLabels(["資訊"])
header = self.overview_table.horizontalHeader()
header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents)
self.overview_table.verticalHeader().setVisible(False)
for i, txt in enumerate(self.info_types):
item = QTableWidgetItem(txt)
item.setFlags(Qt.ItemFlag.ItemIsEnabled)
item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
self.overview_table.setItem(i, 0, item)
self.overview_table = OverviewTable()
self.left_tab.addTab(self.overview_table, "總覽")
# — 分頁 3：通訊設定
self.comm_panel = CommPanel()
self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added)
self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added)
self.comm_panel.serial_connection_added.connect(self.handle_serial_connection_added)
self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection)
self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection)
self.comm_panel.serial_connection_toggled.connect(self.toggle_serial_connection)
self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection)
self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection)
self.comm_panel.serial_connection_removed.connect(self.remove_serial_connection)
self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout))
self.left_tab.addTab(self.comm_panel, "通訊")
@ -278,6 +254,7 @@ class ControlStationUI(QMainWindow):
# 添加地圖
right_layout.addWidget(self.drone_map.get_widget())
self.drone_map.get_gps_signal().connect(self.handle_map_click)
self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked)
# Add widgets to splitter
@ -299,7 +276,7 @@ class ControlStationUI(QMainWindow):
}
# 启动接收器
receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'])
receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'], self.monitor)
receiver.start()
self.udp_receivers.append(receiver)
new_conn['receiver'] = receiver
@ -322,7 +299,7 @@ class ControlStationUI(QMainWindow):
}
# 启动接收器
receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'])
receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'], self.monitor)
receiver.start()
self.monitor.ws_receivers.append(receiver)
new_conn['receiver'] = receiver
@ -436,6 +413,69 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000)
def handle_serial_connection_added(self, port, baudrate):
"""處理添加 Serial 連接"""
conn = {
'name': f'Serial',
'port': port,
'baudrate': baudrate,
'enabled': False,
'receiver': None
}
# 添加到列表
self.serial_connections.append(conn)
# 在 UI 中顯示
panel = self.comm_panel.add_serial_panel(conn)
self.statusBar().showMessage(f"已添加 Serial 连接: {port} @ {baudrate}", 3000)
def toggle_serial_connection(self, conn, btn, status_label):
"""切換 Serial 連接狀態"""
if conn.get('enabled', False):
# 停止連接
if conn.get('receiver'):
conn['receiver'].stop()
conn['receiver'] = None
conn['enabled'] = False
btn.setText("啟動")
status_label.setStyleSheet("color: #888; font-size: 16px;")
status_label.setToolTip("已停止")
self.statusBar().showMessage(f"已停止 Serial 連接: {conn['port']}", 3000)
else:
# 啟動連接
try:
receiver = self.monitor.start_serial_connection(conn['port'], conn['baudrate'])
conn['receiver'] = receiver
conn['enabled'] = True
btn.setText("停止")
status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
except Exception as e:
self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
def remove_serial_connection(self, conn, panel):
"""移除 Serial 連接"""
# 停止連接
if conn.get('enabled', False) and conn.get('receiver'):
conn['receiver'].stop()
# 从列表移除
if conn in self.serial_connections:
self.serial_connections.remove(conn)
# 从 comm_panel 列表移除
self.comm_panel.remove_serial_connection_from_list(conn)
# 从 UI 移除
panel.setParent(None)
panel.deleteLater()
self.statusBar().showMessage(f"已移除 Serial 连接: {conn['port']}", 3000)
def create_socket_group_panel(self, socket_id):
"""創建 socket 分組面板"""
color = self.socket_colors.get(socket_id, self.socket_colors['default'])
@ -540,6 +580,9 @@ class ControlStationUI(QMainWindow):
cells = round(voltage / 3.95)
# 計算電量百分比
if cells == 0:
percentage = 0
else:
percentage = (voltage / cells - 3.7) / 0.5 * 100
# 根據百分比設置顏色
@ -562,9 +605,12 @@ class ControlStationUI(QMainWindow):
elif msg_type == 'gps':
lat, lon = data.get('lat', 0), data.get('lon', 0)
self.drone_positions[drone_id] = (lat, lon)
text = f"{lat:.6f}°, {lon:.6f}°"
self.update_field(panel, drone_id, 'gps', text)
self.update_overview_table(drone_id, 'gps', text)
lon_text = f"{lon:.6f}°"
lat_text = f"{lat:.6f}°"
self.update_field(panel, drone_id, 'longitude', lon_text)
self.update_field(panel, drone_id, 'latitude', lat_text)
self.update_overview_table(drone_id, 'longitude', lon_text)
self.update_overview_table(drone_id, 'latitude', lat_text)
# ================================================================================
# 【新增】同時儲存到 monitor.drone_gps（處理 UDP/WebSocket 的 GPS 資料）
@ -579,6 +625,10 @@ class ControlStationUI(QMainWindow):
}
# ================================================================================
# 更新地圖上的無人機位置
heading = self.drone_headings.get(drone_id, 0) # 如果沒有航向，使用 0
self.drone_map.update_drone_position(drone_id, lat, lon, heading)
elif msg_type == 'altitude':
altitude = data.get('altitude', 0)
text = f"{altitude:.1f} m"
@ -594,15 +644,50 @@ class ControlStationUI(QMainWindow):
heading = data.get('heading')
self.drone_headings[drone_id] = heading
groundspeed = data.get('groundspeed')
airspeed = data.get('airspeed')
throttle = data.get('throttle')
hud_alt = data.get('alt')
climb = data.get('climb')
heading_text = f"{heading:.1f}°"
if isinstance(groundspeed, (int, float)):
groundspeed_text = f"{groundspeed:.1f} m/s"
else:
groundspeed_text = "--"
if isinstance(airspeed, (int, float)):
airspeed_text = f"{airspeed:.1f} m/s"
else:
airspeed_text = "--"
if isinstance(throttle, (int, float)):
throttle_text = f"{throttle:.0f}%"
else:
throttle_text = "--"
if isinstance(hud_alt, (int, float)):
hud_alt_text = f"{hud_alt:.1f} m"
else:
hud_alt_text = "--"
if isinstance(climb, (int, float)):
climb_text = f"{climb:.1f} m/s"
else:
climb_text = "--"
self.update_field(panel, drone_id, 'heading', heading_text)
self.update_field(panel, drone_id, 'groundspeed', groundspeed_text)
self.update_overview_table(drone_id, 'heading', heading_text)
self.update_overview_table(drone_id, 'groundspeed', groundspeed_text)
self.update_overview_table(drone_id, 'airspeed', airspeed_text)
self.update_overview_table(drone_id, 'throttle', throttle_text)
self.update_overview_table(drone_id, 'hud_alt', hud_alt_text)
self.update_overview_table(drone_id, 'climb', climb_text)
# 如果有位置資訊，也更新地圖上的航向
if drone_id in self.drone_positions:
lat, lon = self.drone_positions[drone_id]
self.drone_map.update_drone_position(drone_id, lat, lon, heading)
elif msg_type == 'loss_rate':
loss_rate = data.get('loss_rate', 0)
@ -631,12 +716,6 @@ class ControlStationUI(QMainWindow):
self.update_overview_table(drone_id, 'pitch', pitch_text)
self.update_overview_table(drone_id, 'yaw', yaw_text)
# 更新地圖上的無人機位置
if drone_id in self.drone_positions and drone_id in self.drone_headings:
lat, lon = self.drone_positions[drone_id]
heading = self.drone_headings[drone_id]
self.drone_map.update_drone_position(drone_id, lat, lon, heading)
# 新增處理分組勾選的方法
def handle_group_selection(self, socket_id, state):
"""處理 socket 分組勾選狀態變化"""
@ -756,47 +835,23 @@ class ControlStationUI(QMainWindow):
panel.update_field(field, text, color)
def update_overview_table(self, drone_id=None, field=None, value=None):
# Ensure the widget is available
"""更新總覽表格"""
if not hasattr(self, 'overview_table') or self.overview_table is None:
return
# Update a specific cell in the overview table
if drone_id and field and value:
col = 1 + list(self.drones.keys()).index(drone_id)
row = self.info_type_map.get(field, -1) # Get row from field mapping
if row == -1:
return # Invalid field, exit
item = self.overview_table.item(row, col)
if not item:
item = QTableWidgetItem()
self.overview_table.setItem(row, col, item)
item.setText(value) # Update the cell's text
item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) # Center the text
# If no specific update, refresh entire table
if drone_id is None:
cols = 1 + len(self.drones)
self.overview_table.setColumnCount(cols)
headers = ["資訊"] + list(self.drones.keys())
self.overview_table.setHorizontalHeaderLabels(headers)
for col, did in enumerate(self.drones, start=1):
panel = self.drones[did]
for field, row in self.info_type_map.items():
lbl = panel.findChild(QLabel, f"{did}_{field}")
val = lbl.text() if lbl else "--"
val_item = QTableWidgetItem(val)
val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
self.overview_table.setItem(row, col, val_item)
# 更新無人機引用
self.overview_table.set_drones(self.drones)
# 委託給 OverviewTable 處理
self.overview_table.update_table(drone_id, field, value)
def get_socket_id(self, drone_id):
"""從 drone_id 提取 socket_id (例如 's9_1' -> '9')"""
"""從 drone_id 提取 socket_id (例如 's0_1' -> '0')"""
import re
match = re.match(r's(\d+)_\d+', drone_id)
return match.group(1) if match else 'unknown'
match = re.match(r's(\d+)_(\d+)', drone_id)
if not match:
return 'unknown'
return match.group(1)
def add_drone(self, drone_id):
if drone_id in self.drones:
@ -978,6 +1033,21 @@ class ControlStationUI(QMainWindow):
import traceback
traceback.print_exc()
def handle_drone_clicked(self, drone_id):
"""
處理地圖上無人機被點擊事件，切換該無人機的選取狀態
Args:
drone_id: 無人機 ID
"""
print(f"地圖上點擊無人機: {drone_id}")
if drone_id in self.drones:
panel = self.drones[drone_id]
checkbox = panel.get_checkbox()
# 切換勾選狀態
checkbox.setChecked(not checkbox.isChecked())
def show_planned_waypoints(self):
"""
顯示規劃的航點（在終端輸出）

534
src/GUI/map_layout.py
Unescape Escape View File

@ -32,24 +32,120 @@ class DroneMap:
<script src="qrc:///qtwebchannel/qwebchannel.js"></script>
<style>
html, body, #map { height: 100%; margin: 0; }
#map {
user-select: none;
-webkit-user-select: none;
-moz-user-select: none;
-ms-user-select: none;
}
.map-controls {
position: absolute;
top: 10px;
right: 10px;
z-index: 1000;
}
.map-controls-bottom {
position: absolute;
bottom: 10px;
left: 10px;
z-index: 1000;
}
.control-button {
padding: 8px 12px;
background-color: #f44336;
background-color: #F44336;
color: white;
border: none;
border-radius: 4px;
cursor: pointer;
font-size: 12px;
font-size: 13px;
font-weight: bold;
box-shadow: 0 2px 5px rgba(0,0,0,0.2);
}
.control-button:hover {
background-color: #d32f2f;
background-color: #D32F2F;
}
.map-toggle-button {
padding: 8px 12px;
background-color: #2E7D32;
color: white;
border: none;
border-radius: 4px;
cursor: pointer;
font-size: 13px;
font-weight: bold;
box-shadow: 0 2px 5px rgba(0,0,0,0.2);
}
.map-toggle-button:hover {
background-color: #1B5E20;
}
.mission-info-panel {
position: absolute;
bottom: 10px;
right: 10px;
z-index: 1000;
background-color: rgba(255, 255, 255, 0.95);
padding: 12px;
border-radius: 6px;
box-shadow: 0 2px 8px rgba(0,0,0,0.3);
min-width: 200px;
}
.mission-info-row {
margin-bottom: 8px;
font-size: 12px;
color: #333;
}
.mission-info-label {
font-weight: bold;
color: #555;
}
.mission-info-value {
color: #2196F3;
font-family: monospace;
}
.mission-start-button {
width: 100%;
padding: 10px;
background-color: #7FBA82;
color: white;
border: none;
border-radius: 4px;
cursor: pointer;
font-size: 13px;
font-weight: bold;
margin-top: 8px;
}
.mission-start-button:hover {
background-color: #6FA872;
}
.mission-start-button:disabled {
background-color: #ccc;
cursor: not-allowed;
}
.selection-buttons {
display: flex;
flex-direction: column;
gap: 5px;
margin-bottom: 10px;
padding-bottom: 10px;
border-bottom: 1px solid #ddd;
}
.selection-button {
width: 100%;
padding: 8px;
background-color: #64B5F6;
color: white;
border: none;
border-radius: 4px;
cursor: pointer;
font-size: 13px;
font-weight: bold;
transition: background-color 0.2s;
}
.selection-button:hover {
background-color: #42A5F5;
}
.selection-button.active {
background-color: #1976D2;
}
</style>
</head>
@ -58,6 +154,25 @@ class DroneMap:
<div class="map-controls">
<button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
</div>
<div class="map-controls-bottom">
<button class="map-toggle-button" onclick="toggleMapLayer()">地圖：預設</button>
</div>
<div class="mission-info-panel">
<div class="selection-buttons">
<button class="selection-button" id="select-drones-btn" onclick="toggleDroneSelection()">框選無人機</button>
<button class="selection-button" id="select-rect-btn" onclick="toggleRectSelection()">框選方形區域</button>
<button class="selection-button" id="select-polygon-btn" onclick="togglePolygonSelection()">多點選擇區域</button>
</div>
<div class="mission-info-row">
<span class="mission-info-label">中心點: </span>
<span class="mission-info-value" id="center-position">未設定</span>
</div>
<div class="mission-info-row">
<span class="mission-info-label">目標點: </span>
<span class="mission-info-value" id="target-position">未設定</span>
</div>
<button class="mission-start-button" id="start-mission-btn" onclick="startMission()" disabled>開始任務</button>
</div>
<script>
var bridge;
@ -66,25 +181,107 @@ class DroneMap:
});
var map = L.map('map').setView([0, 0], 19);
L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
// 創建不同的地圖圖層
var streetLayer = L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
maxZoom: 19,
attribution: '© OpenStreetMap contributors'
}).addTo(map);
});
var satelliteLayer = L.tileLayer('https://server.arcgisonline.com/ArcGIS/rest/services/World_Imagery/MapServer/tile/{z}/{y}/{x}', {
maxZoom: 19,
attribution: 'Tiles © Esri'
});
// 默認使用街道圖
var currentLayer = streetLayer;
currentLayer.addTo(map);
var isSatellite = false;
// 切換地圖圖層函數
function toggleMapLayer() {
if (isSatellite) {
map.removeLayer(satelliteLayer);
streetLayer.addTo(map);
isSatellite = false;
document.querySelector('.map-toggle-button').textContent = '地圖：預設';
} else {
map.removeLayer(streetLayer);
satelliteLayer.addTo(map);
isSatellite = true;
document.querySelector('.map-toggle-button').textContent = '地圖：衛星';
}
}
// 地圖點擊事件
map.on('click', function(e) {
if (selectionMode === 'polygon') {
// 多點選擇模式：添加點
addPolygonPoint(e.latlng.lat, e.latlng.lng);
} else if (!selectionMode) {
// 正常模式：發送GPS信號
if (bridge) {
bridge.emitGpsSignal(e.latlng.lat, e.latlng.lng);
console.log('點擊位置:', e.latlng.lat, e.latlng.lng);
}
}
});
var arrowIcon = L.icon({
iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png',
iconSize: [40, 40],
iconAnchor: [20, 20]
// 地圖拖曳事件（用於矩形選擇）
map.on('mousedown', function(e) {
if (selectionMode === 'rect' || selectionMode === 'drones') {
isDrawing = true;
drawStartPoint = e.latlng;
L.DomEvent.preventDefault(e.originalEvent);
L.DomEvent.stopPropagation(e);
}
});
map.on('mousemove', function(e) {
if (isDrawing && (selectionMode === 'rect' || selectionMode === 'drones') && drawStartPoint) {
// 更新臨時矩形
if (tempRectangle) {
selectionLayer.removeLayer(tempRectangle);
}
var bounds = L.latLngBounds(drawStartPoint, e.latlng);
tempRectangle = L.rectangle(bounds, {
color: selectionMode === 'drones' ? '#9C27B0' : '#FF6B6B',
weight: 2,
fillOpacity: selectionMode === 'drones' ? 0 : 0.2,
dashArray: selectionMode === 'drones' ? '5, 5' : null
}).addTo(selectionLayer);
}
});
map.on('mouseup', function(e) {
if (isDrawing && (selectionMode === 'rect' || selectionMode === 'drones') && drawStartPoint) {
isDrawing = false;
var bounds = L.latLngBounds(drawStartPoint, e.latlng);
finishRectSelection(bounds);
drawStartPoint = null;
}
});
function createArrowIcon(color) {
return L.divIcon({
className: 'drone-arrow',
html: `
<div style="
width: 30px; height: 30px;
display: flex;
align-items: center;
justify-content: center;
">
<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: [30, 30],
iconAnchor: [15, 15] // ★ 箭頭往左上移，使 ID 顯示在右下
});
}
function getColorBySocketId(id) {
if (id.startsWith("s0_")) return "#00BFFF"; // 天藍色
if (id.startsWith("s1_")) return "#FFD700"; // 金色
@ -100,14 +297,11 @@ class DroneMap:
}
function createIdIcon(id) {
const color = getColorBySocketId(id);
const sysid = id.split('_')[1];
return L.divIcon({
className: 'drone-id',
html: `<div style="
position: relative;
left: 2px;
background-color: ${color};
background-color: transparent;
color: black;
width: 16px;
height: 16px;
@ -116,10 +310,11 @@ class DroneMap:
align-items: center;
justify-content: center;
font-weight: bold;
font-size: 10px;
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: [20, 20],
iconAnchor: [10, 10]
iconSize: [16, 16],
iconAnchor: [8, 6] // ★ icon 中心 = 經緯度點
});
}
@ -138,6 +333,251 @@ class DroneMap:
var centerMarker = null; // 中心點標記
var targetMarker = null; // 目標點標記
var missionLine = null; // 中心點到目標點的虛線
var centerPosition = null; // 中心點經緯度
var targetPosition = null; // 目標點經緯度
// 選擇工具變量
var selectionMode = null; // 'drones', 'rect', 'polygon', null
var selectionLayer = L.layerGroup().addTo(map);
var polygonPoints = []; // 多點選擇的點
var polygonMarkers = []; // 多點選擇的標記
var tempRectangle = null; // 臨時矩形
var isDrawing = false; // 是否正在繪製
var drawStartPoint = null; // 繪製起點
// ================================================================================
// 更新任務信息面板
function updateMissionInfo() {
const centerElem = document.getElementById('center-position');
const targetElem = document.getElementById('target-position');
const startBtn = document.getElementById('start-mission-btn');
if (centerPosition) {
centerElem.textContent = `${centerPosition.lat.toFixed(6)}°, ${centerPosition.lng.toFixed(6)}°`;
} else {
centerElem.textContent = '未設定';
}
if (targetPosition) {
targetElem.textContent = `${targetPosition.lat.toFixed(6)}°, ${targetPosition.lng.toFixed(6)}°`;
} else {
targetElem.textContent = '未設定';
}
// 只有當中心點和目標點都設定時才啟用按鈕
if (centerPosition && targetPosition) {
startBtn.disabled = false;
} else {
startBtn.disabled = true;
}
}
// ================================================================================
// 選擇工具函數
// ================================================================================
function toggleDroneSelection() {
clearSelectionMode();
if (selectionMode === 'drones') {
selectionMode = null;
document.getElementById('select-drones-btn').classList.remove('active');
map.dragging.enable();
} else {
selectionMode = 'drones';
document.getElementById('select-drones-btn').classList.add('active');
map.dragging.disable();
}
}
function toggleRectSelection() {
clearSelectionMode();
if (selectionMode === 'rect') {
selectionMode = null;
document.getElementById('select-rect-btn').classList.remove('active');
map.dragging.enable();
} else {
selectionMode = 'rect';
document.getElementById('select-rect-btn').classList.add('active');
map.dragging.disable();
}
}
function togglePolygonSelection() {
if (selectionMode === 'polygon') {
// 第二次點擊：完成選擇
if (polygonPoints.length >= 3) {
finishPolygonSelection();
} else {
alert('至少需要3個點來形成區域');
// 清除並重置
clearSelectionMode();
clearPolygonPoints();
selectionMode = null;
document.getElementById('select-polygon-btn').classList.remove('active');
}
} else {
// 第一次點擊：清除上次的點位並啟動模式
clearSelectionMode();
clearPolygonPoints();
selectionMode = 'polygon';
document.getElementById('select-polygon-btn').classList.add('active');
map.dragging.disable();
}
}
function clearSelectionMode() {
// 清除所有按鈕的激活狀態
document.getElementById('select-drones-btn').classList.remove('active');
document.getElementById('select-rect-btn').classList.remove('active');
document.getElementById('select-polygon-btn').classList.remove('active');
// 清除選擇圖層
selectionLayer.clearLayers();
tempRectangle = null;
// 重新啟用地圖拖曳
map.dragging.enable();
}
function addPolygonPoint(lat, lng) {
polygonPoints.push([lat, lng]);
// 添加邊界點標記
var marker = L.circleMarker([lat, lng], {
radius: 5,
color: '#FF6B6B',
fillColor: '#FF6B6B',
fillOpacity: 0.8
}).addTo(selectionLayer);
polygonMarkers.push(marker);
// 如果有多個點，繪製連線
if (polygonPoints.length > 1) {
L.polyline(polygonPoints, {
color: '#FF6B6B',
weight: 2,
dashArray: '5, 5'
}).addTo(selectionLayer);
}
console.log('添加邊界點:', lat, lng, '總共:', polygonPoints.length);
}
function clearPolygonPoints() {
polygonPoints = [];
polygonMarkers.forEach(m => selectionLayer.removeLayer(m));
polygonMarkers = [];
selectionLayer.clearLayers();
}
function finishPolygonSelection() {
if (polygonPoints.length < 3) {
alert('至少需要3個點來形成區域');
return;
}
// 繪製最終多邊形
var polygon = L.polygon(polygonPoints, {
color: '#FF6B6B',
fillColor: '#FF6B6B',
fillOpacity: 0.2,
weight: 2
}).addTo(selectionLayer);
// 發送多邊形數據到Python
if (bridge) {
var pointsStr = JSON.stringify(polygonPoints);
bridge.polygonSelected(pointsStr);
console.log('多邊形選擇完成:', polygonPoints);
}
// 重置狀態
selectionMode = null;
document.getElementById('select-polygon-btn').classList.remove('active');
map.dragging.enable();
}
function finishRectSelection(bounds) {
var selectedDrones = [];
// 清除臨時矩形
if (tempRectangle) {
selectionLayer.removeLayer(tempRectangle);
tempRectangle = null;
}
if (selectionMode === 'drones') {
// 框選無人機模式
Object.keys(markers).forEach(droneId => {
var pos = markers[droneId].getLatLng();
if (bounds.contains(pos)) {
selectedDrones.push(droneId);
// 觸發無人機點擊信號
if (bridge) {
bridge.emitDroneClicked(droneId);
}
}
});
console.log('框選無人機:', selectedDrones);
// 不保留選擇框，直接完成
} else if (selectionMode === 'rect') {
// 框選區域模式
var rectPoints = [
[bounds.getNorth(), bounds.getWest()],
[bounds.getNorth(), bounds.getEast()],
[bounds.getSouth(), bounds.getEast()],
[bounds.getSouth(), bounds.getWest()]
];
// 繪製最終矩形
L.rectangle(bounds, {
color: '#FF6B6B',
fillColor: '#FF6B6B',
fillOpacity: 0.2,
weight: 2
}).addTo(selectionLayer);
// 添加四個角的標記
rectPoints.forEach(point => {
L.circleMarker(point, {
radius: 5,
color: '#FF6B6B',
fillColor: '#FF6B6B',
fillOpacity: 0.8
}).addTo(selectionLayer);
});
// 發送矩形數據到Python
if (bridge) {
var pointsStr = JSON.stringify(rectPoints);
bridge.rectangleSelected(pointsStr);
console.log('矩形選擇完成:', rectPoints);
}
}
// 重置狀態
selectionMode = null;
document.getElementById('select-drones-btn').classList.remove('active');
document.getElementById('select-rect-btn').classList.remove('active');
map.dragging.enable();
}
// ================================================================================
// 開始任務
function startMission() {
if (!centerPosition || !targetPosition) {
alert('請先設定中心點和目標點');
return;
}
if (bridge) {
bridge.startMissionSignal(
centerPosition.lat, centerPosition.lng,
targetPosition.lat, targetPosition.lng
);
console.log('開始任務:', centerPosition, targetPosition);
}
}
// ================================================================================
function initTrajectory(id) {
@ -195,12 +635,16 @@ class DroneMap:
initTrajectory(id);
addTrajectoryPoint(id, lat, lon);
const color = getColorBySocketId(id);
markers[id] = L.marker([lat, lon], {
icon: arrowIcon,
icon: createArrowIcon(color),
rotationAngle: heading,
rotationOrigin: 'center'
})
.on('click', function () {
if (bridge) {
bridge.emitDroneClicked(id);
}
focusOn(id);
})
.addTo(map);
@ -210,6 +654,9 @@ class DroneMap:
zIndexOffset: 1000
})
.on('click', function() {
if (bridge) {
bridge.emitDroneClicked(id);
}
focusOn(id);
})
.addTo(map);
@ -237,6 +684,11 @@ class DroneMap:
// 清除舊的任務規劃標記
clearMissionPlan();
// 保存中心點和目標點位置
centerPosition = {lat: centerLat, lng: centerLon};
targetPosition = {lat: targetLat, lng: targetLon};
updateMissionInfo();
// 繪製中心點標記 "C"（縮小到 20px）
var centerIcon = L.divIcon({
className: 'mission-center',
@ -323,6 +775,11 @@ class DroneMap:
missionLine = null;
}
// 清除位置資訊
centerPosition = null;
targetPosition = null;
updateMissionInfo();
console.log('任務規劃已清除');
}
// ================================================================================
@ -410,9 +867,29 @@ class DroneMap:
"""獲取 GPS 信號"""
return self.bridge.gps_signal
def get_drone_clicked_signal(self):
"""獲取無人機點擊信號"""
return self.bridge.drone_clicked
def get_start_mission_signal(self):
"""獲取開始任務信號"""
return self.bridge.start_mission_signal
def get_rectangle_selected_signal(self):
"""獲取矩形選擇信號"""
return self.bridge.rectangle_selected
def get_polygon_selected_signal(self):
"""獲取多邊形選擇信號"""
return self.bridge.polygon_selected
class MapBridge(QObject):
"""JavaScript 和 Python 之間的橋接類"""
gps_signal = pyqtSignal(float, float) # lat, lon
drone_clicked = pyqtSignal(str) # drone_id
start_mission_signal = pyqtSignal(float, float, float, float) # center_lat, center_lon, target_lat, target_lon
rectangle_selected = pyqtSignal(str) # JSON string of rectangle points
polygon_selected = pyqtSignal(str) # JSON string of polygon points
def __init__(self):
super().__init__()
@ -421,3 +898,26 @@ class MapBridge(QObject):
def emitGpsSignal(self, lat, lon):
"""供 JavaScript 調用的方法"""
self.gps_signal.emit(lat, lon)
@pyqtSlot(str)
def emitDroneClicked(self, drone_id):
"""供 JavaScript 調用的方法 - 當無人機被點擊時"""
self.drone_clicked.emit(drone_id)
@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)
print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
@pyqtSlot(str)
def rectangleSelected(self, points_json):
"""供 JavaScript 調用的方法 - 矩形選擇完成"""
self.rectangle_selected.emit(points_json)
print(f"📦 矩形區域已選擇: {points_json}")
@pyqtSlot(str)
def polygonSelected(self, points_json):
"""供 JavaScript 調用的方法 - 多邊形選擇完成"""
self.polygon_selected.emit(points_json)
print(f"🔷 多邊形區域已選擇: {points_json}")

116
src/GUI/overview_table.py
Unescape Escape View File

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

190
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
Unescape Escape View File

@ -0,0 +1,190 @@
這個檔案整理 此專案下 程式代碼的流程與思路
只會挑出重要的變數與方法描述
以利後續開發使用
# 開發此專案的注意事項
- 預設 autopilot 的 component id = 1
- 不允許 system id 重複
- 增加一個固定數值監控然後要到 ros2 topic
- mavlinkROS2Node.py 檔案內
- PublishRateController.topic_intervals 建立
- VehicleStatusPublisher._publish_vehicle_status 登記
- VehicleStatusPublisher._publish_XXX 實作
- mavlinkObject.py 檔案內
- mavlink_bridge.message_handlers 登記
- mavlink_bridge._handle_XXX 實作
- mavlink_object.bridge_msg_types 登記 (這個可以用介面調)
- mavlinkVehicleView.py 檔案內
- 注意對應的資料存放區
---
# 檔案結構
特別注意:
1. 有標註 [async method] 都是不該被直接呼叫的內部方法
- *valuable* 這個是變數 **沒有括號**
- *method (parameters...)* 這個是方法 **有括號**
## mainOrchestrator.py : 程式進入點
### **[Class]** Orchestrator
最上層的發配資源與啟動終端機面板的調配者
- *self.manager* 存放 async_io_manager 實例
- *self.bridge* 存放 mavlink_bridge 實例
- *self.plumber* 存放 serial_manager 實例
- *self.vehicle_registry* 存放 vehicle_registry 實例
- *self.panel_thread* 面板的執行緒
- *self.panelState* 暫存面板與調配者互動的資料流動區
- 面板運行狀態
- 面板操作結果
- 其他模組的運行狀態
---
- *mainLoop()* 核心方法
- 更新個模組狀態到 *self.panelState*
- 對應面板來的操作指令
---
對於 async_io_manager 控制實現
- *create_udp_object()*
- *delete_udp_object()*
- *add_target_to_object()*
- *remove_target_from_object()*
---
關於載具管理與檢視
- *_update_vehicles_list()*
- *_prepare_vehicle_info()*
---
關於 serial_manager 控制實現
- *create_serial_port_object()*
### **[Class]** ControlPanel
面板的核心運行物件
把自己的變數 獨立出來都放到 PanelState 去
- *panel_thread()* 核心方法
- 主選單的引入
- 主選單下所有的按鍵操作
- 定義所有人為操作後續面板執行緒行為
- *menu_tree()* 基礎選單的定義檔
---
關於 udp object 的操作
- *create_object_list_menu()* object 選單的定義檔
- *show_object_info()* 顯示 object 資訊
- *select_target_socket()* object 對於轉拋功能的操作
---
關於 serial 的操作
- *create_serial_port_menu()*
- *create_linked_serial_menu()*
- *show_linked_serial_info()*
---
關於載具檢視與操作
- *create_vehicles_list_menu()*
- *show_vehicle_info()*
### **[Class]** PanelState
作為面板執行緒(ControlPanel)與調配者(Orchestrator)溝通的管道
不包含具體實作方法 是 ControlPanel 的延伸
- *self.panel_info_msg_list* 顯示在面板上的資訊訊息
## mavlinkObject.py
### 全域變數
- *stream_bridge_ring*
- *return_packet_ring*
### **[Class]** mavlink_bridge
唯一實例
實際去解析 mavlink 封包的地方
接收 stream_bridge_ring 與 return_packet_ring 的資料
這邊是比較偏自動化 不會被操作的
- *self.thread* 自己的執行緒
---
- *_run_thread()* 核心方法
- *_handle_XXXXX()* 每一種單項 mavlink 封包的解析
- *send_message()* 是 _send_to_socket() 的高階包裝 跟 ros2 介面做互動的方法
- *_send_to_socket()* 把要傳送的封包 丟給 mavlink 去處理
### **[Class]** async_io_manager
唯一實例
異步 event loop
沒有核心方法
這邊主要是管理 mavlink_object 的地方 (但不會對於某個 mavlink_object 內部需求做操作)
- *self.thread* 自己的執行緒
- *self.managed_objects* 資料結構 socket_id: mavlink_object
---
- *add_mavlink_object(mavlink_object)* [call method] 把一個 mavlink_object 物件加入管理
- *_async_add_mavlink_object(mavlink_object)* [async method] 對應上面的內部方法 不該直接使用
- *remove_mavlink_object(socket_id)* [call method] 從管理區把指定 mavlink_object 移除
### **[Class]** mavlink_object
儲存 mavlink socket
處理 mavlink 封包分流的地方
- *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) }
- *self.mavlink_socket* 從 pymavlink 繼承的socket物件
- *self.state* 描述這個 socket 物件的狀態
---
- *process_data()* [async method] 核心方法
- *remove_target_socket()* *add_target_socket()*
- *message_put_queue()* 把要傳送的封包放到自己這個物件的暫存區 會由 process_data() 依照異步流程被實際丟出
## serialManager.py
看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件
### **[Class]** serial_manager
異步 event loop
管理 mavlink_object 的地方
- *self.thread* 自己的執行緒
- *self.loop* 自己的事件迴圈
---
- *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通
- *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序
- *remove_serial_link()* [call method] 關閉指定的 serial 端口
- *_async_remove_serial_link()* [async method]
### **[Class]** serial_object
被塞在 serial_manager 裡面
只是一個變數物件
用來被儲存 serial 的資訊
- *self.transport*
- *self.protocol*
- *self.udp_handler* UDP 端口物件
- *self.serial_handler* Serial 端口物件
### **[Class]** UDPHandler
處理 UDP 收發的端口 作為一個端口物件
作為 UDP OutBound 使用 所以不會佔用系統監聽資源
- *self.transport* 自己的傳輸物件
---
- *datagram_received()* 先加碼成 Xbee 再呼叫 Serial 端口物件送出
### **[Class]** SerialHandler
處理 Serial 收發的端口 作為一個端口物件
- *self.transport* 自己的傳輸物件
---
- *data_received()* 先組合 Serial 封包 再解碼 再呼叫 UDP 端口物件送出
## mavlinkVehicleView.py
這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用
# 開發記錄
## 已實現功能
1. mavlink 分流解析
2. mavlink socket 建立
3. mavlink socket 轉拋 proxy
4. 建立 Serial 轉 UDP 連結 並管理
5. 建立 serial 連線
6. 各單元模組化
7. 終端機介面控制
8. 基礎載具流量觀測
9. 載具狀態收集與彙整
10. a. ros2 topic 應用開發介面
### 待開發功能
5-1. 建立 serial 連線 並可以對接收器下達AT指令
5-2. 模組化 serial 連線機制 以利後期擴容其他模組
10. a. ros2 應用開發介面

1597
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
View File

File diff suppressed because it is too large Load Diff

830
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
View File

File diff suppressed because it is too large Load Diff

6
src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py
Unescape Escape View File

@ -9,6 +9,8 @@
publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic>
'''
import os
# ROS2 的 import
import std_msgs.msg
import sensor_msgs.msg
@ -18,9 +20,9 @@ import mavros_msgs.msg
import math
# 自定義的 import
from .theLogger import setup_logger
from .utils import setup_logger
logger = setup_logger("mavlinkPublish.py")
logger = setup_logger(os.path.basename(__file__))
class mavlink_publisher():

902
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
Unescape Escape View File

@ -0,0 +1,902 @@
"""
MAVLink ROS2 Nodes
包含兩個獨立的 ROS2 Node：
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
"""
import os
import time
import math
import threading
from typing import Dict, Optional
# ROS2 imports
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
# ROS2 Message imports
import std_msgs.msg
import sensor_msgs.msg
import geometry_msgs.msg
import mavros_msgs.msg
# 自定義 imports
from . import mavlinkVehicleView as mvv
from .utils import setup_logger
logger = setup_logger(os.path.basename(__file__))
# ============================================================================
# 頻率控制器
# ============================================================================
class PublishRateController:
"""發布頻率控制器 - 按時間間隔控制發布頻率"""
def __init__(self):
# ═══════════════════════════════════════════════════════════════
# 【新增 Topic 位置 1/4】
# 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔
# 例如：'ekf_status': 1.0, # EKF 狀態
# ═══════════════════════════════════════════════════════════════
# 各 topic 的發布間隔（秒）
self.topic_intervals = {
'position': 0.5, # GPS位置
'attitude': 0.5, # 姿態
'velocity': 0.5, # 速度
'battery': 1.0, # 電池
'vfr_hud': 0.5, # VFR HUD
'mode': 1.0, # 飛行模式
'summary': 1.0, # 載具摘要
# 在這裡新增更多 topics...
}
# 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
self.last_publish_time: Dict[tuple, float] = {}
def should_publish(self, sysid: int, topic: str) -> bool:
"""
檢查是否應該發布此 topic
Args:
sysid: 系統ID
topic: topic 名稱
Returns:
bool: True 表示應該發布
"""
key = (sysid, topic)
now = time.time()
# 當間隔設定為0或負數時 關閉該 topic 的發布
interval = self.topic_intervals.get(topic, 0)
if interval <= 0:
return False
# 首次發布
if key not in self.last_publish_time:
self.last_publish_time[key] = now
return True
# 檢查時間間隔
if now - self.last_publish_time[key] >= interval:
self.last_publish_time[key] = now
return True
return False
def reset(self):
"""重置所有計時器"""
self.last_publish_time.clear()
# ============================================================================
# VehicleStatusPublisher Node
# ============================================================================
class VehicleStatusPublisher(Node):
"""
載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
職責:
- 定期從 vehicle_registry 讀取載具狀態
- 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
- 發布標準 ROS2 消息類型
- 檢測訂閱者，按需發布
"""
topicString_prefix = f'/fc_network/vehicle'
def __init__(self):
super().__init__('vehicle_status_publisher')
# 頻率控制器
self.rate_controller = PublishRateController()
# fc_publishers 字典 {(sysid, topic_name): publisher}
self.fc_publishers: Dict[tuple, any] = {}
# 定時器：以較高頻率檢查 vehicle_registry 並發布
# 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
self.timer_period = 0.1 # 100ms
self.timer = self.create_timer(self.timer_period, self.timer_callback)
# 狀態標誌
self.running = True
# logger.info("VehicleStatusPublisher initialized")
def timer_callback(self):
"""定時器回調 - 檢查所有載具並發布狀態"""
if not self.running:
return
# 從 vehicle_registry 獲取所有載具
all_vehicles = mvv.vehicle_registry.get_all()
for sysid, vehicle in all_vehicles.items():
self._publish_vehicle_status(vehicle)
def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
"""
發布單個載具的所有狀態
Args:
vehicle: VehicleView 實例
"""
sysid = vehicle.sysid
# 假設只有一個 autopilot component (component_id=1)
component = vehicle.get_component(1)
if not component:
return
status = component.status
# ═══════════════════════════════════════════════════════════════
# 【新增 Topic 位置 2/4】
# 若要新增 topic，請在此處調用對應的發布方法
# 例如：self._publish_ekf_status(sysid, status)
# ═══════════════════════════════════════════════════════════════
# 發布各種狀態（通過頻率控制器判斷是否發布）
self._publish_position(sysid, status)
self._publish_attitude(sysid, status)
self._publish_velocity(sysid, status)
self._publish_battery(sysid, status)
self._publish_vfr_hud(sysid, status)
self._publish_mode(sysid, status)
self._publish_summary(vehicle)
# 在這裡新增更多 publish 方法調用...
def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1):
"""
獲取或創建 publisher
Args:
sysid: 系統ID
topic: topic 名稱
msg_type: ROS2 消息類型
qos: QoS 設置
Returns:
publisher 對象
"""
key = (sysid, topic)
if key not in self.fc_publishers:
topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
publisher = self.create_publisher(msg_type, topic_name, qos)
self.fc_publishers[key] = publisher
logger.info(f"Created publisher: {topic_name}")
return self.fc_publishers[key]
def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
"""發布 GPS 位置"""
if not self.rate_controller.should_publish(sysid, 'position'):
return
pos = status.position
if pos.latitude is None or pos.longitude is None:
return
publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix)
# 檢查是否有訂閱者
if publisher.get_subscription_count() == 0:
return
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.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus
publisher.publish(msg)
def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
"""發布姿態（IMU）"""
if not self.rate_controller.should_publish(sysid, 'attitude'):
return
att = status.attitude
if att.roll is None:
return
publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
if publisher.get_subscription_count() == 0:
return
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)
def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus):
"""發布速度"""
if not self.rate_controller.should_publish(sysid, 'velocity'):
return
vfr = status.vfr
if vfr.groundspeed is None:
return
publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped)
if publisher.get_subscription_count() == 0:
return
msg = geometry_msgs.msg.TwistStamped()
msg.header.stamp = self.get_clock().now().to_msg()
# 使用 VFR HUD 的地速和航向計算速度分量
if vfr.heading is not None:
heading_rad = math.radians(vfr.heading)
msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad)
msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad)
# 爬升率作為 z 軸速度
if vfr.climb is not None:
msg.twist.linear.z = vfr.climb
publisher.publish(msg)
def _publish_battery(self, sysid: int, status: mvv.ComponentStatus):
"""發布電池狀態"""
if not self.rate_controller.should_publish(sysid, 'battery'):
return
bat = status.battery
if bat.voltage is None:
return
publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState)
if publisher.get_subscription_count() == 0:
return
msg = sensor_msgs.msg.BatteryState()
msg.voltage = bat.voltage
if bat.current is not None:
msg.current = bat.current
if bat.remaining is not None:
msg.percentage = bat.remaining / 100.0
if bat.temperature is not None:
msg.temperature = bat.temperature
publisher.publish(msg)
def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus):
"""發布 VFR HUD"""
if not self.rate_controller.should_publish(sysid, 'vfr_hud'):
return
vfr = status.vfr
if vfr.airspeed is None:
return
publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud)
if publisher.get_subscription_count() == 0:
return
msg = mavros_msgs.msg.VfrHud()
msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0
msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0
msg.heading = vfr.heading if vfr.heading is not None else 0
msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0
msg.climb = vfr.climb if vfr.climb is not None else 0.0
# altitude 需要從 position 獲取
if status.position.altitude is not None:
msg.altitude = status.position.altitude
publisher.publish(msg)
def _publish_mode(self, sysid: int, status: mvv.ComponentStatus):
"""發布飛行模式"""
if not self.rate_controller.should_publish(sysid, 'mode'):
return
mode = status.mode
if mode.mode_name is None:
return
publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String)
if publisher.get_subscription_count() == 0:
return
msg = std_msgs.msg.String()
msg.data = mode.mode_name
publisher.publish(msg)
def _publish_summary(self, vehicle: mvv.VehicleView):
"""
發布載具摘要資訊（自定義格式，使用 String 暫時代替）
TODO: 未來可以定義專門的 VehicleSummary.msg
"""
sysid = vehicle.sysid
if not self.rate_controller.should_publish(sysid, 'summary'):
return
publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String)
if publisher.get_subscription_count() == 0:
return
# 獲取 autopilot component
component = vehicle.get_component(1)
if not component:
return
status = component.status
# 構建摘要資訊（JSON 格式字串）
import json
summary = {
'sysid': sysid,
'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0,
'autopilot': component.mav_autopilot if component.mav_autopilot else 0,
'socket_id': vehicle.custom_meta.get('socket_id', -1), # 重要！
'armed': status.armed if status.armed is not None else False,
# 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
# 'latitude': status.position.latitude if status.position.latitude else 0.0,
# '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,
'connection_type': vehicle.connected_via.value,
'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
}
msg = std_msgs.msg.String()
msg.data = json.dumps(summary)
publisher.publish(msg)
# ═══════════════════════════════════════════════════════════════
# 【新增 Topic 位置 3/4】
# 若要新增 topic，請在此處實作對應的發布方法
# 方法命名規則：def _publish_<topic_name>(self, sysid: int, status: mvv.ComponentStatus):
# 例如：
# def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus):
# """發布 EKF 狀態"""
# if not self.rate_controller.should_publish(sysid, 'ekf_status'):
# return
#
# ekf = status.ekf
# if ekf.flags is None:
# return
#
# publisher = self._get_or_create_publisher(sysid, 'ekf_status', ...
# # ... 實作發布邏輯
# ═══════════════════════════════════════════════════════════════
@staticmethod
def _euler_to_quaternion(roll, pitch, yaw):
"""
歐拉角轉四元數
Args:
roll: 橫滾角 (弧度)
pitch: 俯仰角 (弧度)
yaw: 偏航角 (弧度)
Returns:
tuple: (qx, qy, qz, qw)
"""
qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
return (qx, qy, qz, qw)
def stop(self):
"""停止發布"""
self.running = False
# logger.info("VehicleStatusPublisher stopped")
# ============================================================================
# MavlinkCommandService Node
# ============================================================================
class MavlinkCommandService(Node):
"""
MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令
職責:
- 作為 service server，等待 client 請求
- 接收請求，組裝 MAVLink 封包
- 調用 mavlinkObject 發送封包
- 處理 ACK 等待和超時（未來實現）
設計理念：回歸 MAVLink 純粹結構
- 只負責將 ROS2 請求轉換為 MAVLink 封包
- 不預設功能（如 ARM/DISARM），保持模組化
- 高層應用可透過此 service 實現各種功能
"""
def __init__(self):
super().__init__('mavlink_command_service')
# ═══════════════════════════════════════════════════════════════════
# ROS2 Service 架構說明：
#
# 1. Service 定義：由 .srv 檔案定義（Request + Response）
# - Request: client 發送的請求內容
# - Response: server 回傳的結果
#
# 2. Service Server 創建：
# self.create_service(srv_type, service_name, callback_function)
# - srv_type: service 的訊息類型（需要自定義或使用標準）
# - service_name: service 的名稱（client 用此名稱呼叫）
# - callback_function: 處理請求的回調函數
#
# 3. Callback 函數：
# def callback(self, request, response):
# # request: 包含 client 發送的數據
# # response: 需要填充並返回給 client
# return response
#
# 4. Service Client 使用方式（在其他程式中）：
# client = node.create_client(srv_type, service_name)
# request = srv_type.Request()
# future = client.call_async(request) # 異步調用
# # 或 response = client.call(request) # 同步調用
# ═══════════════════════════════════════════════════════════════════
# 由於 ROS2 自定義 service 需要 .srv 檔案編譯
# 這裡先使用標準 String service 作為簡化實現
# TODO: 未來可創建專門的 .srv 檔案
from std_srvs.srv import Trigger
from example_interfaces.srv import SetBool
# ═══════════════════════════════════════════════════════════════════
# Service 1: 發送 MAVLink Message（通用介面）
# 使用 Trigger 作為臨時實現，未來應使用自定義 service
# ═══════════════════════════════════════════════════════════════════
# TODO: 創建 SendMavlinkMessage.srv
# Request:
# uint8 target_sysid
# uint8 target_compid
# uint16 message_id
# string fields_json # JSON 格式的字段數據
# bool wait_response
# uint16 response_msgid
# float32 timeout
# Response:
# bool success
# string response_json
# string error_message
# 暫時使用簡化版本（僅示範架構）
self.srv_send_message = self.create_service(
Trigger,
'/mavlink/send_message',
self.handle_send_message
)
# ═══════════════════════════════════════════════════════════════════
# Service 2: 發送 COMMAND_LONG
# ═══════════════════════════════════════════════════════════════════
self.srv_command_long = self.create_service(
Trigger,
'/mavlink/send_command_long',
self.handle_command_long
)
# ═══════════════════════════════════════════════════════════════════
# Service 3: 參數請求
# ═══════════════════════════════════════════════════════════════════
self.srv_param_request = self.create_service(
Trigger,
'/mavlink/param_request',
self.handle_param_request
)
# 狀態標記
self.running = True
# mavlinkObject 的引用（將由外部設置）
self.mavlink_analyzer = None
logger.info("MavlinkCommandService initialized")
def set_mavlink_analyzer(self, mavlink_analyzer):
"""
設置 mavlink_analyzer 引用
Args:
mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例
"""
self.mavlink_analyzer = mavlink_analyzer
logger.info("MavlinkCommandService: mavlink_analyzer set")
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 發送 MAVLink Message
# ═══════════════════════════════════════════════════════════════════════
def handle_send_message(self, request, response):
"""
處理發送 MAVLink 訊息的請求
ROS2 Service Callback 說明：
- 此函數會在 client 調用 service 時被執行
- request: 包含 client 傳入的參數
- response: 需要填充結果並返回給 client
- 必須 return response
Args:
request: Trigger.Request (暫時使用，未來改為自定義)
response: Trigger.Response
Returns:
response: 填充後的回應
"""
logger.info("Received send_message request")
# 檢查 mavlink_analyzer 是否已設置
if self.mavlink_analyzer is None:
response.success = False
response.message = "Error: mavlink_analyzer not set"
logger.error(response.message)
return response
# TODO: 實際實現
# 1. 從 request 解析參數（target_sysid, message_id, fields 等）
# 2. 使用 pymavlink 組裝 MAVLink 封包
# 3. 調用 mavlink_analyzer.send_message() 發送
# 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應
# 暫時返回成功（示範用）
response.success = True
response.message = "Message sent (placeholder implementation)"
return response
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 發送 COMMAND_LONG
# ═══════════════════════════════════════════════════════════════════════
def handle_command_long(self, request, response):
"""
處理發送 COMMAND_LONG 的請求
COMMAND_LONG (MAVLink message ID=76):
- 用於發送簡單命令給載具
- 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等
Args:
request: Trigger.Request
response: Trigger.Response
Returns:
response: 填充後的回應
"""
logger.info("Received command_long request")
if self.mavlink_analyzer is None:
response.success = False
response.message = "Error: mavlink_analyzer not set"
return response
# TODO: 實際實現
# 1. 從 request 解析 COMMAND_LONG 參數
# - target_sysid, target_compid
# - command (MAV_CMD_xxx)
# - param1~param7
# 2. 組裝 COMMAND_LONG 封包
# 3. 發送並等待 COMMAND_ACK (message ID=77)
# 4. 解析 ACK 結果（ACCEPTED/FAILED 等）
response.success = True
response.message = "Command sent (placeholder implementation)"
return response
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 參數請求
# ═══════════════════════════════════════════════════════════════════════
def handle_param_request(self, request, response):
"""
處理參數讀取請求
MAVLink 參數協議：
- PARAM_REQUEST_READ (ID=20): 請求讀取參數
- PARAM_VALUE (ID=22): 參數值回應
- PARAM_SET (ID=23): 設置參數值
═══════════════════════════════════════════════════════════════════
【使用 mavlinkObject 回應機制的步驟】
1. 設置回應訊息類型：
self.mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE
2. 發送請求封包：
message_bytes = ... # 組裝 PARAM_REQUEST_READ
self.mavlink_analyzer.send_message(
message_bytes,
target_sysid=1
)
3. 監聽回應（在獨立線程或定時器中）：
from ..fc_network_adapter import mavlinkObject as mo
# 等待回應（帶超時）
timeout = 3.0
start_time = time.time()
while time.time() - start_time < timeout:
items = mo.return_packet_ring.get_all()
for socket_id, timestamp, msg in items:
if msg.get_type() == 'PARAM_VALUE':
# 找到回應！
param_id = msg.param_id
param_value = msg.param_value
# 處理回應...
return
time.sleep(0.01) # 短暫等待
# 超時處理
4. 清理（可選）：
self.mavlink_analyzer.set_return_message_types([]) # 清空
mo.return_packet_ring.clear() # 清空緩衝區
注意事項：
- return_packet_ring 是全域的，所有 mavlink_object 共用
- 需要通過 socket_id 或 sysid 來識別回應來源
- 實際使用時建議實現專門的回應管理器
═══════════════════════════════════════════════════════════════════
Args:
request: Trigger.Request
response: Trigger.Response
Returns:
response: 填充後的回應
"""
logger.info("Received param_request")
if self.mavlink_analyzer is None:
response.success = False
response.message = "Error: mavlink_analyzer not set"
return response
# TODO: 實際實現
# 1. 從 request 解析參數名稱或索引
# 2. 設置 mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE
# 3. 發送 PARAM_REQUEST_READ
# 4. 監聽 return_packet_ring，等待 PARAM_VALUE
# 5. 解析回應並填充到 response
response.success = True
response.message = "Param request sent (placeholder implementation)"
return response
# ═══════════════════════════════════════════════════════════════════════
# 【新增 Service 位置 4/4】
# 若要新增 service，請在此處添加新的 handler 方法
# 並在 __init__ 中創建對應的 service server
# ═══════════════════════════════════════════════════════════════════════
def stop(self):
"""停止服務"""
self.running = False
logger.info("MavlinkCommandService stopped")
# ============================================================================
# ROS2 節點管理器
# ============================================================================
class fc_ros_manager:
"""
MAVLink ROS2 節點管理器
管理兩個獨立的 ROS2 Node：
- VehicleStatusPublisher
- MavlinkCommandService
提供統一的啟動/停止介面給 mainOrchestrator
"""
def __init__(self):
self.initialized = False
self.running = False
# 两个 node 实例
self.status_publisher: Optional[VehicleStatusPublisher] = None
self.command_service: Optional[MavlinkCommandService] = None
# Executor & Thread
self.spin_thread: Optional[threading.Thread] = None
self.executor: Optional[MultiThreadedExecutor] = None
def initialize(self):
"""初始化 ROS2 环境和 nodes"""
if self.initialized:
logger.warning("fc_ros_manager already initialized")
return False
try:
# 初始化 ROS2
rclpy.init()
# 創建節點 node
self.status_publisher = VehicleStatusPublisher()
self.command_service = MavlinkCommandService()
# 創建執行者 MultiThreadedExecutor
self.executor = MultiThreadedExecutor()
self.executor.add_node(self.status_publisher)
self.executor.add_node(self.command_service)
self.initialized = True
# logger.info("fc_ros_manager initialized")
return True
except Exception as e:
logger.error(f"Failed to initialize fc_ros_manager: {e}")
return False
def start(self):
"""啟動 ROS2 nodes (在獨立執行緒中運行 executor) """
if not self.initialized:
logger.error("fc_ros_manager initialize failed or not called")
return False
if self.running:
logger.warning("fc_ros_manager already running")
return False
try:
self.running = True
self.spin_thread = threading.Thread(
target=self._spin_executor,
daemon=True,
name="ROS2ExecutorThread"
)
self.spin_thread.start()
logger.info("fc_ros_manager started <-")
return True
except Exception as e:
logger.error(f"Failed to start fc_ros_manager: {e}")
self.running = False
return False
def _spin_executor(self):
"""在 thread 中運行的 executor"""
try:
# logger.info("ROS2 executor spinning...")
while self.running:
self.executor.spin_once(timeout_sec=0.1)
except Exception as e:
logger.error(f"fc_ros_manager error in spinning executor: {e}")
def stop(self):
"""停止 ROS2 nodes"""
if not self.running:
logger.warning("fc_ros_manager not running")
return False
try:
# 標記停止
self.running = False
# 停止各個 node
if self.status_publisher:
self.status_publisher.stop()
if self.command_service:
self.command_service.stop()
# 等待 spin 執行緒結束
if self.spin_thread and self.spin_thread.is_alive():
self.spin_thread.join(timeout=2.0)
logger.info("fc_ros_manager thread END!")
return True
except Exception as e:
logger.error(f"Error stopping fc_ros_manager: {e}")
return False
def shutdown(self):
"""完全關閉並清理資源"""
if self.running:
self.stop()
if self.initialized:
try:
# 銷毀 nodes
if self.status_publisher:
self.status_publisher.destroy_node()
if self.command_service:
self.command_service.destroy_node()
# 關閉 ROS2
if rclpy.ok():
rclpy.shutdown()
self.initialized = False
logger.info("fc_ros_manager Node END!")
except Exception as e:
logger.error(f"Error during shutdown: {e}")
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,
}
# ============================================================================
# 全域實例
# ============================================================================
# 全域管理器實例（供 mainOrchestrator 使用）
ros2_manager = fc_ros_manager()
'''
================= 改版記錄 ============================
2026.01.20
1. 重構自 mavlinkPublish.py (該檔案將被棄用)
2. 提供 fc_ros_manager 統一管理介面
3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態
4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
5. 預留 MavlinkCommandService 結構（稍後實現）
'''

453
src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
Unescape Escape View File

@ -0,0 +1,453 @@
"""
VehicleView - Pure State Container
純粹的狀態容器，不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量
只提供狀態存取介面，由外部手動餵資料（push state）
"""
import os
from typing import Dict, Optional, Any
from dataclasses import dataclass, field
from enum import Enum
from .utils import setup_logger
logger = setup_logger(os.path.basename(__file__))
# ====================== Enums =====================
class ConnectionType(Enum):
"""連接類型"""
SERIAL = "serial"
UDP = "udp"
TCP = "tcp"
OTHER = "other"
class ComponentType(Enum):
"""組件類型"""
AUTOPILOT = "autopilot"
GCS = "gcs"
CAMERA = "camera"
GIMBAL = "gimbal"
OTHER = "other"
class RFModuleType(Enum):
"""RF模組類型"""
XBEE = "xbee"
UDP = "udp"
TCP = "tcp"
OTHER = "other"
# ====================== Data Classes =====================
@dataclass
class Position:
"""位置資訊"""
latitude: Optional[float] = None # 緯度 (度)
longitude: Optional[float] = None # 經度 (度)
altitude: Optional[float] = None # 海拔高度 (公尺)
relative_altitude: Optional[float] = None # 相對高度 (公尺)
timestamp: Optional[float] = None # 時間戳記
@dataclass
class Attitude:
"""姿態資訊"""
roll: Optional[float] = None # 橫滾角 (弧度)
pitch: Optional[float] = None # 俯仰角 (弧度)
yaw: Optional[float] = None # 偏航角 (弧度)
rollspeed: Optional[float] = None # 橫滾速度 (弧度/秒)
pitchspeed: Optional[float] = None # 俯仰速度 (弧度/秒)
yawspeed: Optional[float] = None # 偏航速度 (弧度/秒)
timestamp: Optional[float] = None # 時間戳記
@dataclass
class FlightMode:
"""飛行模式資訊"""
base_mode: Optional[int] = None # MAVLink base mode
custom_mode: Optional[int] = None # 自定義模式
mode_name: Optional[str] = None # 模式名稱 (例如: "GUIDED", "AUTO")
timestamp: Optional[float] = None # 時間戳記
@dataclass
class Battery:
"""電池資訊"""
voltage: Optional[float] = None # 電壓 (V)
current: Optional[float] = None # 電流 (A)
remaining: Optional[int] = None # 剩餘電量 (%)
temperature: Optional[float] = None # 溫度 (攝氏)
timestamp: Optional[float] = None # 時間戳記
@dataclass
class EKF:
"""EKF狀態資訊"""
flags: Optional[int] = None # EKF 旗標
velocity_variance: Optional[float] = None # 速度變異
pos_horiz_variance: Optional[float] = None # 水平位置變異
pos_vert_variance: Optional[float] = None # 垂直位置變異
compass_variance: Optional[float] = None # 羅盤變異
terrain_alt_variance: Optional[float] = None # 地形高度變異
timestamp: Optional[float] = None # 時間戳記
@dataclass
class GPS:
"""GPS資訊"""
fix_type: Optional[int] = None # GPS fix 類型 (0=無, 1=無fix, 2=2D, 3=3D, 4=DGPS, 5=RTK)
satellites_visible: Optional[int] = None # 可見衛星數
eph: Optional[int] = None # GPS HDOP 水平精度因子
epv: Optional[int] = None # GPS VDOP 垂直精度因子
timestamp: Optional[float] = None # 時間戳記
@dataclass
class VFR:
"""VFR HUD資訊"""
airspeed: Optional[float] = None # 空速 (m/s)
groundspeed: Optional[float] = None # 地速 (m/s)
heading: Optional[int] = None # 航向 (度)
throttle: Optional[int] = None # 油門 (%)
climb: Optional[float] = None # 爬升率 (m/s)
timestamp: Optional[float] = None # 時間戳記
@dataclass
class ComponentStatus:
"""組件狀態容器"""
position: Position = field(default_factory=Position)
attitude: Attitude = field(default_factory=Attitude)
mode: FlightMode = field(default_factory=FlightMode)
battery: Battery = field(default_factory=Battery)
ekf: EKF = field(default_factory=EKF)
gps: GPS = field(default_factory=GPS)
vfr: VFR = field(default_factory=VFR)
# 系統狀態
system_status: Optional[int] = None # MAV_STATE
armed: Optional[bool] = None # 解鎖狀態
# 其他自定義狀態
custom_status: Dict[str, Any] = field(default_factory=dict)
@dataclass
class PacketStats:
"""封包統計資訊"""
received_count: int = 0 # 接收封包數
lost_count: int = 0 # 遺失封包數
last_seq: Optional[int] = None # 最後序號
last_msg_time: Optional[float] = None # 最後訊息時間
msg_type_count: Dict[int, int] = field(default_factory=dict) # 各類訊息計數 {msg_type: count}
@dataclass
class RFStatus:
"""RF模組狀態"""
rssi: Optional[int] = None # 信號強度
noise: Optional[int] = None # 噪音水平
at_response: Optional[str] = None # AT 命令回應
link_quality: Optional[int] = None # 連接品質
timestamp: Optional[float] = None # 時間戳記
custom_status: Dict[str, Any] = field(default_factory=dict) # 其他自定義狀態
@dataclass
class SocketInfo:
"""Socket連接資訊"""
ip: Optional[str] = None # IP位址
port: Optional[int] = None # 埠號
local_ip: Optional[str] = None # 本地IP
local_port: Optional[int] = None # 本地埠號
connected: bool = False # 連接狀態
# ====================== Component Class =====================
class VehicleComponent:
"""載具組件 - 代表一個 MAVLink component"""
def __init__(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER):
self.component_id = component_id
self.type = comp_type
# MAVLink 組件資訊
self.mav_type: Optional[int] = None # MAV_TYPE
self.mav_autopilot: Optional[int] = None # MAV_AUTOPILOT
# 狀態容器
self.status = ComponentStatus()
# 參數容器 (只存放，不主動下載)
self.parameters: Dict[str, Any] = {}
# 封包統計
self.packet_stats = PacketStats()
def update_packet_stats(self, seq: int, msg_type: int, timestamp: float) -> None:
"""
更新封包統計
Args:
seq: 訊息序號
msg_type: 訊息類型
timestamp: 時間戳記
"""
stats = self.packet_stats
# 檢查遺失封包
if stats.last_seq is not None:
expected_seq = (stats.last_seq + 1) % 256
diff = seq - expected_seq
if diff < 0:
diff += 256
stats.lost_count += diff
# 更新統計資訊
stats.received_count += 1
stats.last_seq = seq
stats.last_msg_time = timestamp
# 更新訊息類型計數
if msg_type in stats.msg_type_count:
stats.msg_type_count[msg_type] += 1
else:
stats.msg_type_count[msg_type] = 1
def reset_packet_stats(self) -> None:
"""重置封包統計"""
self.packet_stats = PacketStats()
def set_parameter(self, param_name: str, param_value: Any) -> None:
"""設定參數 (手動餵入)"""
self.parameters[param_name] = param_value
def get_parameter(self, param_name: str, default: Any = None) -> Any:
"""取得參數"""
return self.parameters.get(param_name, default)
def __str__(self) -> str:
return (f"Component(id={self.component_id}, type={self.type.value}, "
f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, "
f"lost={self.packet_stats.lost_count})")
# ====================== RF Module Class =====================
class RFModule:
"""RF模組資訊容器"""
def __init__(self, rf_type: RFModuleType = RFModuleType.OTHER):
self.type = rf_type
self.status = RFStatus()
self.socket_info = SocketInfo()
def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
"""更新RSSI"""
self.status.rssi = rssi
if timestamp:
self.status.timestamp = timestamp
def update_at_response(self, response: str, timestamp: Optional[float] = None) -> None:
"""更新AT命令回應"""
self.status.at_response = response
if timestamp:
self.status.timestamp = timestamp
def update_socket_info(self, ip: str = None, port: int = None,
local_ip: str = None, local_port: int = None,
connected: bool = None) -> None:
"""更新Socket資訊"""
if ip is not None:
self.socket_info.ip = ip
if port is not None:
self.socket_info.port = port
if local_ip is not None:
self.socket_info.local_ip = local_ip
if local_port is not None:
self.socket_info.local_port = local_port
if connected is not None:
self.socket_info.connected = connected
def __str__(self) -> str:
return (f"RFModule(type={self.type.value}, rssi={self.status.rssi}, "
f"connected={self.socket_info.connected})")
# ====================== Main VehicleView Class =====================
class VehicleView:
"""
載具視圖 - 純狀態容器
特點:
1. 只有狀態容器，沒有任何主動通訊功能
2. 不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量
3. 可以手動餵資料 (push state)
4. 可擴充 (支援 RF 模組狀態)
"""
# TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了
def __init__(self, sysid: int):
# Meta 資訊
self.sysid = sysid
self.kind: Optional[str] = None # 載具種類描述 (例如: "Copter", "Plane")
self.vehicle_type: Optional[int] = None # MAV_TYPE
self.connected_via: ConnectionType = ConnectionType.OTHER
# 組件容器 {component_id: VehicleComponent}
self.components: Dict[int, VehicleComponent] = {}
# RF模組
self.rf_module: Optional[RFModule] = None
# 其他自定義meta資訊
self.custom_meta: Dict[str, Any] = {}
def add_component(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER) -> VehicleComponent:
"""
新增組件
Args:
component_id: 組件ID
comp_type: 組件類型
Returns:
VehicleComponent: 新增的組件
"""
if component_id not in self.components:
self.components[component_id] = VehicleComponent(component_id, comp_type)
# logger.debug(f"Added component {component_id} to system {self.sysid}")
return self.components[component_id]
def get_component(self, component_id: int) -> Optional[VehicleComponent]:
"""取得組件"""
return self.components.get(component_id)
def remove_component(self, component_id: int) -> bool:
"""移除組件"""
if component_id in self.components:
del self.components[component_id]
# logger.debug(f"Removed component {component_id} from system {self.sysid}")
return True
return False
def reset_component_stats(self, component_id: int) -> None:
"""重置指定組件的封包統計"""
component = self.get_component(component_id)
if component:
component.reset_packet_stats()
# logger.info(f"Reset packet stats for component {component_id} in system {self.sysid}")
def set_rf_module(self, rf_type: RFModuleType) -> RFModule:
"""設定RF模組"""
self.rf_module = RFModule(rf_type)
return self.rf_module
def get_rf_module(self) -> Optional[RFModule]:
"""取得RF模組"""
return self.rf_module
def __str__(self) -> str:
comp_list = ", ".join([str(cid) for cid in self.components.keys()])
return (f"VehicleView(sysid={self.sysid}, kind={self.kind}, "
f"connected_via={self.connected_via.value}, "
f"components=[{comp_list}], "
f"rf_module={self.rf_module is not None})")
def to_dict(self) -> Dict[str, Any]:
"""轉換為字典格式 (方便序列化/除錯)"""
return {
'meta': {
'sysid': self.sysid,
'kind': self.kind,
'vehicle_type': self.vehicle_type,
'connected_via': self.connected_via.value,
'custom_meta': self.custom_meta
},
'components': {
cid: {
'component_id': comp.component_id,
'type': comp.type.value,
'mav_type': comp.mav_type,
'mav_autopilot': comp.mav_autopilot,
'packet_stats': {
'received': comp.packet_stats.received_count,
'lost': comp.packet_stats.lost_count,
'last_seq': comp.packet_stats.last_seq,
'last_msg_time': comp.packet_stats.last_msg_time
},
'parameters_count': len(comp.parameters)
}
for cid, comp in self.components.items()
},
'rf_module': {
'type': self.rf_module.type.value,
'rssi': self.rf_module.status.rssi,
'socket_connected': self.rf_module.socket_info.connected
} if self.rf_module else None
}
# ====================== Registry =====================
class VehicleViewRegistry:
"""載具視圖註冊表 - 管理所有的 VehicleView"""
def __init__(self):
self._vehicles: Dict[int, VehicleView] = {}
def register(self, sysid: int) -> VehicleView:
"""註冊新的載具視圖"""
if sysid not in self._vehicles:
self._vehicles[sysid] = VehicleView(sysid)
logger.info(f"Registered new VehicleView for system {sysid}")
return self._vehicles[sysid]
def get(self, sysid: int) -> Optional[VehicleView]:
"""取得載具視圖"""
return self._vehicles.get(sysid)
def unregister(self, sysid: int) -> bool:
"""註銷載具視圖"""
if sysid in self._vehicles:
del self._vehicles[sysid]
logger.info(f"Unregistered VehicleView for system {sysid}")
return True
return False
def get_all(self) -> Dict[int, VehicleView]:
"""取得所有載具視圖"""
return self._vehicles.copy()
def clear(self) -> None:
"""清空所有載具視圖"""
self._vehicles.clear()
logger.info("Cleared all VehicleViews")
def __len__(self) -> int:
return len(self._vehicles)
def __contains__(self, sysid: int) -> bool:
return sysid in self._vehicles
# ====================== Global Instance =====================
# 全域註冊表實例
vehicle_registry = VehicleViewRegistry()
'''
================= 改版記錄 ============================
2026.01.16
1. 新增 重置指定組件的封包統計 功能
'''

611
src/fc_network_adapter/fc_network_adapter/serialManager.py
Unescape Escape View File

@ -0,0 +1,611 @@
'''
'''
# 基礎功能的 import
import asyncio
import serial_asyncio
import os
import sys
import serial
import signal
import time
import threading
import struct
from enum import Enum, auto
# # XBee 模組
# from xbee.frame import APIFrame
# 自定義的 import
from .utils import setup_logger
# ====================== 分割線 =====================
logger = setup_logger(os.path.basename(__file__))
# ====================== 分割線 =====================
class XBeeFrameHandler:
"""XBee API Frame 處理器"""
@staticmethod
def parse_at_command_response(frame: bytes) -> dict:
"""解析 AT Command Response (0x88)"""
if len(frame) < 8:
return None
frame_type = frame[3]
if frame_type != 0x88:
return None
frame_id = frame[4]
at_command = frame[5:7]
status = frame[7]
data = frame[8:] if len(frame) > 8 else b''
return {
'frame_id': frame_id,
'command': at_command,
'status': status,
'data': data,
'is_ok': status == 0x00
}
@staticmethod
def parse_receive_packet(frame: bytes) -> dict:
# """解析 RX Packet (0x90) - 未來擴展用"""
# if len(frame) < 15 or frame[3] != 0x90:
# return None
# return {
# 'source_addr': frame[4:12],
# 'reserved': frame[12:14],
# 'options': frame[14],
# 'data': frame[15:-1]
# }
pass
return None
@staticmethod
def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes:
"""
將數據封裝為 XBee API 傳輸幀
使用 XBee API 格式封裝數據:
- 傳輸請求幀 (0x10)
- 使用廣播地址
- 添加適當的頭部和校驗和
"""
frame_type = 0x10
dest_addr16 = b'\xFF\xFE'
broadcast_radius = 0x00
options = 0x00
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF)
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
@staticmethod
def decapsulate_data(data: bytes):
# 這裡可以根據需要進行數據解封裝
# XBee API 幀格式:
# 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節)
# 檢查幀起始符 (0x7E)
if not data or len(data) < 5 or data[0] != 0x7E:
return data
# 獲取數據長度 (不包括校驗和)
# length = (data[1] << 8) + data[2]
length = (data[1] << 8) | data[2]
# 檢查幀完整性
if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和
return data
# 提取API標識符和數據
frame_type = data[3]
# frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中
# 根據不同的幀類型進行處理
if frame_type == 0x90: # 例如，這是"接收數據包"類型
rf_data_start = 3 + 12
return data[rf_data_start:3 + length]
else:
return None
return data
class ATCommandHandler:
"""AT 指令回應處理器"""
def __init__(self, serial_port: str):
self.serial_port = serial_port
self.handlers = {
b'DB': self._handle_rssi,
b'SH': self._handle_serial_high,
b'SL': self._handle_serial_low,
# 可擴展其他 AT 指令
}
def handle_response(self, response: dict):
"""根據 AT 指令類型分派處理"""
if not response or not response['is_ok']:
if response:
print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
return
command = response['command']
handler = self.handlers.get(command)
if handler:
handler(response['data'])
else:
print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
def _handle_rssi(self, data: bytes):
"""處理 DB (RSSI) 回應"""
if not data:
return
rssi_value = data[0]
now = time.time()
# 檢查是否最近有收到 MAVLink
last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0)
if now - last_mavlink_time > 0.5:
print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
return
# 取得對應的 sysid
sysid = serial_to_sysid.get(self.serial_port)
if sysid is None:
print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
return
# 記錄 RSSI
rssi_history[sysid].append(-rssi_value)
time_history[sysid].append(now)
# print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
def _handle_serial_high(self, data: bytes):
# """處理 SH (Serial Number High) - 範例"""
# if len(data) >= 4:
# serial_high = int.from_bytes(data[:4], 'big')
# print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}")
pass
def _handle_serial_low(self, data: bytes):
# """處理 SL (Serial Number Low) - 範例"""
# if len(data) >= 4:
# serial_low = int.from_bytes(data[:4], 'big')
# print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}")
pass
# ====================== 分割線 =====================
class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發
def __init__(self, udp_handler, serial_port_str):
self.udp_handler = udp_handler # UDP 的傳輸物件
self.serial_port_str = serial_port_str
self.at_handler = ATCommandHandler(serial_port_str)
self.buffer = bytearray() # 用於緩存接收到的資料
self.transport = None # Serial 自己的傳輸物件
# self.first_data = True # 標記是否為第一次收到資料
# self.has_processed = False # 測試模式用 處理數據旗標 # debug
def connection_made(self, transport):
self.transport = transport
if hasattr(self.udp_handler, 'set_serial_handler'):
self.udp_handler.set_serial_handler(self)
# logger.info(f"Serial port {self.serial_port_str} connected.") # debug
# Serial 收到資料的處理過程
def data_received(self, data):
# 1. 把收到的資料加入緩衝區
self.buffer.extend(data)
# 2. 需要完整的 header 才能解析
while len(self.buffer) >= 3:
# 3. 瞄準 XBee API Frame (0x7E 開頭的封包)
if self.buffer[0] != 0x7E:
self.buffer.pop(0) # 如果不是就丟掉
continue
# 4. 讀取 payload 長度
length = (self.buffer[1] << 8) | self.buffer[2]
full_length = 3 + length + 1
# 5. 等待完整封包
if len(self.buffer) < full_length:
break
# 6. 提取完整 frame 並從緩衝區移除
an_frame = self.buffer[:full_length]
del self.buffer[:full_length]
# 7. 判斷 frame 類型
frame_type = an_frame[3]
if frame_type == 0x88:
# 處理 AT Command 回應
# response = XBeeFrameHandler.parse_at_command_response(an_frame)
# self.at_handler.handle_response(response)
pass
elif frame_type == 0x90:
# Receive Packet (RX) payload 先解碼
processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame))
# 轉換失敗就捨棄了
if processed_data is None:
continue
# 再透過 UDP 送出
self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port))
elif frame_type == 0x8B:
pass
else:
# 其他類型的 frame 未來可擴展處理 現在忽略
logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}")
# # RSSI
# if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包
# # frame[5:7] == b'DB' -> API 封包的DB參數
# status = frame[7] #
# if status == 0x00 and len(frame) > 8: # status == 0x00 -> 這個封包是有效封包
# rssi_value = frame[8]
# now = time.time()
# # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
# 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)
# # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
# else:
# print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
# else:
# print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
# else:
# print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發
LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端IP
def __init__(self, target_port):
self.target_port = target_port # 目標 UDP 端口
self.serial_handler = None # Serial 的傳輸物件
self.transport = None # UDP 自己的傳輸物件
self.remote_addr = None # 儲存動態獲取的遠程地址 # debug
# self.has_processed = False # 測試模式用 處理數據旗標 # debug
def connection_made(self, transport):
self.transport = transport
# logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug
def set_serial_handler(self, serial_handler):
self.serial_handler = serial_handler
# UDP 收到資料的處理過程
def datagram_received(self, data, addr):
# 儲存對方的地址（這樣就能向同一個來源回傳數據）
# self.remote_addr = addr # debug
# print(f"Received UDP data from {addr}, setting as remote address")
processed_data = XBeeFrameHandler.encapsulate_data(data)
if self.serial_handler:
self.serial_handler.transport.write(processed_data)
#==================================================================
class SerialReceiverType(Enum):
"""連接類型"""
TELEMETRY = auto()
XBEEAPI2AT = auto()
OTHER = auto()
class serial_manager:
class serial_object:
def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc
self.baudrate = baudrate
self.receiver_type = receiver_type
self.target_port = target_port # 指向的 UPD 端口
self.transport = None # TODO 這個變數可能沒有作用
self.protocol = None # TODO 這個變數可能沒有作用
self.udp_handler = None
self.serial_handler = None
def __init__(self):
self.thread = None
self.loop = None
self.running = False
self.serial_count = 0
self.serial_objects = {} # serial id num : serial_object
def __del__(self):
self.loop = None
self.thread = None
def start(self):
if self.running:
logger.warning("serial_manager already running")
return
self.running = True
# 啟動獨立線程 命名為 SerialManager
self.thread = threading.Thread(
target=self._run_event_loop,
name="SerialManager"
)
self.thread.daemon = False # 不設為 daemon，確保正確關閉
self.thread.start()
# 等待 _run_event_loop 建立事件循環的物件 self.loop
start_timeout = 2.0
start_time = time.time()
while not self.loop and time.time() - start_time < start_timeout:
time.sleep(0.1)
# 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop
if self.loop:
logger.info("serial_manager thread started <-")
return True
else:
logger.error("serial_manager failed to start")
return False
def shutdown(self):
"""停止 serial_manager 和其管理的所有 serial_object"""
# 自己在 running 狀態下才執行停止程序
if not self.running:
logger.warning("serial_manager is not running")
return
# 停止所有被管理的 serial_object
for serial_id in list(self.serial_objects.keys()):
self.remove_serial_link(serial_id)
# 停止自己
self.running = False
# 解開事件循環的阻塞
if self.loop:
self.loop.call_soon_threadsafe(self.loop.stop)
# 等待線程結束
if self.thread and self.thread.is_alive():
self.thread.join(timeout=5.0)
if self.thread.is_alive():
logger.warning("serial_manager thread did not stop gracefully")
logger.info("serial_manager thread END!")
def _run_event_loop(self):
"""在獨立線程中運行 asyncio 事件循環"""
self.loop = asyncio.new_event_loop()
asyncio.set_event_loop(self.loop)
# # 為每個 serial_object 建立連接
# for serial_obj in self.serial_objects:
# coro = serial_asyncio.create_serial_connection(
# self.loop,
# lambda: SerialProtocol(serial_obj.receiver_type),
# serial_obj.serial_port,
# baudrate=serial_obj.baudrate
# )
# transport, protocol = self.loop.run_until_complete(coro)
# serial_obj.transport = transport
# serial_obj.protocol = protocol
try:
self.loop.run_forever()
finally:
self.loop.close()
def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
if not self.running or not self.loop:
logger.error("Event loop not running, cannot create serial link")
return False
# 檢查 serial port 有效
if not self.check_serial_port(serial_port, baudrate):
logger.error(f"Serial port {serial_port} validation failed")
return False
# 使用 run_coroutine_threadsafe 執行協程並獲取結果
future = asyncio.run_coroutine_threadsafe(
self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type),
self.loop
)
try:
# 等待結果，設定合理的超時時間
result = future.result(timeout=5.0)
if result:
logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}")
return True
except asyncio.TimeoutError:
logger.error(f"Timeout creating serial link for {serial_port}")
return False
except Exception as e:
logger.error(f"Failed to create serial link for {serial_port}: {e}")
return False
async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
"""在事件循環線程中執行實際的連接創建"""
try:
# 創建 serial_object 實例
serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type)
# 建立 UDP 處理器並指定目標端口位置
serial_obj.udp_handler = UDPHandler(target_port)
# 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配
udp_transport, udp_protocol = await self.loop.create_datagram_endpoint(
lambda: serial_obj.udp_handler,
local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口
)
serial_obj.transport = udp_transport
serial_obj.protocol = udp_protocol
# logger.info(f"UDP endpoint created for {serial_port}") # debug
# 建立 Serial 處理器，將 UDP 處理器傳給它
serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port)
# 建立 Serial 連接
serial_transport, _ = await serial_asyncio.create_serial_connection(
self.loop,
lambda: serial_obj.serial_handler,
serial_port,
baudrate=baudrate
)
# logger.info(f"Serial connection created for {serial_port}") # debug
# 將 serial_object 加入管理列表
serial_id = self.serial_count + 1
self.serial_objects[serial_id] = serial_obj
self.serial_count += 1
# logger.info(f"Serial object {serial_id} added to manager") # debug
return True
except Exception as e:
logger.error(f"Exception in _async_create_serial_link for {serial_port}: {str(e)}")
# 清理已創建的資源
if 'serial_obj' in locals():
if hasattr(serial_obj, 'transport') and serial_obj.transport:
serial_obj.transport.close()
return False
def remove_serial_link(self, serial_id):
"""移除串口連接（線程安全方式）"""
# 確保事件循環正在運行
if not self.loop:
logger.error("Event loop not running")
return False
# 檢查 serial_id 是否存在
if serial_id not in self.serial_objects:
logger.warning(f"Serial object {serial_id} not found")
return False
# 使用 run_coroutine_threadsafe 執行協程
future = asyncio.run_coroutine_threadsafe(
self._async_remove_serial_link(serial_id),
self.loop
)
try:
result = future.result(timeout=3.0)
if result:
logger.info(f"Remove Serial Link {serial_id}")
return result
except asyncio.TimeoutError:
logger.error(f"Timeout removing serial link {serial_id}")
return False
except Exception as e:
logger.error(f"Failed to remove serial link {serial_id}: {e}")
return False
async def _async_remove_serial_link(self, serial_id):
"""在事件循環線程中執行實際的連接移除"""
if serial_id not in self.serial_objects:
logger.warning(f"Serial object {serial_id} not in managed list")
return False
try:
serial_obj = self.serial_objects[serial_id]
# 關閉 UDP transport
if hasattr(serial_obj, 'transport') and serial_obj.transport:
serial_obj.transport.close()
# 關閉 Serial transport
if hasattr(serial_obj, 'serial_handler') and serial_obj.serial_handler:
if hasattr(serial_obj.serial_handler, 'transport') and serial_obj.serial_handler.transport:
serial_obj.serial_handler.transport.close()
# 從管理列表中移除
del self.serial_objects[serial_id]
# logger.info(f"Serial object {serial_id} removed from manager") # debug
return True
except Exception as e:
logger.error(f"Exception in _async_remove_serial_link for {serial_id}: {str(e)}")
return False
def get_serial_link(self):
ret = {} # serial id num : serial_port string
for key, obj in self.serial_objects.items():
ret[key] = obj.serial_port
return ret
@staticmethod
def check_serial_port(serial_port, baudrate):
"""檢查串口是否存在與可用"""
# 檢查設備是否存在
if not os.path.exists(serial_port):
logger.error(f"Serial Device {serial_port} Not Found")
return False
# 檢查是否有權限訪問設備
try:
if not os.access(serial_port, os.R_OK | os.W_OK):
logger.error(f"No permission to access {serial_port}")
return False
except Exception as e:
logger.error(f"Cannot Access Serial Device {serial_port}: {str(e)}")
return False
# 檢查是否被占用
try:
# 嘗試打開串口
ser = serial.Serial(serial_port, baudrate)
ser.close() # 打開成功後立即關閉
return True
except serial.SerialException as e:
logger.error(f"Serial Device {serial_port} is Occupied or Inaccessible: {str(e)}")
return False
except Exception as e:
logger.error(f"Unknown Error: {str(e)}")
return False
if __name__ == '__main__':
sm = serial_manager()
sm.start()
SERIAL_PORT = '/dev/ttyUSB0' # 手動指定
SERIAL_BAUDRATE = 115200
UDP_REMOTE_PORT = 14571
sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT)
linked_serial = sm.get_serial_link()
print(linked_serial)
time.sleep(10)
sm.remove_serial_link(1)
time.sleep(3)
sm.shutdown()

14
src/fc_network_adapter/fc_network_adapter/socketManager.py
Unescape Escape View File

@ -1,14 +0,0 @@
'''
透過某個地方 得到 udp 或 uart 接口
對於每個接口 視為一個獨立的物件
物件對於不同的接口 是為不同類型的物件
每個類型的物件 創建一個獨立的執行緒 來處理資料
關於執行緒的實作 是寫在另一個模組
物件之間 也可以做資料轉換/轉拋
'''

7
src/fc_network_adapter/fc_network_adapter/utils/__init__.py
Unescape Escape View File

@ -0,0 +1,7 @@
"""
共用工具模組
"""
from .ringBuffer import RingBuffer
from .theLogger import setup_logger
__all__ = ['RingBuffer', 'setup_logger']

129
src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py
Unescape Escape View File

@ -0,0 +1,129 @@
import socket
import random
import os
def get_used_ports():
"""
從 /proc/net/tcp 和 /proc/net/udp 讀取系統已占用的 port
直接讀取 Linux 系統資訊，避免暴力嘗試
Returns:
set: 已被占用的 port 號集合
"""
used_ports = set()
# 讀取 TCP 占用的 port (包含 IPv4 和 IPv6)
for filepath in ['/proc/net/tcp', '/proc/net/tcp6']:
if os.path.exists(filepath):
try:
with open(filepath, 'r') as f:
lines = f.readlines()[1:] # 跳過標題行
for line in lines:
parts = line.split()
if len(parts) > 1:
# local_address 格式: "0100007F:1F90" (hex)
local_addr = parts[1]
port_hex = local_addr.split(':')[1]
port = int(port_hex, 16)
used_ports.add(port)
except (IOError, PermissionError):
pass
# 讀取 UDP 占用的 port (包含 IPv4 和 IPv6)
for filepath in ['/proc/net/udp', '/proc/net/udp6']:
if os.path.exists(filepath):
try:
with open(filepath, 'r') as f:
lines = f.readlines()[1:] # 跳過標題行
for line in lines:
parts = line.split()
if len(parts) > 1:
local_addr = parts[1]
port_hex = local_addr.split(':')[1]
port = int(port_hex, 16)
used_ports.add(port)
except (IOError, PermissionError):
pass
return used_ports
def is_port_available(port):
"""
測試指定 port 是否可用 (TCP 和 UDP 都測試)
Args:
port (int): 要測試的 port 號
Returns:
bool: True 表示可用，False 表示被占用
"""
# 測試 TCP
try:
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.bind(('', port))
except OSError:
return False
# 測試 UDP
try:
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.bind(('', port))
except OSError:
return False
return True
def find_available_port(start_port=1024, end_port=65535):
"""
在指定的 port 區間內隨機找出一個未被占用的 port
使用 Linux /proc/net 系統資訊來過濾已占用的 port，避免暴力嘗試
確保 TCP 和 UDP 都可用
Args:
start_port (int): 起始 port 號 (預設 1024)
end_port (int): 結束 port 號 (預設 65535)
Returns:
int: 可用的 port 號，如果找不到則返回 None
"""
if start_port < 1 or end_port > 65535 or start_port >= end_port:
raise ValueError("Port 範圍必須在 1-65535 之間，且起始 port 必須小於結束 port")
# 從系統讀取已占用的 port
used_ports = get_used_ports()
# 計算可用的 port 列表 (排除已占用的)
available_ports = [p for p in range(start_port, end_port + 1) if p not in used_ports]
if not available_ports:
return None
# 隨機打亂順序
random.shuffle(available_ports)
# 從可用列表中挑選，再用 socket 雙重確認 TCP 和 UDP 都可用
for port in available_ports:
if is_port_available(port):
return port
# 如果都不可用
return None
if __name__ == "__main__":
# 使用範例
port = find_available_port(8000, 9000)
if port:
print(f"找到可用的 port: {port}")
else:
print("找不到可用的 port")
# 自訂範圍範例
port = find_available_port(10000, 20000)
if port:
print(f"在 10000-20000 範圍找到可用的 port: {port}")

111
src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py
Unescape Escape View File

@ -0,0 +1,111 @@
"""
Serial Port Discovery Utility
This module provides functions to discover available serial ports on the system.
It uses glob pattern matching to find serial device files in /dev/.
"""
import glob
from typing import List, Union
def get_serial_ports() -> List[str]:
"""
獲取系統中所有可用的串口設備列表
在 Linux 系統中，會搜尋以下模式的設備：
- /dev/ttyUSB* (USB 串口設備)
- /dev/ttyACM* (USB CDC ACM 設備)
- /dev/ttyS* (標準串口)
Returns:
List[str]: 包含所有找到的串口設備路徑的列表
Example:
>>> ports = get_serial_ports()
>>> print(ports)
['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyS0']
"""
serial_ports = []
# 搜尋不同類型的串口設備
patterns = [
'/dev/ttyUSB*', # USB 串口轉換器
'/dev/ttyACM*', # USB CDC ACM 設備（如 Arduino）
'/dev/ttyS*', # 標準串口
]
for pattern in patterns:
serial_ports.extend(glob.glob(pattern))
# 排序以便於閱讀
serial_ports.sort()
return serial_ports
def get_serial_ports_with_filter(filter_patterns: Union[str, List[str]] = None) -> List[str]:
"""
獲取串口設備列表，可選擇性地使用自訂篩選模式
Args:
filter_patterns (Union[str, List[str]], optional):
單一或多個 glob 模式
- 字串: '/dev/ttyUSB*'
- 列表: ['/dev/ttyUSB*', '/dev/ttyACM*']
如果為 None，則使用預設模式搜尋所有串口
Returns:
List[str]: 符合條件的串口設備路徑列表
Example:
>>> # 單一 pattern
>>> ports = get_serial_ports_with_filter('/dev/ttyUSB*')
>>> print(ports)
['/dev/ttyUSB0', '/dev/ttyUSB1']
>>> # 多個 patterns
>>> ports = get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
>>> print(ports)
['/dev/ttyACM0', '/dev/ttyUSB0', '/dev/ttyUSB1']
"""
if filter_patterns is None:
return get_serial_ports()
# 統一轉成 list 處理
if isinstance(filter_patterns, str):
filter_patterns = [filter_patterns]
serial_ports = []
for pattern in filter_patterns:
serial_ports.extend(glob.glob(pattern))
serial_ports.sort()
return serial_ports
if __name__ == "__main__":
# 使用範例
print("=== Serial Port Discovery ===\n")
# 1. 獲取所有串口設備
all_ports = get_serial_ports()
print(f"找到 {len(all_ports)} 個串口設備:")
for port in all_ports:
print(f" - {port}")
print("\n" + "="*30 + "\n")
# 2. 只搜尋 USB 串口
usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*')
print(f"找到 {len(usb_ports)} 個 USB 串口設備:")
for port in usb_ports:
print(f" - {port}")
print("\n" + "="*30 + "\n")
# 3. 只搜尋 ACM 設備
acm_ports = get_serial_ports_with_filter('/dev/ttyACM*')
print(f"找到 {len(acm_ports)} 個 ACM 設備:")
for port in acm_ports:
print(f" - {port}")

231
src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py
Unescape Escape View File

@ -0,0 +1,231 @@
# import array
import threading
import ctypes
from typing import Any, Optional, Tuple
class RingBuffer:
"""
高效能無鎖環形緩衝區，使用原子操作避免鎖定
用於在不同執行緒間高效傳遞資料
"""
# 緩衝區計數器，用於自動分配 buffer_id
_buffer_counter = 0
_counter_lock = threading.Lock()
def __init__(self, capacity: int = 256, buffer_id: int = None):
"""
初始化環形緩衝區
Args:
capacity: 緩衝區容量 (必須是 2 的次方)
buffer_id: 緩衝區 ID，如果為 None 則自動分配
"""
# 確保容量是 2 的次方，便於使用位運算取模
if capacity & (capacity - 1) != 0:
# 找到大於等於 capacity 的最小 2 的次方
capacity = 1 << (capacity - 1).bit_length()
# 分配緩衝區 ID
if buffer_id is None:
with RingBuffer._counter_lock:
buffer_id = RingBuffer._buffer_counter
RingBuffer._buffer_counter += 1
self.buffer_id = buffer_id
self.capacity = capacity
self.mask = capacity - 1 # 用於快速取模
self.buffer = [None] * capacity
# 原子計數器作為讀寫指標
self.write_index = ctypes.c_uint64(0)
self.read_index = ctypes.c_uint64(0)
# 用於檢測上次操作的執行緒 ID
self._last_read_thread = None
self._last_write_thread = None
# 添加同時讀寫統計
self.concurrent_write_count = ctypes.c_uint64(0) # 同時寫入計數
self.concurrent_read_count = ctypes.c_uint64(0) # 同時讀取計數
self.total_write_count = ctypes.c_uint64(0) # 總寫入操作計數
self.total_read_count = ctypes.c_uint64(0) # 總讀取操作計數
self.overflow_count = ctypes.c_uint64(0) # 緩衝區溢出次數
# 記錄各執行緒的操作次數
self.thread_write_counts = {} # {thread_id: count}
self.thread_read_counts = {} # {thread_id: count}
# 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作）
self._stats_lock = threading.Lock()
def put(self, item: Any) -> bool:
"""
將項目存入緩衝區
Args:
item: 要存入的項目
Returns:
bool: 成功寫入返回 True，緩衝區已滿返回 False
"""
# 更新寫入統計
self.total_write_count.value += 1
# 檢測上次寫入的執行緒
current_thread = threading.get_ident()
# 記錄當前執行緒寫入次數
with self._stats_lock:
self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1
# 檢測是否為不同執行緒同時寫入
if self._last_write_thread is not None and current_thread != self._last_write_thread:
self.concurrent_write_count.value += 1
self._last_write_thread = current_thread
# 原子獲取當前寫入位置
current = self.write_index.value
next_pos = (current + 1) & self.mask
# 檢查緩衝區是否已滿
if next_pos == self.read_index.value:
self.overflow_count.value += 1
return False
# 寫入資料並原子更新寫入位置
self.buffer[current] = item
self.write_index.value = next_pos
return True
def get(self) -> Optional[Any]:
"""
從緩衝區讀取項目
Returns:
項目或 None（如果緩衝區為空）
"""
# 更新讀取統計
self.total_read_count.value += 1
# 檢測上次讀取的執行緒
current_thread = threading.get_ident()
# 記錄當前執行緒讀取次數
with self._stats_lock:
self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1
# 檢測是否為不同執行緒同時讀取
if self._last_read_thread is not None and current_thread != self._last_read_thread:
self.concurrent_read_count.value += 1
self._last_read_thread = current_thread
# 檢查緩衝區是否為空
if self.read_index.value == self.write_index.value:
return None
# 原子獲取當前讀取位置並讀取資料
current = self.read_index.value
item = self.buffer[current]
# 原子更新讀取位置
self.read_index.value = (current + 1) & self.mask
return item
def get_all(self) -> list:
"""
獲取緩衝區中的所有項目
Returns:
list: 所有可用項目的列表
"""
items = []
while True:
item = self.get()
if item is None:
break
items.append(item)
return items
def size(self) -> int:
# 返回目前緩衝區內項目數量
return (self.write_index.value - self.read_index.value) & self.mask
def is_empty(self) -> bool:
# 檢查緩衝區是否為空
return self.read_index.value == self.write_index.value
def is_full(self) -> bool:
# 檢查緩衝區是否已滿
return ((self.write_index.value + 1) & self.mask) == self.read_index.value
def clear(self) -> None:
"""清空緩衝區"""
self.read_index.value = self.write_index.value
def get_stats(self) -> dict:
"""
獲取緩衝區統計資訊
Returns:
dict: 包含各種統計數據的字典
"""
with self._stats_lock:
stats = {
"buffer_id": self.buffer_id,
"capacity": self.capacity,
"current_size": self.size(),
"is_full": self.is_full(),
"is_empty": self.is_empty(),
"total_writes": self.total_write_count.value,
"total_reads": self.total_read_count.value,
"concurrent_writes": self.concurrent_write_count.value,
"concurrent_reads": self.concurrent_read_count.value,
"overflow_count": self.overflow_count.value,
"write_threads": len(self.thread_write_counts),
"read_threads": len(self.thread_read_counts),
"concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value),
"concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value),
"top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3],
"top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3],
}
return stats
def print_stats(self) -> None:
"""輸出當前緩衝區統計資訊"""
stats = self.get_stats()
print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===")
print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}")
print(f"Total Write Operations: {stats['total_writes']}")
print(f"Total Read Operations: {stats['total_reads']}")
print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})")
print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})")
print(f"Buffer Overflow Count: {stats['overflow_count']}")
print(f"Writing Threads: {stats['write_threads']}")
print(f"Reading Threads: {stats['read_threads']}")
print("Top Writer Threads:")
for thread_id, count in stats['top_writer_threads']:
print(f" Thread {thread_id}: {count} writes")
print("Top Reader Threads:")
for thread_id, count in stats['top_reader_threads']:
print(f" Thread {thread_id}: {count} reads")
print("=============================\n")
def reset_stats(self) -> None:
"""重置所有統計數據"""
with self._stats_lock:
self.concurrent_write_count.value = 0
self.concurrent_read_count.value = 0
self.total_write_count.value = 0
self.total_read_count.value = 0
self.overflow_count.value = 0
self.thread_write_counts.clear()
self.thread_read_counts.clear()
def __str__(self) -> str:
"""返回緩衝區的字符串表示"""
return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})"

43
src/fc_network_adapter/fc_network_adapter/utils/theLogger.py
Unescape Escape View File

@ -0,0 +1,43 @@
import logging
import os
from logging.handlers import TimedRotatingFileHandler
# 全域 Logger 實例
_global_logger = None
def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger:
global _global_logger
if _global_logger is None:
# 確保 logs 資料夾存在
os.makedirs(log_dir, exist_ok=True)
# 建立全域 Logger
_global_logger = logging.getLogger("global_logger")
_global_logger.setLevel(level)
_global_logger.propagate = False # 防止重複輸出
formatter = logging.Formatter(
fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s",
datefmt="%m-%d %H:%M:%S"
)
# 檔案輸出（每天輪替）
file_handler = TimedRotatingFileHandler(
filename=os.path.join(log_dir, "application.log"),
when="midnight", # 每天 0 點輪替
backupCount=7, # 保留 7 天
encoding="utf-8"
)
file_handler.setFormatter(formatter)
_global_logger.addHandler(file_handler)
# 終端機輸出
console_handler = logging.StreamHandler()
console_handler.setFormatter(formatter)
_global_logger.addHandler(console_handler)
# 為每個模組建立子 Logger，並設定名稱
module_logger = _global_logger.getChild(name)
module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱
return module_logger

1
src/fc_network_adapter/setup.py
Unescape Escape View File

@ -20,6 +20,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main',
],
},
)

277
src/fc_network_adapter/tests/demo_integration.py
Unescape Escape View File

@ -0,0 +1,277 @@
import os
import sys
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
# 基礎功能的 import
import queue
import time
# ROS2 的 import
import rclpy
# mavlink 的 import
from pymavlink import mavutil
# 自定義的 import
from ..fc_network_adapter import mavlinkObject as mo
from ..fc_network_adapter import mavlinkVehicleView as mvv
# from ..fc_network_adapter import mavlinkDevice as md
# ====================== 分割線 =====================
test_item = 1
running_time = 3
print('test_item : ', test_item)
'''
測試項 個位數 表示分離的功能
測試項 1X 表示 mavlink_object 的功能 測試連線的能力
'''
if test_item == 1:
print('===> Start of Program .Test ', test_item)
connection_string="udp:127.0.0.1:14591"
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
# mavlink_object1 = mo.mavlink_object(mavlink_socket1)
time.sleep(1)
print("mark A")
# print("Socket IP:", mavlink_socket1.target_system)
print("Socket port:", mavlink_socket1.port.getsockname())
# print("=== ", dir(mavlink_socket1.port))
elif test_item == 10:
# 需要開啟一個 ardupilot 的模擬器
# 測試 mavlink_object 放入 ring buffer 的應用
print('===> Start of Program .Test ', test_item)
# 清空 ring buffer
mo.stream_bridge_ring.clear()
mo.return_packet_ring.clear()
manager = mo.async_io_manager()
manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 初始化輸入通道
connection_string="udp:127.0..1:14571"
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
sock = mavlink_socket1.port
print("Socket port:", sock)
manager.add_mavlink_object(mavlink_object1)
start_time = time.time()
while (time.time() - start_time) < running_time:
items_a = mo.stream_bridge_ring.get_all()
items_b = mo.return_packet_ring.get_all()
try:
print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}")
except IndexError:
print("stream_bridge_ring is empty")
try:
print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}")
except IndexError:
print("return_packet_ring is empty")
time.sleep(1)
manager.shutdown()
print('<=== End of Program')
elif test_item == 11:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
print('===> Start of Program .Test ', test_item)
# 清空 ring buffer
mo.stream_bridge_ring.clear()
mo.return_packet_ring.clear()
manager = mo.async_io_manager()
manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 初始化輸入通道
connection_string="udp:127.0.0.1:14571"
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
manager.add_mavlink_object(mavlink_object1)
# 啟動 mavlink_bridge
bridge = mo.mavlink_bridge()
bridge.start()
time.sleep(3)
# 印出目前所有 mavlink_systems 的內容
print('目前所有的系統 : ')
all_vehicles = mvv.vehicle_registry.get_all()
for sysid, vehicle in all_vehicles.items():
print(f" System {sysid}: {vehicle}")
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
if (time.time() - show_time) >= 2:
# print("mark B")
show_time = time.time()
for sysid, vehicle in all_vehicles.items():
for compid in vehicle.components:
comp = vehicle.get_component(compid)
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count))
comp.reset_packet_stats()
print("===================")
manager.shutdown()
bridge.stop()
print('<=== End of Program')
elif test_item == 12:
# 需要開啟一個 ardupilot 的模擬器 與 GCS
# 這邊是測試 mavlink object 作為交換器功能的代碼
print('===> Start of Program .Test ', test_item)
# 清空 ring buffer
mo.stream_bridge_ring.clear()
mo.return_packet_ring.clear()
manager = mo.async_io_manager()
manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 初始化輸入通道
connection_string="udp:127.0.0.1:14571"
mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
connection_string="udp:127.0.0.1:14571"
mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
# 初始化輸出通道
connection_string="udpout:127.0.0.1:14551"
mavlink_socket_out = mavutil.mavlink_connection(connection_string)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
manager.add_mavlink_object(mavlink_object_out)
manager.add_mavlink_object(mavlink_object_in1)
manager.add_mavlink_object(mavlink_object_in2)
time.sleep(1) # 等待通道啟動
mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id)
mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id)
mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id)
mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id)
start_time = time.time()
while (time.time() - start_time) < running_time:
time.sleep(1)
manager.shutdown()
print('<=== End of Program')
elif test_item == 21:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
print('===> Start of Program .Test ', test_item)
# 初始化 rclpy 才能使用 node
rclpy.init()
# 清空 ring buffer
mo.stream_bridge_ring.clear()
mo.return_packet_ring.clear()
manager = mo.async_io_manager()
manager.start()
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
# 關於 Node 的初始化
show_time = time.time()
analyzer._init_node() # 初始化 node
print('初始化 node 完成 耗時 : ',time.time() - show_time)
time.sleep(0.5) # 系統 Setup 完成
# 創建通道
connection_string="udp:127.0.0.1:14560"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object3 = mo.mavlink_object(mavlink_socket)
manager.add_mavlink_object(mavlink_object3)
print('=== waiting for mavlink data ...')
time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
compid = 1
sysid = 1
start_time = time.time()
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
end_time = time.time()
print(f"Execution time for create_flightMode: {end_time - start_time} seconds")
print("start emit info")
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
# print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode'])
analyzer.emit_info() # 這邊是測試 node 的運行
time.sleep(1)
except KeyboardInterrupt:
break
# 程式結束
analyzer.destroy_node()
rclpy.shutdown()
# 結束程式 退出所有 thread
manager.stop()
analyzer.stop()
analyzer.thread.join()
mavlink_socket.close()
print('<=== End of Program')
elif test_item == 52:
print('===> Start of Program .Test ', test_item)
manager = mo.async_io_manager()
manager.start()
# print(manager.thread.is_alive())
manager.shutdown()
time.sleep(1)
print('manager stopped')

331
src/fc_network_adapter/tests/demo_mavlinkVehicleView.py
Unescape Escape View File

@ -0,0 +1,331 @@
"""
VehicleView 使用範例
展示如何使用純狀態容器來管理 MAVLink 載具資訊
"""
import time
from ..fc_network_adapter.mavlinkVehicleView import (
VehicleView,
VehicleComponent,
RFModule,
vehicle_registry,
ConnectionType,
ComponentType,
RFModuleType
)
def example_basic_usage():
"""基本使用範例"""
print("=== 基本使用範例 ===\n")
# 1. 建立載具視圖
vehicle = VehicleView(sysid=1)
vehicle.kind = "Copter"
vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR
vehicle.connected_via = ConnectionType.UDP
print(f"建立載具: {vehicle}\n")
# 2. 新增 autopilot 組件
autopilot = vehicle.add_component(
component_id=1,
comp_type=ComponentType.AUTOPILOT
)
autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR
autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA
print(f"新增組件: {autopilot}\n")
# 3. 手動餵入位置資訊
autopilot.status.position.latitude = 25.0330
autopilot.status.position.longitude = 121.5654
autopilot.status.position.altitude = 100.5
autopilot.status.position.timestamp = time.time()
print(f"位置: 緯度={autopilot.status.position.latitude}, "
f"經度={autopilot.status.position.longitude}, "
f"高度={autopilot.status.position.altitude}m\n")
# 4. 手動餵入姿態資訊
autopilot.status.attitude.roll = 0.05 # 弧度
autopilot.status.attitude.pitch = -0.02
autopilot.status.attitude.yaw = 1.57
autopilot.status.attitude.timestamp = time.time()
print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, "
f"Pitch={autopilot.status.attitude.pitch:.3f}, "
f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n")
# 5. 手動餵入飛行模式
autopilot.status.mode.base_mode = 89
autopilot.status.mode.custom_mode = 4
autopilot.status.mode.mode_name = "GUIDED"
autopilot.status.mode.timestamp = time.time()
print(f"飛行模式: {autopilot.status.mode.mode_name}\n")
# 6. 手動餵入電池資訊
autopilot.status.battery.voltage = 12.6
autopilot.status.battery.current = 15.2
autopilot.status.battery.remaining = 75
autopilot.status.battery.timestamp = time.time()
print(f"電池: 電壓={autopilot.status.battery.voltage}V, "
f"電流={autopilot.status.battery.current}A, "
f"剩餘={autopilot.status.battery.remaining}%\n")
def example_packet_tracking():
"""封包追蹤範例"""
print("\n=== 封包追蹤範例 ===\n")
vehicle = VehicleView(sysid=2)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# 模擬接收封包
timestamp = time.time()
# 接收 HEARTBEAT (msg_type=0)
autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp)
# 接收 ATTITUDE (msg_type=30)
autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1)
# 接收 GLOBAL_POSITION_INT (msg_type=33)
autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2)
# 模擬封包遺失 (seq 跳過 3, 4, 5)
autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3)
stats = autopilot.packet_stats
print(f"封包統計:")
print(f" 接收: {stats.received_count}")
print(f" 遺失: {stats.lost_count}")
print(f" 最後序號: {stats.last_seq}")
print(f" 訊息類型計數: {stats.msg_type_count}\n")
def example_parameters():
"""參數管理範例"""
print("\n=== 參數管理範例 ===\n")
vehicle = VehicleView(sysid=3)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# 手動設定參數 (不會主動下載)
autopilot.set_parameter("ARMING_CHECK", 1)
autopilot.set_parameter("ANGLE_MAX", 4500)
autopilot.set_parameter("WPNAV_SPEED", 500)
print(f"參數數量: {len(autopilot.parameters)}")
print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}")
print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}")
print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n")
def example_rf_module():
"""RF模組範例"""
print("\n=== RF模組範例 ===\n")
vehicle = VehicleView(sysid=4)
vehicle.connected_via = ConnectionType.SERIAL
# 設定 XBee RF 模組
rf = vehicle.set_rf_module(RFModuleType.XBEE)
# 更新 Socket 資訊
rf.update_socket_info(
ip="192.168.1.100",
port=14550,
local_ip="192.168.1.1",
local_port=14551,
connected=True
)
# 更新 RSSI
rf.update_rssi(rssi=-65, timestamp=time.time())
# 更新 AT 命令回應
rf.update_at_response("OK", timestamp=time.time())
# 自定義狀態
rf.status.custom_status['signal_quality'] = 'excellent'
rf.status.custom_status['packet_error_rate'] = 0.001
print(f"RF模組: {rf}")
print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}")
print(f"RSSI: {rf.status.rssi} dBm")
print(f"AT回應: {rf.status.at_response}")
print(f"自定義狀態: {rf.status.custom_status}\n")
def example_multiple_components():
"""多組件範例"""
print("\n=== 多組件範例 ===\n")
vehicle = VehicleView(sysid=5)
vehicle.kind = "Copter with Gimbal"
# Autopilot 組件
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
autopilot.mav_type = 2
autopilot.status.mode.mode_name = "AUTO"
# Gimbal 組件
gimbal = vehicle.add_component(154, ComponentType.GIMBAL)
gimbal.mav_type = 26 # MAV_TYPE_GIMBAL
gimbal.status.attitude.pitch = -0.785 # 向下45度
gimbal.status.attitude.yaw = 0.0
# Camera 組件
camera = vehicle.add_component(100, ComponentType.CAMERA)
camera.mav_type = 30 # MAV_TYPE_CAMERA
camera.status.custom_status['recording'] = True
camera.status.custom_status['photo_interval'] = 2.0
print(f"載具: {vehicle}")
print(f"組件數量: {len(vehicle.components)}")
for cid, comp in vehicle.components.items():
print(f" 組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}")
print()
def example_registry():
"""註冊表使用範例"""
print("\n=== 註冊表使用範例 ===\n")
# 註冊多個載具
v1 = vehicle_registry.register(sysid=1)
v1.kind = "Copter-1"
v1.add_component(1, ComponentType.AUTOPILOT)
v2 = vehicle_registry.register(sysid=2)
v2.kind = "Plane-1"
v2.add_component(1, ComponentType.AUTOPILOT)
v3 = vehicle_registry.register(sysid=3)
v3.kind = "Rover-1"
v3.add_component(1, ComponentType.AUTOPILOT)
print(f"註冊表中的載具數量: {len(vehicle_registry)}")
# 取得所有載具
all_vehicles = vehicle_registry.get_all()
for sysid, vehicle in all_vehicles.items():
print(f" System {sysid}: {vehicle.kind}")
# 檢查載具是否存在
print(f"\nSystem 2 存在? {2 in vehicle_registry}")
print(f"System 99 存在? {99 in vehicle_registry}")
# 取得特定載具
vehicle = vehicle_registry.get(2)
if vehicle:
print(f"\n取得載具: {vehicle}")
# 註銷載具
vehicle_registry.unregister(3)
print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n")
def example_serialization():
"""序列化範例 (除錯/日誌用)"""
print("\n=== 序列化範例 ===\n")
vehicle = VehicleView(sysid=10)
vehicle.kind = "Test Copter"
vehicle.connected_via = ConnectionType.UDP
vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0'
vehicle.custom_meta['frame_type'] = 'X'
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
autopilot.mav_type = 2
autopilot.status.position.altitude = 50.0
autopilot.status.battery.voltage = 12.4
autopilot.update_packet_stats(0, 0, time.time())
autopilot.update_packet_stats(1, 30, time.time())
rf = vehicle.set_rf_module(RFModuleType.UDP)
rf.update_rssi(-70)
rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True)
# 轉換為字典
data = vehicle.to_dict()
print("載具資料 (字典格式):")
import json
print(json.dumps(data, indent=2, ensure_ascii=False))
def example_gps_ekf():
"""GPS 與 EKF 範例"""
print("\n\n=== GPS 與 EKF 範例 ===\n")
vehicle = VehicleView(sysid=11)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# GPS 資訊
autopilot.status.gps.fix_type = 3 # 3D Fix
autopilot.status.gps.satellites_visible = 12
autopilot.status.gps.eph = 120 # HDOP = 1.2
autopilot.status.gps.epv = 180 # VDOP = 1.8
autopilot.status.gps.timestamp = time.time()
print(f"GPS:")
print(f" Fix Type: {autopilot.status.gps.fix_type}")
print(f" 衛星數: {autopilot.status.gps.satellites_visible}")
print(f" HDOP: {autopilot.status.gps.eph/100}")
# EKF 資訊
autopilot.status.ekf.flags = 0x1FF # 所有 flags 都 OK
autopilot.status.ekf.velocity_variance = 0.5
autopilot.status.ekf.pos_horiz_variance = 1.2
autopilot.status.ekf.pos_vert_variance = 2.0
autopilot.status.ekf.timestamp = time.time()
print(f"\nEKF:")
print(f" Flags: 0x{autopilot.status.ekf.flags:X}")
print(f" 速度變異: {autopilot.status.ekf.velocity_variance}")
print(f" 水平位置變異: {autopilot.status.ekf.pos_horiz_variance}")
print(f" 垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n")
def example_vfr_hud():
"""VFR HUD 範例"""
print("\n=== VFR HUD 範例 ===\n")
vehicle = VehicleView(sysid=12)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# VFR HUD 資訊
autopilot.status.vfr.airspeed = 15.5 # m/s
autopilot.status.vfr.groundspeed = 14.8 # m/s
autopilot.status.vfr.heading = 90 # 東方
autopilot.status.vfr.throttle = 65 # %
autopilot.status.vfr.climb = 2.5 # m/s
autopilot.status.vfr.timestamp = time.time()
print(f"VFR HUD:")
print(f" 空速: {autopilot.status.vfr.airspeed} m/s")
print(f" 地速: {autopilot.status.vfr.groundspeed} m/s")
print(f" 航向: {autopilot.status.vfr.heading}°")
print(f" 油門: {autopilot.status.vfr.throttle}%")
print(f" 爬升率: {autopilot.status.vfr.climb} m/s\n")
if __name__ == "__main__":
# 執行所有範例
# example_basic_usage()
# example_packet_tracking()
# example_parameters()
# example_rf_module()
# example_multiple_components()
# example_registry()
# example_serialization()
# example_gps_ekf()
example_vfr_hud()
print("\n" + "="*50)
print("所有範例執行完成！")
print("="*50)

152
src/fc_network_adapter/tests/demo_ringBuffer.py
Unescape Escape View File

@ -0,0 +1,152 @@
import os
import sys
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
import time
import threading
from ..fc_network_adapter.utils import RingBuffer
def producer(buffer, count, interval=0.01):
"""生產者：向緩衝區添加資料"""
print(f"Producer started (thread {threading.get_ident()})")
for i in range(count):
# 嘗試寫入數據，直到成功
while not buffer.put(f"Item-{i}"):
print(f"Buffer full, producer waiting... (thread {threading.get_ident()})")
time.sleep(0.1)
print(f"Produced: Item-{i}, buffer size: {buffer.size()}")
time.sleep(interval) # 模擬生產過程
print(f"Producer finished (thread {threading.get_ident()})")
def consumer(buffer, max_items, interval=0.05):
"""消費者：從緩衝區讀取資料"""
print(f"Consumer started (thread {threading.get_ident()})")
items_consumed = 0
while items_consumed < max_items:
# 嘗試讀取數據
item = buffer.get()
if item:
print(f"Consumed: {item}, buffer size: {buffer.size()}")
items_consumed += 1
else:
print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})")
time.sleep(interval) # 模擬消費過程
print(f"Consumer finished (thread {threading.get_ident()})")
def batch_consumer(buffer, interval=0.2):
"""批量消費者：一次性讀取緩衝區中的所有資料"""
print(f"Batch consumer started (thread {threading.get_ident()})")
for _ in range(5): # 執行5次批量讀取
time.sleep(interval) # 等待緩衝區積累數據
items = buffer.get_all()
if items:
print(f"Batch consumed {len(items)} items: {items}")
else:
print("Buffer empty for batch consumer")
print(f"Batch consumer finished (thread {threading.get_ident()})")
def demonstrate_multi_writer():
"""示範多個寫入執行緒同時操作緩衝區"""
print("\n=== Demonstrating Multiple Writers ===")
buffer = RingBuffer(capacity=80)
# 創建多個生產者執行緒
threads = []
for i in range(3):
thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1)))
threads.append(thread)
thread.start()
# 等待所有執行緒完成
for thread in threads:
thread.join()
buffer.print_stats() # 印出統計資訊
# 讀出所有剩餘資料
remaining = buffer.get_all()
print(f"Remaining items in buffer after multiple writers: {remaining}")
def demonstrate_basic_usage():
"""示範基本使用方式"""
print("\n=== Demonstrating Basic Usage ===")
# 創建緩衝區
buffer = RingBuffer(capacity=20, buffer_id=7)
# 檢查初始狀態
print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
# 添加幾個項目
for i in range(5):
buffer.put(f"Test-{i}")
# 檢查狀態
print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
# 讀取一個項目
item = buffer.get()
print(f"Read item: {item}")
print(f"After reading 1 item - Content Size: {buffer.size()}")
# 添加更多項目直到滿
items_added = 0
while not buffer.is_full():
buffer.put(f"Fill-{items_added}")
items_added += 1
print(f"Added {items_added} more items until full")
print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
# 嘗試添加到已滿的緩衝區
result = buffer.put("Overflow")
print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}")
# 獲取所有項目
all_items = buffer.get_all()
print(f"All items in buffer: {all_items}")
print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}")
# 印出統計資訊
buffer.print_stats()
def demonstrate_producer_consumer():
"""示範生產者-消費者模式"""
print("\n=== Demonstrating Producer-Consumer Pattern ===")
buffer = RingBuffer(capacity=16)
# 創建生產者和消費者執行緒
producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1))
consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2))
batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5))
# 啟動執行緒
producer_thread.start()
consumer_thread.start()
batch_thread.start()
# 等待執行緒完成
producer_thread.join()
consumer_thread.join()
batch_thread.join()
# 檢查最終狀態
print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}")
buffer.print_stats()
if __name__ == "__main__":
# 展示各種使用場景
# demonstrate_basic_usage()
# demonstrate_producer_consumer()
demonstrate_multi_writer()
print("\nAll demonstrations completed!")

430
src/fc_network_adapter/tests/test_mavlinkObject.py
Unescape Escape View File

@ -13,10 +13,9 @@ import time
import threading
import socket
import asyncio
from unittest.mock import MagicMock, patch
# 導入要測試的模組
from fc_network_adapter.mavlinkObject import (
from ..fc_network_adapter.mavlinkObject import (
mavlink_object,
async_io_manager,
MavlinkObjectState,
@ -24,62 +23,100 @@ from fc_network_adapter.mavlinkObject import (
return_packet_ring
)
# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式)
# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes)
HEARTBEAT_PACKET_1 = bytes([
0xFE, # STX (MAVLink 1.0)
0x09, # payload length (9 bytes)
0x00, # sequence
0x01, # system ID = 1
0x01, # component ID = 1
0x00, # message ID (HEARTBEAT = 0)
# Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1)
0x00, 0x00, 0x00, 0x00, # custom_mode = 0
0x02, # type = MAV_TYPE_QUADROTOR (2)
0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3)
0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64)
0x03, # system_status = MAV_STATE_STANDBY (3)
0x03, # mavlink_version = 3
0x62, 0x8E # CRC (simplified placeholder)
])
HEARTBEAT_PACKET_2 = bytes([
0xFE, # STX
0x09, # payload length
0x01, # sequence (增加)
0x01, # system ID = 1
0x01, # component ID = 1
0x00, # message ID (HEARTBEAT = 0)
0x00, 0x00, 0x00, 0x00,
0x02, 0x03, 0x41, 0x03, 0x03,
0x33, 0xEC
])
HEARTBEAT_PACKET_3 = bytes([
0xFE, # STX
0x09, # payload length
0x02, # sequence
0x02, # system ID = 2
0x01, # component ID = 1
0x00, # message ID (HEARTBEAT = 0)
0x00, 0x00, 0x00, 0x00,
0x02, 0x03, 0x42, 0x03, 0x03,
0x37, 0x44
])
class MockMavlinkSocket:
"""模擬 Mavlink Socket 的類別，用於測試"""
def __init__(self, test_data=None):
class MockMavlinkSocket:
"""模擬 Mavlink Socket 的類別，用於測試
使用真實的 MAVLink 封包，而不是模擬的訊息對象
"""
def __init__(self, test_packets=None):
"""
Args:
test_packets: list of bytes，每個元素都是完整的 MAVLink 封包
"""
self.closed = False
self.test_data = test_data or []
self.data_index = 0
self.test_packets = test_packets or []
self.packet_index = 0
self.written_data = []
# 使用 pymavlink 來解析封包
from pymavlink import mavutil
self.mav_parser = mavutil.mavlink.MAVLink(self)
def recv_msg(self):
if not self.test_data or self.data_index >= len(self.test_data):
"""返回解析後的 MAVLink 訊息對象"""
if not self.test_packets or self.packet_index >= len(self.test_packets):
return None
data = self.test_data[self.data_index]
self.data_index += 1
return data
packet = self.test_packets[self.packet_index]
self.packet_index += 1
# 使用 pymavlink 解析封包
try:
for byte in packet:
msg = self.mav_parser.parse_char(bytes([byte]))
if msg:
return msg
except Exception as e:
print(f"Error parsing packet: {e}")
return None
return None
def write(self, data):
"""寫入數據（用於檢查轉發）"""
self.written_data.append(data)
def close(self):
"""關閉 socket"""
self.closed = True
class MockMavlinkMessage:
"""模擬 Mavlink 訊息的類別，用於測試"""
def __init__(self, msg_id, sysid, compid, seq=0):
self.msg_id = msg_id
self.sysid = sysid
self.compid = compid
self.seq = seq
self.msg_buf = bytes([msg_id, sysid, compid, seq])
def get_msgId(self):
return self.msg_id
def get_srcSystem(self):
return self.sysid
def get_srcComponent(self):
return self.compid
def get_seq(self):
return self.seq
def get_msgbuf(self):
return self.msg_buf
def get_type(self):
return f"MSG_ID_{self.msg_id}"
class TestMavlinkObject(unittest.TestCase):
"""測試 mavlink_object 類別"""
"""測試 mavlink_object 類別的獨立功能"""
def setUp(self):
"""在每個測試方法執行前準備環境"""
@ -91,28 +128,19 @@ class TestMavlinkObject(unittest.TestCase):
stream_bridge_ring.clear()
return_packet_ring.clear()
# 創建測試訊息
self.heartbeat_msg = MockMavlinkMessage(msg_id=0, sysid=1, compid=1)
self.attitude_msg = MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
self.position_msg = MockMavlinkMessage(msg_id=32, sysid=1, compid=1)
# 創建模擬的 socket # 假的 Mavlink Socket
self.mock_socket = MockMavlinkSocket([
self.heartbeat_msg,
self.attitude_msg,
self.position_msg
])
# 創建模擬的 socket，使用真實封包
self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1])
# 創建測試對象
self.mavlink_obj = mavlink_object(self.mock_socket)
def test_initialization(self):
"""測試 mavlink_object 初始化是否正確"""
# print("Testing mavlink_object initialization")
self.assertEqual(self.mavlink_obj.socket_id, 0)
self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT)
self.assertEqual(len(self.mavlink_obj.target_sockets), 0)
self.assertEqual(self.mavlink_obj.bridge_msg_types, [0])
self.assertEqual(self.mavlink_obj.return_msg_types, [])
def test_add_remove_target_socket(self):
"""測試添加和移除目標端口功能"""
@ -120,6 +148,7 @@ class TestMavlinkObject(unittest.TestCase):
self.assertTrue(self.mavlink_obj.add_target_socket(1))
self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
self.assertEqual(self.mavlink_obj.target_sockets[0], 1)
self.assertTrue(self.mavlink_obj.add_target_socket(2))
self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
self.assertIn(2, self.mavlink_obj.target_sockets)
@ -153,78 +182,31 @@ class TestMavlinkObject(unittest.TestCase):
self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid"))
self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"]))
def test_send_message(self):
"""測試 send_message 功能"""
# 創建一個新的 mavlink_object 實例
mock_socket = MockMavlinkSocket()
mavlink_obj = mavlink_object(mock_socket)
# 準備測試數據
test_message1 = b"test_message_1"
test_message2 = b"test_message_2"
# 測試初始狀態
self.assertEqual(len(mock_socket.written_data), 0)
def test_send_message_validation(self):
"""測試 send_message 的數據驗證功能（不需要啟動 manager）"""
# 測試非運行狀態下發送消息
self.assertFalse(mavlink_obj.send_message(test_message1))
self.assertEqual(len(mock_socket.written_data), 0)
# 啟動 manager
manager = async_io_manager()
manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 添加對象到 manager
manager.add_mavlink_object(mavlink_obj)
time.sleep(0.1) # 等待對象啟動
# 確認對象狀態
self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
# 測試發送消息
self.assertTrue(mavlink_obj.send_message(test_message1))
time.sleep(0.2) # 等待消息處理
# 確認消息已發送
self.assertEqual(len(mock_socket.written_data), 1)
self.assertEqual(mock_socket.written_data[0], test_message1)
# 測試連續發送多條消息
self.assertTrue(mavlink_obj.send_message(test_message2))
time.sleep(0.2) # 等待消息處理
# 測試無效的數據類型
self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態
self.assertFalse(self.mavlink_obj.send_message("invalid"))
self.assertFalse(self.mavlink_obj.send_message(123))
# 確認兩條消息都已發送
self.assertEqual(len(mock_socket.written_data), 2)
self.assertEqual(mock_socket.written_data[1], test_message2)
# 模擬發送出錯的情況
class ErrorWriteSocket(MockMavlinkSocket):
def write(self, data):
raise Exception("Write error")
error_socket = ErrorWriteSocket()
error_obj = mavlink_object(error_socket)
manager.add_mavlink_object(error_obj)
time.sleep(0.1) # 等待對象啟動
# 發送消息到錯誤的 socket
self.assertTrue(error_obj.send_message(test_message1))
time.sleep(0.2) # 等待消息處理
# 測試太短的封包
self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00])))
# 即使寫入失敗，send_message 應該也返回 True
# 因為消息已成功加入到佇列中，只是後續的實際發送失敗
# 測試無效的起始標記
invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00])
self.assertFalse(self.mavlink_obj.send_message(invalid_packet))
# 停止 manager
manager.stop()
time.sleep(0.5) # 等待 manager 停止
# 測試有效的封包可以加入佇列
self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1)
# 測試對象已關閉後發送消息
self.assertFalse(mavlink_obj.send_message(test_message1))
self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加
self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態
class TestAsyncIOManager(unittest.TestCase):
"""測試 async_io_manager 類別"""
"""測試 async_io_manager 類別的獨立功能"""
def setUp(self):
"""在每個測試方法執行前準備環境"""
@ -239,14 +221,9 @@ class TestAsyncIOManager(unittest.TestCase):
# 創建 async_io_manager 實例
self.manager = async_io_manager()
# 模擬 mavlink 對象
self.mock_socket1 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
])
self.mock_socket2 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=2, compid=1)
])
# 創建模擬 mavlink 對象，使用真實封包
self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2])
self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3])
self.mavlink_obj1 = mavlink_object(self.mock_socket1)
self.mavlink_obj2 = mavlink_object(self.mock_socket2)
@ -254,10 +231,10 @@ class TestAsyncIOManager(unittest.TestCase):
def tearDown(self):
"""在每個測試方法執行後清理環境"""
if self.manager.running:
self.manager.stop()
self.manager.shutdown()
def test_singleton_pattern(self):
"""測試 async_io_manager 的單例模式 就是不會產生兩個 magager"""
"""測試 async_io_manager 的單例模式"""
manager1 = async_io_manager()
manager2 = async_io_manager()
self.assertIs(manager1, manager2)
@ -275,7 +252,7 @@ class TestAsyncIOManager(unittest.TestCase):
self.assertIs(self.manager.thread, old_thread)
# 停止管理器
self.manager.stop()
self.manager.shutdown()
self.assertFalse(self.manager.running)
# 最多等待 5 秒讓線程結束
@ -313,89 +290,115 @@ class TestAsyncIOManager(unittest.TestCase):
self.assertFalse(self.manager.remove_mavlink_object(999))
# 停止管理器
self.manager.stop()
self.manager.shutdown()
def test_data_processing(self):
"""測試數據處理"""
# 創建用於轉發的 mavlink_objects
mock_socket3 = MockMavlinkSocket()
mavlink_obj3 = mavlink_object(mock_socket3)
class TestIntegration(unittest.TestCase):
"""整合測試，測試多個 mavlink_object 之間的互動與資料流"""
# 設置轉發
self.mavlink_obj1.add_target_socket(2) # socket1 轉發到 socket3
def setUp(self):
"""在每個測試方法執行前準備環境"""
# 清空全局變數
mavlink_object.mavlinkObjects = {}
mavlink_object.socket_num = 0
# 設置訊息類型
self.mavlink_obj1.set_bridge_message_types([0, 30]) # HEARTBEAT, ATTITUDE
# self.mavlink_obj1.enable_return(True)
self.mavlink_obj1.set_return_message_types([30]) # ATTITUDE
# 清空 ring buffer
stream_bridge_ring.clear()
return_packet_ring.clear()
# 啟動管理器並添加對象
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 創建 async_io_manager 實例
self.manager = async_io_manager()
self.manager.add_mavlink_object(self.mavlink_obj1)
self.manager.add_mavlink_object(mavlink_obj3)
def tearDown(self):
"""在每個測試方法執行後清理環境"""
if self.manager.running:
self.manager.shutdown()
# 等待處理完成
time.sleep(1.0)
def test_send_message_with_manager(self):
"""測試透過 async_io_manager 發送訊息的完整流程"""
# 創建一個新的 mavlink_object 實例
mock_socket = MockMavlinkSocket()
mavlink_obj = mavlink_object(mock_socket)
# print("testing Mark A")
# 測試初始狀態
self.assertEqual(len(mock_socket.written_data), 0)
# 檢查 Ring buffer 是否有正確的數據
self.assertEqual(stream_bridge_ring.size(), 2) # HEARTBEAT + ATTITUDE
self.assertEqual(return_packet_ring.size(), 1) # ATTITUDE
# 測試非運行狀態下發送消息
self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
self.assertEqual(len(mock_socket.written_data), 0)
a = stream_bridge_ring.get()
print(f"stream_bridge_ring: {a}")
# 啟動 manager
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 檢查是否正確轉發
self.assertEqual(len(mock_socket3.written_data), 2) # HEARTBEAT + ATTITUDE
# 添加對象到 manager
self.manager.add_mavlink_object(mavlink_obj)
time.sleep(0.1) # 等待對象啟動
# print("testing Mark B")
# 確認對象狀態
self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
# 停止管理器
self.manager.stop()
# 測試發送消息
self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
time.sleep(0.2) # 等待消息處理
def test_error_handling(self):
"""測試錯誤處理情況"""
print("=== mark A ===")
# 創建一個會引發異常的 socket
class ErrorSocket(MockMavlinkSocket):
def recv_msg(self):
raise Exception("Test exception")
# 確認消息已發送
self.assertEqual(len(mock_socket.written_data), 1)
self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1)
error_socket = ErrorSocket()
mavlink_obj_err = mavlink_object(error_socket)
# 測試連續發送多條消息
self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2))
time.sleep(0.2) # 等待消息處理
# 啟動管理器並添加對象
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 確認兩條消息都已發送
self.assertEqual(len(mock_socket.written_data), 2)
self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2)
self.manager.add_mavlink_object(mavlink_obj_err)
# 停止 manager
self.manager.shutdown()
time.sleep(0.5) # 等待 manager 停止
print("=== mark B ===")
# 測試對象已關閉後發送消息
self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加
# 等待錯誤處理
time.sleep(1.0)
def test_data_processing_and_forwarding(self):
"""測試數據處理與轉發流程"""
# 創建用於轉發的 mavlink_objects
mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,])
mock_socket3 = MockMavlinkSocket()
# 對象應該進入錯誤狀態，但不會崩潰
# self.assertEqual(mavlink_obj_err.state, MavlinkObjectState.ERROR)
mavlink_obj1 = mavlink_object(mock_socket1)
mavlink_obj3 = mavlink_object(mock_socket3)
print("=== mark C ===")
# 設置訊息類型
mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT
# 管理器應該仍在運行
self.assertTrue(self.manager.running)
# 設置轉發: obj1 -> obj3
mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1)
# 啟動管理器並添加對象
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 故意等一段時間 確認 socket 有被 manager 處理掉
time.sleep(3)
"""
這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現
mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包
若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失
"""
self.manager.add_mavlink_object(mavlink_obj3)
self.manager.add_mavlink_object(mavlink_obj1)
# 停止管理器
self.manager.stop()
# 等待處理完成
time.sleep(0.5)
# 檢查 Ring buffer 是否有正確的數據
self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT
class TestIntegration(unittest.TestCase):
"""整合測試，測試多個 mavlink_object 之間的互動"""
# 檢查是否正確轉發
self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT
# 停止管理器
self.manager.shutdown()
def test_bidirectional_forwarding(self):
"""測試雙向轉發"""
@ -406,18 +409,9 @@ class TestIntegration(unittest.TestCase):
return_packet_ring.clear()
# 創建三個 mavlink 對象，模擬三個通道
socket1 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
])
socket2 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=2, compid=1),
MockMavlinkMessage(msg_id=32, sysid=2, compid=1)
])
socket3 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=3, compid=1),
MockMavlinkMessage(msg_id=33, sysid=3, compid=1)
])
socket1 = MockMavlinkSocket()
socket2 = MockMavlinkSocket()
socket3 = MockMavlinkSocket()
obj1 = mavlink_object(socket1)
obj2 = mavlink_object(socket2)
@ -431,38 +425,44 @@ class TestIntegration(unittest.TestCase):
obj3.add_target_socket(1) # obj3 -> obj2
# 啟動 async_io_manager
manager = async_io_manager()
manager.start()
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 添加所有 mavlink_object
manager.add_mavlink_object(obj1)
manager.add_mavlink_object(obj2)
manager.add_mavlink_object(obj3)
self.manager.add_mavlink_object(obj1)
self.manager.add_mavlink_object(obj2)
self.manager.add_mavlink_object(obj3)
# 對三個對象添加數據
socket1.test_packets.append(HEARTBEAT_PACKET_1)
socket2.test_packets.append(HEARTBEAT_PACKET_2)
socket3.test_packets.append(HEARTBEAT_PACKET_3)
# 等待處理所有訊息
time.sleep(1.5)
time.sleep(1.0)
# 檢查轉發結果
# socket1 應該收到 socket2 和 socket3 的訊息
self.assertEqual(len(socket1.written_data), 4) # 2 from obj2, 2 from obj3 via obj2
# socket1 應該收到 socket2 的訊息
self.assertGreaterEqual(len(socket1.written_data), 1)
# socket2 應該收到 socket1 和 socket3 的訊息
self.assertEqual(len(socket2.written_data), 4) # 2 from obj1, 2 from obj3
self.assertGreaterEqual(len(socket2.written_data), 2)
# socket3 應該收到 socket1 和 socket2 的訊息
self.assertEqual(len(socket3.written_data), 4) # 2 from obj1 via obj2, 2 from obj2
# socket3 應該收到 socket2 的訊息
self.assertGreaterEqual(len(socket3.written_data), 1)
# 檢查 ring buffer 的數據
# 假設所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
# 應該有 3 個 HEARTBEAT 訊息
self.assertEqual(stream_bridge_ring.size(), 3) # 3 HEARTBEAT
# 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT
# 停止管理器
manager.stop()
self.manager.shutdown()
if __name__ == "__main__":
unittest.main(defaultTest="TestMavlinkObject.test_send_message")
# 可以指定要運行的測試
# unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation")
# unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects")
unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding")
unittest.main()

400
src/fc_network_adapter/tests/test_ringBuffer.py
Unescape Escape View File

@ -1,152 +1,296 @@
import os
import sys
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
#!/usr/bin/env python
"""
測試 RingBuffer 類別的功能
"""
import time
import unittest
import threading
import time
from concurrent.futures import ThreadPoolExecutor
from ..fc_network_adapter.utils import RingBuffer
class TestRingBuffer(unittest.TestCase):
"""測試 RingBuffer 基本功能"""
def setUp(self):
"""每個測試前的準備"""
self.buffer = RingBuffer(capacity=8)
def test_initialization(self):
"""測試初始化"""
self.assertEqual(self.buffer.capacity, 8)
self.assertTrue(self.buffer.is_empty())
self.assertFalse(self.buffer.is_full())
self.assertEqual(self.buffer.size(), 0)
def test_put_get_basic(self):
"""測試基本的放入和取出"""
# 測試放入
self.assertTrue(self.buffer.put("item1"))
self.assertTrue(self.buffer.put("item2"))
self.assertEqual(self.buffer.size(), 2)
self.assertFalse(self.buffer.is_empty())
# 測試取出
item1 = self.buffer.get()
self.assertEqual(item1, "item1")
self.assertEqual(self.buffer.size(), 1)
item2 = self.buffer.get()
self.assertEqual(item2, "item2")
self.assertTrue(self.buffer.is_empty())
# 空緩衝區取出應返回 None
self.assertIsNone(self.buffer.get())
def test_capacity_overflow(self):
"""測試緩衝區容量限制"""
# 填滿緩衝區 (容量-1，因為需要預留一個位置)
for i in range(7): # 8-1=7
self.assertTrue(self.buffer.put(f"item{i}"))
self.assertTrue(self.buffer.is_full())
# 嘗試再放入一個應該失敗
self.assertFalse(self.buffer.put("overflow"))
self.assertEqual(self.buffer.overflow_count.value, 1)
def test_get_all(self):
"""測試取出所有項目"""
items = ["a", "b", "c", "d"]
for item in items:
self.buffer.put(item)
all_items = self.buffer.get_all()
self.assertEqual(all_items, items)
self.assertTrue(self.buffer.is_empty())
def test_clear(self):
"""測試清空緩衝區"""
for i in range(5):
self.buffer.put(f"item{i}")
self.buffer.clear()
self.assertTrue(self.buffer.is_empty())
self.assertEqual(self.buffer.size(), 0)
class TestRingBufferThreadSafety(unittest.TestCase):
"""測試 RingBuffer 的線程安全性"""
from fc_network_adapter.ringBuffer import RingBuffer
def setUp(self):
"""每個測試前的準備"""
self.buffer = RingBuffer(capacity=256)
self.results = []
self.write_count = 1000
self.read_count = 1000
def test_concurrent_producers_consumers(self):
"""測試多生產者多消費者場景"""
self.results = []
stats = self.buffer.get_stats()
self.assertEqual(stats['total_writes'], 0)
def producer(buffer, count, interval=0.01):
"""生產者：向緩衝區添加資料"""
print(f"Producer started (thread {threading.get_ident()})")
def producer(producer_id, count):
"""生產者函數"""
for i in range(count):
# 嘗試寫入數據，直到成功
while not buffer.put(f"Item-{i}"):
print(f"Buffer full, producer waiting... (thread {threading.get_ident()})")
time.sleep(0.1)
print(f"Produced: Item-{i}, buffer size: {buffer.size()}")
time.sleep(interval) # 模擬生產過程
print(f"Producer finished (thread {threading.get_ident()})")
def consumer(buffer, max_items, interval=0.05):
"""消費者：從緩衝區讀取資料"""
print(f"Consumer started (thread {threading.get_ident()})")
items_consumed = 0
while items_consumed < max_items:
# 嘗試讀取數據
item = buffer.get()
if item:
print(f"Consumed: {item}, buffer size: {buffer.size()}")
items_consumed += 1
else:
print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})")
time.sleep(interval) # 模擬消費過程
print(f"Consumer finished (thread {threading.get_ident()})")
def batch_consumer(buffer, interval=0.2):
"""批量消費者：一次性讀取緩衝區中的所有資料"""
print(f"Batch consumer started (thread {threading.get_ident()})")
for _ in range(5): # 執行5次批量讀取
time.sleep(interval) # 等待緩衝區積累數據
items = buffer.get_all()
if items:
print(f"Batch consumed {len(items)} items: {items}")
else:
print("Buffer empty for batch consumer")
print(f"Batch consumer finished (thread {threading.get_ident()})")
def demonstrate_multi_writer():
"""示範多個寫入執行緒同時操作緩衝區"""
print("\n=== Demonstrating Multiple Writers ===")
buffer = RingBuffer(capacity=80)
# 創建多個生產者執行緒
threads = []
for i in range(3):
thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1)))
threads.append(thread)
thread.start()
# 等待所有執行緒完成
for thread in threads:
thread.join()
buffer.print_stats() # 印出統計資訊
# 讀出所有剩餘資料
remaining = buffer.get_all()
print(f"Remaining items in buffer after multiple writers: {remaining}")
def demonstrate_basic_usage():
"""示範基本使用方式"""
print("\n=== Demonstrating Basic Usage ===")
# 創建緩衝區
buffer = RingBuffer(capacity=20, buffer_id=7)
# 檢查初始狀態
print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
# 添加幾個項目
item = f"producer_{producer_id}_item_{i}"
while not self.buffer.put(item):
time.sleep(0.001) # 緩衝區滿時稍微等待
def consumer(consumer_id, count):
"""消費者函數"""
items = []
for _ in range(count):
item = None
while item is None:
item = self.buffer.get()
if item is None:
time.sleep(0.001) # 緩衝區空時稍微等待
items.append(item)
self.results.extend(items)
# 創建多個生產者和消費者
with ThreadPoolExecutor(max_workers=8) as executor:
# 2 個生產者，每個寫入 500 個項目
producer_futures = [
executor.submit(producer, 0, 500),
executor.submit(producer, 1, 500)
]
# 2 個消費者，每個讀取 500 個項目
consumer_futures = [
executor.submit(consumer, 0, 500),
executor.submit(consumer, 1, 500)
]
# 等待所有任務完成
for future in producer_futures + consumer_futures:
future.result()
# 驗證結果
self.assertEqual(len(self.results), 1000)
self.assertTrue(self.buffer.is_empty())
# 檢查統計數據
stats = self.buffer.get_stats()
self.assertEqual(stats['total_writes'], 1000)
self.assertGreater(stats['total_reads'], 1000) # 包含失敗的讀取嘗試
self.assertGreater(stats['write_threads'], 1)
self.assertGreater(stats['read_threads'], 1)
def test_high_throughput(self):
"""測試高吞吐量場景"""
items_per_thread = 10000
num_threads = 4
def writer():
for i in range(items_per_thread):
while not self.buffer.put(i):
pass # 忙等待
def reader():
items = []
for _ in range(items_per_thread):
item = None
while item is None:
item = self.buffer.get()
items.append(item)
self.results.extend(items)
start_time = time.time()
with ThreadPoolExecutor(max_workers=num_threads * 2) as executor:
# 啟動寫入線程
writer_futures = [executor.submit(writer) for _ in range(num_threads)]
# 啟動讀取線程
reader_futures = [executor.submit(reader) for _ in range(num_threads)]
# 等待完成
for future in writer_futures + reader_futures:
future.result()
end_time = time.time()
# 驗證結果
total_items = items_per_thread * num_threads
self.assertEqual(len(self.results), total_items)
# 性能統計
duration = end_time - start_time
throughput = total_items / duration
print(f"\nHigh Throughput Test Results:")
print(f"Total items: {total_items}")
print(f"Duration: {duration:.2f}s")
print(f"Throughput: {throughput:.0f} items/sec")
# 顯示詳細統計
self.buffer.print_stats()
class TestRingBufferStatistics(unittest.TestCase):
"""測試 RingBuffer 的統計功能"""
def test_statistics_tracking(self):
"""測試統計數據追蹤"""
buffer = RingBuffer(capacity=16)
# 寫入一些數據
for i in range(10):
buffer.put(f"item{i}")
# 讀取一些數據
for _ in range(5):
buffer.get()
stats = buffer.get_stats()
# 驗證基本統計
self.assertEqual(stats['total_writes'], 10)
self.assertEqual(stats['total_reads'], 5)
self.assertEqual(stats['current_size'], 5)
self.assertEqual(stats['write_threads'], 1)
self.assertEqual(stats['read_threads'], 1)
def test_reset_statistics(self):
"""測試重置統計數據"""
buffer = RingBuffer(capacity=16)
# 產生一些活動
for i in range(5):
buffer.put(f"Test-{i}")
buffer.put(f"item{i}")
for _ in range(3):
buffer.get()
# 檢查狀態
print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
# 重置統計
buffer.reset_stats()
# 讀取一個項目
item = buffer.get()
print(f"Read item: {item}")
print(f"After reading 1 item - Content Size: {buffer.size()}")
stats = buffer.get_stats()
self.assertEqual(stats['total_writes'], 0)
self.assertEqual(stats['total_reads'], 0)
self.assertEqual(stats['concurrent_writes'], 0)
self.assertEqual(stats['concurrent_reads'], 0)
self.assertEqual(stats['overflow_count'], 0)
# 添加更多項目直到滿
items_added = 0
while not buffer.is_full():
buffer.put(f"Fill-{items_added}")
items_added += 1
print(f"Added {items_added} more items until full")
print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
def benchmark_ringbuffer():
"""RingBuffer 性能基準測試"""
print("\n=== RingBuffer Performance Benchmark ===")
# 嘗試添加到已滿的緩衝區
result = buffer.put("Overflow")
print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}")
buffer = RingBuffer(capacity=1024)
num_operations = 100000
# 獲取所有項目
all_items = buffer.get_all()
print(f"All items in buffer: {all_items}")
print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}")
# 單線程性能測試
start_time = time.time()
for i in range(num_operations):
buffer.put(i)
for _ in range(num_operations):
buffer.get()
end_time = time.time()
# 印出統計資訊
buffer.print_stats()
single_thread_time = end_time - start_time
throughput = (num_operations * 2) / single_thread_time
def demonstrate_producer_consumer():
"""示範生產者-消費者模式"""
print("\n=== Demonstrating Producer-Consumer Pattern ===")
buffer = RingBuffer(capacity=16)
print(f"Single Thread: {throughput:.0f} ops/sec")
# 創建生產者和消費者執行緒
producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1))
consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2))
batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5))
# 多線程性能測試
buffer = RingBuffer(capacity=1024)
# 啟動執行緒
producer_thread.start()
consumer_thread.start()
batch_thread.start()
def producer():
for i in range(num_operations // 2):
while not buffer.put(i):
pass
# 等待執行緒完成
producer_thread.join()
consumer_thread.join()
batch_thread.join()
def consumer():
for _ in range(num_operations // 2):
while buffer.get() is None:
pass
# 檢查最終狀態
print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}")
start_time = time.time()
with ThreadPoolExecutor(max_workers=2) as executor:
future1 = executor.submit(producer)
future2 = executor.submit(consumer)
future1.result()
future2.result()
end_time = time.time()
multi_thread_time = end_time - start_time
throughput = num_operations / multi_thread_time
print(f"Multi Thread: {throughput:.0f} ops/sec")
print(f"Speedup: {single_thread_time/multi_thread_time:.2f}x")
buffer.print_stats()
if __name__ == "__main__":
# 展示各種使用場景
# demonstrate_basic_usage()
# demonstrate_producer_consumer()
demonstrate_multi_writer()
# 運行單元測試
unittest.main(argv=[''], exit=False, verbosity=2)
print("\nAll demonstrations completed!")
# 運行性能基準測試
benchmark_ringbuffer()

507
src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
Unescape Escape View File

@ -0,0 +1,507 @@
"""
VehicleStatusPublisher 測試程式
測試從 vehicle_registry 讀取資料並發布到 ROS2 topics
"""
import time
import json
import threading
# ROS2 imports
import rclpy
from rclpy.node import Node
# 標準 ROS2 消息類型
import std_msgs.msg
import sensor_msgs.msg
import geometry_msgs.msg
import mavros_msgs.msg
# 專案 imports
from ..fc_network_adapter.mavlinkROS2Nodes import (
VehicleStatusPublisher,
fc_ros_manager,
ros2_manager
)
from ..fc_network_adapter.mavlinkVehicleView import (
vehicle_registry,
ConnectionType,
ComponentType,
)
class TestSubscriber(Node):
"""測試用的訂閱者節點 - 接收並記錄收到的消息"""
def __init__(self, sysid: int = 1):
super().__init__(f'test_subscriber_sys{sysid}')
self.sysid = sysid
self.received_messages = {
'position': [],
'attitude': [],
'velocity': [],
'battery': [],
'vfr_hud': [],
'mode': [],
'summary': [],
}
# 建立所有訂閱者
self._create_subscriptions()
print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics")
def _create_subscriptions(self):
"""建立所有 topic 的訂閱者"""
base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}'
# Position
self.create_subscription(
sensor_msgs.msg.NavSatFix,
f'{base_topic}/position',
lambda msg: self._on_message('position', msg),
10
)
# Attitude
self.create_subscription(
sensor_msgs.msg.Imu,
f'{base_topic}/attitude',
lambda msg: self._on_message('attitude', msg),
10
)
# Velocity
self.create_subscription(
geometry_msgs.msg.TwistStamped,
f'{base_topic}/velocity',
lambda msg: self._on_message('velocity', msg),
10
)
# Battery
self.create_subscription(
sensor_msgs.msg.BatteryState,
f'{base_topic}/battery',
lambda msg: self._on_message('battery', msg),
10
)
# VFR HUD
self.create_subscription(
mavros_msgs.msg.VfrHud,
f'{base_topic}/vfr_hud',
lambda msg: self._on_message('vfr_hud', msg),
10
)
# Mode
self.create_subscription(
std_msgs.msg.String,
f'{base_topic}/mode',
lambda msg: self._on_message('mode', msg),
10
)
# Summary
self.create_subscription(
std_msgs.msg.String,
f'{base_topic}/summary',
lambda msg: self._on_message('summary', msg),
10
)
def _on_message(self, topic_name: str, msg):
"""通用消息接收回調"""
self.received_messages[topic_name].append(msg)
print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}")
def _format_msg(self, topic_name: str, msg) -> str:
"""格式化消息以便顯示"""
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})"
elif topic_name == 'velocity':
return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})"
elif topic_name == 'battery':
return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%"
elif topic_name == 'vfr_hud':
return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}"
elif topic_name == 'mode':
return f"mode={msg.data}"
elif topic_name == 'summary':
try:
data = json.loads(msg.data)
return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}"
except:
return msg.data
return str(msg)
def get_message_count(self, topic_name: str) -> int:
"""獲取收到的消息數量"""
return len(self.received_messages[topic_name])
def clear_messages(self):
"""清空已收到的消息"""
for key in self.received_messages:
self.received_messages[key].clear()
def setup_test_vehicle(sysid: int = 1, socket_id: int = 10):
"""
建立測試用的載具數據
Args:
sysid: 系統 ID
socket_id: Socket ID
"""
print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===")
# 註冊載具
vehicle = vehicle_registry.register(sysid)
vehicle.kind = "Copter"
vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR
vehicle.connected_via = ConnectionType.UDP
vehicle.custom_meta['socket_id'] = socket_id
# 新增 autopilot 組件 (component_id=1)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR
autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA
# 填充狀態資料
status = autopilot.status
# 位置
status.position.latitude = 25.0330
status.position.longitude = 121.5654
status.position.altitude = 100.5
status.position.relative_altitude = 50.0
status.position.timestamp = time.time()
# 姿態
status.attitude.roll = 0.1
status.attitude.pitch = -0.05
status.attitude.yaw = 1.57
status.attitude.rollspeed = 0.01
status.attitude.pitchspeed = 0.02
status.attitude.yawspeed = 0.03
status.attitude.timestamp = time.time()
# 飛行模式
status.mode.base_mode = 89
status.mode.custom_mode = 4
status.mode.mode_name = "GUIDED"
status.mode.timestamp = time.time()
# 電池
status.battery.voltage = 12.6
status.battery.current = 15.3
status.battery.remaining = 75
status.battery.temperature = 35.2
status.battery.timestamp = time.time()
# GPS
status.gps.fix_type = 3 # 3D fix
status.gps.satellites_visible = 12
status.gps.eph = 100
status.gps.epv = 150
status.gps.timestamp = time.time()
# VFR
status.vfr.airspeed = 5.5
status.vfr.groundspeed = 6.0
status.vfr.heading = 90
status.vfr.throttle = 65
status.vfr.climb = 1.2
status.vfr.timestamp = time.time()
# 系統狀態
status.armed = True
status.system_status = 4 # MAV_STATE_ACTIVE
# 更新封包統計
autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time())
print(f"✓ 載具 {sysid} 已建立並填充測試數據")
return vehicle
def test_basic_publishing():
"""測試基本的發布功能"""
print("\n" + "="*60)
print("測試 1: 基本發布功能")
print("="*60)
# 清空 registry
vehicle_registry.clear()
# 建立測試載具
vehicle = setup_test_vehicle(sysid=1, socket_id=10)
# 初始化 ROS2 管理器
if not ros2_manager.initialized:
ros2_manager.initialize()
# 建立測試訂閱者
test_sub = TestSubscriber(sysid=1)
# 啟動 publisher
ros2_manager.start()
print("\n--- 開始發布，等待 5 秒 ---")
# 運行 5 秒，持續 spin
start_time = time.time()
while time.time() - start_time < 5.0:
rclpy.spin_once(test_sub, timeout_sec=0.1)
time.sleep(0.1)
# 檢查收到的消息
print("\n--- 消息統計 ---")
total_messages = 0
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} 條消息")
print(f"\n總計收到: {total_messages} 條消息")
# 驗證
if total_messages > 0:
print("\n✓ 測試通過：成功接收到消息")
else:
print("\n✗ 測試失敗：沒有接收到任何消息")
# 停止
ros2_manager.stop()
test_sub.destroy_node()
def test_frequency_control():
"""測試頻率控制功能"""
print("\n" + "="*60)
print("測試 2: 頻率控制")
print("="*60)
# 清空 registry
vehicle_registry.clear()
# 建立測試載具
vehicle = setup_test_vehicle(sysid=1, socket_id=10)
# 初始化（如果還沒初始化）
if not ros2_manager.initialized:
ros2_manager.initialize()
# 建立測試訂閱者
test_sub = TestSubscriber(sysid=1)
# 修改頻率設定
publisher_node = ros2_manager.status_publisher
publisher_node.rate_controller.topic_intervals = {
'position': 1.5,
'attitude': 1.0,
'velocity': 1.0,
'battery': 1.0,
'vfr_hud': 0.5,
'mode': 0.0,
'summary': 0.0,
}
# 啟動 publisher
ros2_manager.start()
print("\n--- 測試頻率控制，運行 3 秒 ---")
# 運行 3 秒
start_time = time.time()
while time.time() - start_time < 3.0:
rclpy.spin_once(test_sub, timeout_sec=0.1)
time.sleep(0.1)
# 檢查頻率
print("\n--- 頻率分析 ---")
print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
print("2Hz Topics (預期 ~6 條):")
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']:
count = test_sub.get_message_count(topic)
print(f" {topic:15s}: {count:3d} 條")
print("\n✓ 頻率控制測試完成")
# 停止
ros2_manager.stop()
test_sub.destroy_node()
def test_multi_vehicle():
"""測試多載具發布"""
print("\n" + "="*60)
print("測試 3: 多載具發布")
print("="*60)
# 清空 registry
vehicle_registry.clear()
# 建立 3 個測試載具
v1 = setup_test_vehicle(sysid=1, socket_id=10)
v2 = setup_test_vehicle(sysid=2, socket_id=11)
v3 = setup_test_vehicle(sysid=3, socket_id=12)
# 修改各載具的位置以便區分
v2.components[1].status.position.latitude = 26.0
v3.components[1].status.position.latitude = 27.0
# 初始化
if not ros2_manager.initialized:
ros2_manager.initialize()
# 建立 3 個測試訂閱者
test_sub1 = TestSubscriber(sysid=1)
test_sub2 = TestSubscriber(sysid=2)
test_sub3 = TestSubscriber(sysid=3)
# 啟動 publisher
ros2_manager.start()
print("\n--- 測試多載具，運行 3 秒 ---")
# 運行 3 秒
start_time = time.time()
while time.time() - start_time < 3.0:
rclpy.spin_once(test_sub1, timeout_sec=0.05)
rclpy.spin_once(test_sub2, timeout_sec=0.05)
rclpy.spin_once(test_sub3, timeout_sec=0.05)
time.sleep(0.1)
# 檢查每個載具的消息
print("\n--- 各載具消息統計 ---")
for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]:
total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
print(f"載具 {sysid}: {total:3d} 條消息")
# 檢查 summary 中的 socket_id
if test_sub.get_message_count('summary') > 0:
last_summary = test_sub.received_messages['summary'][-1]
data = json.loads(last_summary.data)
print(f" └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}")
print("\n✓ 多載具測試完成")
# 停止
ros2_manager.stop()
test_sub1.destroy_node()
test_sub2.destroy_node()
test_sub3.destroy_node()
def test_dynamic_vehicle():
"""測試動態新增/移除載具"""
print("\n" + "="*60)
print("測試 4: 動態載具管理")
print("="*60)
# 清空 registry
vehicle_registry.clear()
# 初始化
if not ros2_manager.initialized:
ros2_manager.initialize()
# 建立測試訂閱者
test_sub = TestSubscriber(sysid=1)
# 啟動 publisher
ros2_manager.start()
print("\n--- 階段 1: 無載具，運行 1 秒 ---")
start_time = time.time()
while time.time() - start_time < 1.0:
rclpy.spin_once(test_sub, timeout_sec=0.1)
time.sleep(0.1)
count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
print(f"收到消息: {count_before} 條")
# 新增載具
print("\n--- 階段 2: 新增載具，運行 2 秒 ---")
vehicle = setup_test_vehicle(sysid=1, socket_id=10)
start_time = time.time()
while time.time() - start_time < 2.0:
rclpy.spin_once(test_sub, timeout_sec=0.1)
time.sleep(0.1)
count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
print(f"收到消息: {count_after - count_before} 條")
# 移除載具
print("\n--- 階段 3: 移除載具，運行 1 秒 ---")
vehicle_registry.unregister(1)
start_time = time.time()
while time.time() - start_time < 1.0:
rclpy.spin_once(test_sub, timeout_sec=0.1)
time.sleep(0.1)
count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
print(f"收到消息: {count_final - count_after} 條（應該為 0）")
if count_final - count_after == 0:
print("\n✓ 動態載具管理測試通過")
else:
print("\n✗ 移除載具後仍收到消息")
# 停止
ros2_manager.stop()
test_sub.destroy_node()
def main():
"""主測試函數"""
print("\n" + "="*60)
print("VehicleStatusPublisher 測試程式")
print("="*60)
try:
# 執行各項測試
test_basic_publishing()
# time.sleep(1)
# test_frequency_control()
# time.sleep(1)
# test_multi_vehicle()
# time.sleep(1)
# test_dynamic_vehicle()
print("\n" + "="*60)
print("所有測試完成！")
print("="*60)
except KeyboardInterrupt:
print("\n\n測試被中斷")
except Exception as e:
print(f"\n\n測試出錯: {e}")
import traceback
traceback.print_exc()
finally:
# 清理
if ros2_manager.initialized:
ros2_manager.shutdown()
vehicle_registry.clear()
print("\n清理完成")
if __name__ == '__main__':
main()

398
src/unitdev01/unitdev01/comm_panel.py
Unescape Escape View File

@ -0,0 +1,398 @@
#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
QPushButton, QLineEdit)
from PyQt6.QtCore import pyqtSignal
class CommPanel(QWidget):
"""通讯设置面板类"""
# 定义信号
udp_connection_added = pyqtSignal(str, int) # ip, port
udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
ws_connection_added = pyqtSignal(str) # url
ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
status_message = pyqtSignal(str, int) # message, timeout
def __init__(self, parent=None):
super().__init__(parent)
self.udp_connections = []
self.ws_connections = []
self._init_ui()
def _init_ui(self):
"""初始化UI"""
layout = QVBoxLayout(self)
layout.setContentsMargins(10, 10, 10, 10)
layout.setSpacing(10)
# ========== UDP MAVLink 區域 ==========
udp_title = QLabel("UDP")
udp_title.setStyleSheet("""
color: #DDD;
font-size: 14px;
font-weight: bold;
padding: 5px;
background-color: #333;
border-radius: 4px;
""")
layout.addWidget(udp_title)
# UDP 連接列表容器
self.udp_list_widget = QWidget()
self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
self.udp_list_layout.setSpacing(5)
layout.addWidget(self.udp_list_widget)
# UDP 添加新連接區域
add_udp_widget = QWidget()
add_udp_layout = QHBoxLayout(add_udp_widget)
add_udp_layout.setContentsMargins(0, 0, 0, 0)
self.udp_ip_input = QLineEdit()
self.udp_ip_input.setText("127.0.0.1")
self.udp_ip_input.setPlaceholderText("IP")
self.udp_ip_input.setStyleSheet("""
QLineEdit {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
""")
self.udp_port_input = QLineEdit()
self.udp_port_input.setText("14540")
self.udp_port_input.setPlaceholderText("Port")
self.udp_port_input.setFixedWidth(80)
self.udp_port_input.setStyleSheet("""
QLineEdit {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
""")
add_udp_btn = QPushButton("添加")
add_udp_btn.clicked.connect(self._handle_add_udp)
add_udp_btn.setStyleSheet("""
QPushButton {
background-color: #4CAF50;
color: white;
border: none;
padding: 6px 12px;
border-radius: 4px;
min-width: 30px;
}
QPushButton:hover { background-color: #45a049; }
""")
add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
add_udp_layout.addWidget(self.udp_ip_input)
add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
add_udp_layout.addWidget(self.udp_port_input)
add_udp_layout.addWidget(add_udp_btn)
layout.addWidget(add_udp_widget)
# 分隔線
separator = QWidget()
separator.setFixedHeight(20)
layout.addWidget(separator)
# ========== WebSocket 區域 ==========
ws_title = QLabel("WebSocket")
ws_title.setStyleSheet("""
color: #DDD;
font-size: 14px;
font-weight: bold;
padding: 5px;
background-color: #333;
border-radius: 4px;
""")
layout.addWidget(ws_title)
# WebSocket 連接列表容器
self.ws_list_widget = QWidget()
self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
self.ws_list_layout.setSpacing(5)
layout.addWidget(self.ws_list_widget)
# WebSocket 添加新連接區域
add_ws_widget = QWidget()
add_ws_layout = QHBoxLayout(add_ws_widget)
add_ws_layout.setContentsMargins(0, 0, 0, 0)
self.ws_url_input = QLineEdit()
self.ws_url_input.setPlaceholderText("host")
self.ws_url_input.setStyleSheet("""
QLineEdit {
background-color: #333;
color: #DDD;
border: 1px solid #555;
border-radius: 4px;
padding: 5px;
}
""")
add_ws_btn = QPushButton("添加")
add_ws_btn.clicked.connect(self._handle_add_ws)
add_ws_btn.setStyleSheet("""
QPushButton {
background-color: #4CAF50;
color: white;
border: none;
padding: 6px 12px;
border-radius: 4px;
min-width: 30px;
}
QPushButton:hover { background-color: #45a049; }
""")
add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
add_ws_layout.addWidget(self.ws_url_input)
add_ws_layout.addWidget(add_ws_btn)
layout.addWidget(add_ws_widget)
layout.addStretch()
def _handle_add_udp(self):
"""處理添加 UDP 連接"""
ip = self.udp_ip_input.text().strip()
port_text = self.udp_port_input.text().strip()
if not ip or not port_text:
self.status_message.emit("請輸入 IP 和 Port", 3000)
return
try:
port = int(port_text)
if port < 1 or port > 65535:
raise ValueError("Port 超出範圍")
except ValueError:
self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
return
# 檢查是否已存在相同連接
for conn in self.udp_connections:
if conn['ip'] == ip and conn['port'] == port:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.udp_connection_added.emit(ip, port)
# 清空輸入框
self.udp_ip_input.clear()
self.udp_port_input.clear()
def _handle_add_ws(self):
"""處理添加 WebSocket 連接"""
input_url = self.ws_url_input.text().strip()
if not input_url:
self.status_message.emit("請輸入 WebSocket URL", 3000)
return
# 自動添加 ws:// 前綴
if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
url = f'ws://{input_url}'
else:
url = input_url
# 基本 URL 格式驗證
try:
if '://' in url:
parts = url.split('://', 1)
if len(parts) == 2 and ':' not in parts[1]:
self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
return
except:
self.status_message.emit("URL 格式錯誤", 3000)
return
# 檢查是否已存在相同連接
for conn in self.ws_connections:
if conn['url'] == url:
self.status_message.emit("連接已存在", 3000)
return
# 發送信號通知主窗口
self.ws_connection_added.emit(url)
# 清空輸入框
self.ws_url_input.clear()
def create_udp_connection_panel(self, conn):
"""創建 UDP 連接面板"""
panel = QWidget()
panel.setStyleSheet("""
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
padding: 8px;
border: 1px solid #444;
}
""")
layout = QHBoxLayout(panel)
layout.setContentsMargins(8, 8, 8, 8)
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
info_label.setStyleSheet("color: #DDD; font-size: 12px;")
# 狀態指示器
status_label = QLabel("●")
if conn.get('enabled', False):
status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
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))
toggle_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #555; }
""")
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
remove_btn.setStyleSheet("""
QPushButton {
background-color: #d32f2f;
color: white;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #b71c1c; }
""")
layout.addWidget(status_label)
layout.addWidget(info_label)
layout.addStretch()
layout.addWidget(toggle_btn)
layout.addWidget(remove_btn)
# 儲存引用
panel.connection = conn
panel.toggle_btn = toggle_btn
panel.status_label = status_label
return panel
def create_ws_connection_panel(self, conn):
"""創建 WebSocket 連接面板"""
panel = QWidget()
panel.setStyleSheet("""
QWidget {
background-color: #2A2A2A;
border-radius: 6px;
padding: 8px;
border: 1px solid #444;
}
""")
layout = QHBoxLayout(panel)
layout.setContentsMargins(8, 8, 8, 8)
# 連接資訊
info_label = QLabel(f"{conn['name']} - {conn['url']}")
info_label.setStyleSheet("color: #DDD; font-size: 12px;")
# 狀態指示器
status_label = QLabel("●")
if conn.get('enabled', False):
status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
status_label.setToolTip("運行中")
else:
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))
toggle_btn.setStyleSheet("""
QPushButton {
background-color: #444;
color: #DDD;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #555; }
""")
remove_btn = QPushButton("移除")
remove_btn.setFixedWidth(60)
remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
remove_btn.setStyleSheet("""
QPushButton {
background-color: #d32f2f;
color: white;
border: none;
padding: 4px 8px;
border-radius: 3px;
font-size: 11px;
}
QPushButton:hover { background-color: #b71c1c; }
""")
layout.addWidget(status_label)
layout.addWidget(info_label)
layout.addStretch()
layout.addWidget(toggle_btn)
layout.addWidget(remove_btn)
# 儲存引用
panel.connection = conn
panel.toggle_btn = toggle_btn
panel.status_label = status_label
return panel
def add_udp_panel(self, conn):
"""添加 UDP 連接面板到列表"""
panel = self.create_udp_connection_panel(conn)
self.udp_list_layout.addWidget(panel)
self.udp_connections.append(conn)
return panel
def add_ws_panel(self, conn):
"""添加 WebSocket 連接面板到列表"""
panel = self.create_ws_connection_panel(conn)
self.ws_list_layout.addWidget(panel)
self.ws_connections.append(conn)
return panel
def remove_udp_connection_from_list(self, conn):
"""從列表中移除 UDP 連接"""
if conn in self.udp_connections:
self.udp_connections.remove(conn)
def remove_ws_connection_from_list(self, conn):
"""從列表中移除 WebSocket 連接"""
if conn in self.ws_connections:
self.ws_connections.remove(conn)

646
src/unitdev01/unitdev01/communication.py
Unescape Escape View File

@ -0,0 +1,646 @@
from rclpy.node import Node
from PyQt6.QtCore import QObject, pyqtSignal
import math
import re
import threading
from threading import Lock
import asyncio
import websockets
import json
import socket
from pymavlink import mavutil
from geometry_msgs.msg import Point, Vector3
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
from std_msgs.msg import Float64
from mavros_msgs.msg import State, VfrHud
from mavros_msgs.srv import CommandBool, CommandTOL
class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
class UDPMavlinkReceiver(threading.Thread):
"""UDP MAVLink 接收器"""
def __init__(self, ip, port, signals, connection_name):
super().__init__(daemon=True)
self.ip = ip
self.port = port
self.signals = signals
self.connection_name = connection_name
self.running = False
self.sock = None
def run(self):
"""執行 UDP 接收循環"""
self.running = True
try:
print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
# 創建 MAVLink 連接
mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
while self.running:
try:
msg = mav.recv_match(blocking=True, timeout=1.0)
if msg is None:
continue
self.process_mavlink_message(msg)
except socket.timeout:
continue
except Exception as e:
print(f"Error receiving MAVLink message: {e}")
except Exception as e:
print(f"UDP receiver error: {e}")
finally:
if self.sock:
self.sock.close()
def process_mavlink_message(self, msg):
"""處理 MAVLink 訊息"""
try:
msg_type = msg.get_type()
system_id = msg.get_srcSystem()
drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
armed = bool(msg.base_mode & 128)
self.signals.update_signal.emit('state', drone_id, {
'mode': mode,
'armed': armed
})
elif msg_type == "BATTERY_STATUS":
voltage = msg.voltages[0] / 1000
self.signals.update_signal.emit('battery', drone_id, {
'voltage': voltage
})
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
longitude = msg.lon / 1e7
self.signals.update_signal.emit('gps', drone_id, {
'lat': latitude,
'lon': longitude
})
elif msg_type == "GPS_RAW_INT":
fix_type = msg.fix_type
elif msg_type == "LOCAL_POSITION_NED":
x = msg.y
y = msg.x
z = -msg.z
self.signals.update_signal.emit('local_pose', drone_id, {
'x': x,
'y': y,
'z': z
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': z
})
self.signals.update_signal.emit('velocity', drone_id, {
'vx': msg.vx,
'vy': msg.vy,
'vz': msg.vz
})
elif msg_type == "ATTITUDE":
pitch = math.degrees(msg.pitch)
self.signals.update_signal.emit('attitude', drone_id, {
'pitch': pitch,
'roll': 0,
'yaw': 0,
'rates': (0, 0, 0)
})
elif msg_type == "VFR_HUD":
groundspeed = msg.groundspeed
heading = msg.heading
self.signals.update_signal.emit('hud', drone_id, {
'heading': heading,
'groundspeed': groundspeed
})
except Exception as e:
print(f"Error processing MAVLink message: {e}")
def stop(self):
"""停止接收器"""
self.running = False
class SerialMavlinkReceiver(threading.Thread):
"""串口 MAVLink 接收器"""
def __init__(self, port, baudrate, signals, connection_name):
super().__init__(daemon=True)
self.port = port
self.baudrate = baudrate
self.signals = signals
self.connection_name = connection_name
self.running = False
self.mav = None
def run(self):
"""執行串口接收循環"""
self.running = True
try:
print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
# 創建 MAVLink 串口連接
self.mav = mavutil.mavlink_connection(
self.port,
baud=self.baudrate,
source_system=255
)
print(f"Waiting for heartbeat from {self.port}...")
self.mav.wait_heartbeat()
print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
while self.running:
try:
msg = self.mav.recv_match(blocking=True, timeout=1.0)
if msg is None:
continue
self.process_mavlink_message(msg)
except Exception as e:
if self.running:
print(f"Error receiving MAVLink message from serial: {e}")
except Exception as e:
print(f"Serial receiver error: {e}")
finally:
if self.mav:
try:
self.mav.close()
except:
pass
def process_mavlink_message(self, msg):
"""處理 MAVLink 訊息"""
try:
msg_type = msg.get_type()
system_id = msg.get_srcSystem()
drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
armed = bool(msg.base_mode & 128)
self.signals.update_signal.emit('state', drone_id, {
'mode': mode,
'armed': armed
})
elif msg_type == "BATTERY_STATUS":
voltage = msg.voltages[0] / 1000
self.signals.update_signal.emit('battery', drone_id, {
'voltage': voltage
})
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
longitude = msg.lon / 1e7
self.signals.update_signal.emit('gps', drone_id, {
'lat': latitude,
'lon': longitude
})
elif msg_type == "GPS_RAW_INT":
fix_type = msg.fix_type
elif msg_type == "LOCAL_POSITION_NED":
x = msg.y
y = msg.x
z = -msg.z
self.signals.update_signal.emit('local_pose', drone_id, {
'x': x,
'y': y,
'z': z
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': z
})
self.signals.update_signal.emit('velocity', drone_id, {
'vx': msg.vx,
'vy': msg.vy,
'vz': msg.vz
})
elif msg_type == "ATTITUDE":
pitch = math.degrees(msg.pitch)
self.signals.update_signal.emit('attitude', drone_id, {
'pitch': pitch,
'roll': 0,
'yaw': 0,
'rates': (0, 0, 0)
})
elif msg_type == "VFR_HUD":
groundspeed = msg.groundspeed
heading = msg.heading
self.signals.update_signal.emit('hud', drone_id, {
'heading': heading,
'groundspeed': groundspeed
})
except Exception as e:
print(f"Error processing MAVLink message from serial: {e}")
def stop(self):
"""停止接收器"""
self.running = False
class WebSocketMavlinkReceiver(threading.Thread):
"""WebSocket MAVLink 接收器"""
def __init__(self, url, signals, connection_name):
super().__init__(daemon=True)
self.url = url
self.signals = signals
self.connection_name = connection_name
self.running = False
self.max_retries = 5
self.base_delay = 1.0
def run(self):
"""執行 WebSocket 接收循環"""
self.running = True
asyncio.set_event_loop(asyncio.new_event_loop())
asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
async def ws_client_loop(self):
"""WebSocket 連接的主循環"""
retry_count = 0
print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
while self.running and retry_count < self.max_retries:
try:
async with websockets.connect(self.url) as websocket:
print(f"WebSocket {self.connection_name} connected to {self.url}")
retry_count = 0 # 重置重試計數
async for message in websocket:
if not self.running:
break
try:
data = json.loads(message)
data['_connection_source'] = self.connection_name
self.process_websocket_message(data)
except json.JSONDecodeError as e:
print(f"WebSocket {self.connection_name} JSON decode error: {e}")
except Exception as e:
print(f"WebSocket {self.connection_name} message processing error: {e}")
except websockets.exceptions.ConnectionClosedError:
print(f"WebSocket {self.connection_name} connection closed")
if self.running:
retry_count += 1
if retry_count < self.max_retries:
delay = self.base_delay * (2 ** min(retry_count, 4))
print(f"Reconnecting in {delay}s...")
await asyncio.sleep(delay)
else:
break
except Exception as e:
retry_count += 1
if retry_count < self.max_retries and self.running:
delay = self.base_delay * (2 ** min(retry_count, 4))
print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
await asyncio.sleep(delay)
else:
break
print(f"WebSocket client {self.connection_name} stopped")
def process_websocket_message(self, data):
"""處理 WebSocket 訊息"""
try:
drone_id = data.get('system_id')
drone_id = f"s9_{drone_id}" if drone_id else None
if not drone_id:
return
# 模式
if 'mode' in data:
self.signals.update_signal.emit('state', drone_id, {
'mode': data['mode'],
})
# 電池
if 'battery' in data:
self.signals.update_signal.emit('battery', drone_id, {
'percentage': data['battery']
})
# 位置
if 'position' in data:
pos = data['position']
self.signals.update_signal.emit('gps', drone_id, {
'lat': pos.get('lat', 0),
'lon': pos.get('lon', 0)
})
# Local position - 設定 x, y 為 0.0
self.signals.update_signal.emit('local_pose', drone_id, {
'x': 0.0,
'y': 0.0,
'z': 0.0
})
# Altitude - 設定為 0.0
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': 0.0
})
# 航向
if 'heading' in data:
self.signals.update_signal.emit('hud', drone_id, {
'heading': data['heading'],
'groundspeed': 0.0
})
except Exception as e:
print(f"WebSocket message processing error: {e}")
def stop(self):
"""停止接收器"""
self.running = False
class DroneMonitor(Node):
def __init__(self):
super().__init__('drone_monitor')
self.signals = DroneSignals()
self.drone_topics = {}
self.lock = Lock()
self.arm_clients = {}
self.takeoff_clients = {}
self.setpoint_pubs = {}
self.selected_drones = set()
self.latest_data = {}
# 定義需要過濾的模式
self.filtered_modes = ['Mode(0x000000c0)']
# WebSocket 接收器列表
self.ws_receivers = []
# 串口接收器列表
self.serial_receivers = []
# 主题检测定时器
self.create_timer(1.0, self.scan_topics)
def scan_topics(self):
topics = self.get_topic_names_and_types()
drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
found_drones = set()
for topic_name, _ in topics:
if match := drone_pattern.match(topic_name):
drone_id, topic_type = match.groups()
found_drones.add(drone_id)
with self.lock:
self.drone_topics.setdefault(drone_id, set()).add(topic_type)
for drone_id in found_drones:
if not hasattr(self, f'drone_{drone_id}_subs'):
self.setup_drone(drone_id)
def setup_drone(self, drone_id):
base_topic = f'/MavLinkBus/{drone_id}'
# Add service clients
self.arm_clients[drone_id] = self.create_client(
CommandBool,
f'{base_topic}/cmd/arming'
)
self.takeoff_clients[drone_id] = self.create_client(
CommandTOL,
f'{base_topic}/cmd/takeoff'
)
# Add setpoint publisher
self.setpoint_pubs[drone_id] = self.create_publisher(
Point,
f'{base_topic}/setpoint_position/local',
10
)
subs = {
'attitude': self.create_subscription(
Imu,
f'{base_topic}/attitude',
lambda msg, did=drone_id: self.attitude_callback(did, msg),
10
),
'battery': self.create_subscription(
BatteryState,
f'{base_topic}/battery',
lambda msg, did=drone_id: self.battery_callback(did, msg),
10
),
'global': self.create_subscription(
NavSatFix,
f'{base_topic}/global_position/global',
lambda msg, did=drone_id: self.gps_callback(did, msg),
10
),
'rel_alt': self.create_subscription(
Float64,
f'{base_topic}/global_position/rel_alt',
lambda msg, did=drone_id: self.altitude_callback(did, msg),
10
),
'local_pose': self.create_subscription(
Point,
f'{base_topic}/local_position/pose',
lambda msg, did=drone_id: self.local_pose_callback(did, msg),
10
),
'local_vel': self.create_subscription(
Vector3,
f'{base_topic}/local_position/velocity',
lambda msg, did=drone_id: self.local_vel_callback(did, msg),
10
),
'loss_rate': self.create_subscription(
Float64,
f'{base_topic}/packet_loss_rate',
lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
10
),
'state': self.create_subscription(
State,
f'{base_topic}/state',
lambda msg, did=drone_id: self.state_callback(did, msg),
10
),
'ping': self.create_subscription(
Float64,
f'{base_topic}/ping',
lambda msg, did=drone_id: self.ping_callback(did, msg),
10
),
'vfr_hud': self.create_subscription(
VfrHud,
f'{base_topic}/vfr_hud',
lambda msg, did=drone_id: self.hud_callback(did, msg),
10
)
}
setattr(self, f'drone_{drone_id}_subs', subs)
async def arm_drone(self, drone_id, arm):
if drone_id not in self.arm_clients:
return False
client = self.arm_clients[drone_id]
if not client.wait_for_service(timeout_sec=1.0):
return False
request = CommandBool.Request()
request.value = arm
future = client.call_async(request)
try:
response = await future
return response.success
except Exception as e:
self.get_logger().error(f'Arm service call failed: {e}')
return False
async def takeoff_drone(self, drone_id, altitude=10.0):
if drone_id not in self.takeoff_clients:
return False
client = self.takeoff_clients[drone_id]
if not client.wait_for_service(timeout_sec=1.0):
return False
request = CommandTOL.Request()
request.altitude = altitude
request.min_pitch = 0.0
request.yaw = 0.0
future = client.call_async(request)
try:
response = await future
return response.success
except Exception as e:
self.get_logger().error(f'Takeoff service call failed: {e}')
return False
def send_setpoint(self, drone_id, x, y, z):
"""Send setpoint position command"""
if drone_id not in self.setpoint_pubs:
return False
msg = Point()
msg.x = float(x)
msg.y = float(y)
msg.z = float(z)
self.setpoint_pubs[drone_id].publish(msg)
return True
def quaternion_to_euler(self, q):
sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
roll = math.atan2(sinr_cosp, cosr_cosp)
sinp = 2 * (q.w * q.y - q.z * q.x)
pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
yaw = math.atan2(siny_cosp, cosy_cosp)
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
# callbacks
def attitude_callback(self, drone_id, msg):
if hasattr(msg, 'orientation'):
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
self.latest_data[(drone_id, 'attitude')] = {
'roll': roll,
'pitch': pitch,
'yaw': yaw,
'rates': (msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z)
}
def battery_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'battery')] = {
'voltage': msg.voltage
}
def state_callback(self, drone_id, msg):
mode = msg.mode
if mode in self.filtered_modes:
return
self.latest_data[(drone_id, 'state')] = {
'mode': msg.mode,
'armed': msg.armed
}
def gps_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'gps')] = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
}
def local_vel_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'velocity')] = {
'vx': msg.x,
'vy': msg.y,
'vz': msg.z
}
def altitude_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'altitude')] = {
'altitude': msg.data
}
def local_pose_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'local_pose')] = {
'x': msg.x,
'y': msg.y,
'z': msg.z
}
def hud_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'hud')] = {
'airspeed': msg.airspeed,
'groundspeed': msg.groundspeed,
'heading': msg.heading,
'throttle': msg.throttle,
'alt': msg.altitude,
'climb': msg.climb
}
def loss_rate_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'loss_rate')] = {
'loss_rate': msg.data
}
def ping_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'ping')] = {
'ping': msg.data
}
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
"""啟動串口 MAVLink 連接"""
connection_name = f"Serial_{port.replace('/', '_')}"
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name)
receiver.start()
self.serial_receivers.append(receiver)
print(f"Started serial connection on {port} at {baudrate} baud")
return receiver

378
src/unitdev01/unitdev01/drone_panel.py
Unescape Escape View File

@ -0,0 +1,378 @@
#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
QPushButton, QSizePolicy, QCheckBox)
from PyQt6.QtCore import pyqtSignal
class DronePanel(QWidget):
"""單個無人機面板類別"""
# 定義信號
mode_change_requested = pyqtSignal(str) # drone_id
arm_requested = pyqtSignal(str) # drone_id
takeoff_requested = pyqtSignal(str) # drone_id
setpoint_requested = pyqtSignal(str) # drone_id
selection_changed = pyqtSignal(str, int) # drone_id, state
def __init__(self, drone_id, parent=None):
super().__init__(parent)
self.drone_id = drone_id
self.display_id = 's' + drone_id.split('_')[1]
self._init_ui()
def _init_ui(self):
"""初始化UI"""
self.setObjectName(f"panel_{self.drone_id}")
self.setFixedHeight(140)
self.setStyleSheet("""
background-color: #2A2A2A;
border-radius: 8px;
""")
# 主佈局
main_layout = QHBoxLayout(self)
main_layout.setContentsMargins(8, 8, 8, 8)
main_layout.setSpacing(0)
# 創建內容容器（包含 info 和 control）
content_widget = QWidget()
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)
# 左側資訊區域
info_widget = self._create_info_section()
# 右側控制按鈕區域
control_widget = self._create_control_section()
# 將 info 和 control 加入內容容器
content_layout.addWidget(info_widget)
content_layout.addWidget(control_widget)
# 將內容容器加入主佈局
main_layout.addWidget(content_widget)
def _create_info_section(self):
"""創建資訊顯示區域"""
info_widget = QWidget()
info_layout = QVBoxLayout(info_widget)
info_layout.setContentsMargins(0, 0, 0, 0)
info_layout.setSpacing(4)
# 頂部標題欄
header = QWidget()
header_layout = QHBoxLayout(header)
header_layout.setContentsMargins(0, 0, 0, 0)
# 勾選框
self.checkbox = QCheckBox()
self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }")
self.checkbox.stateChanged.connect(
lambda state: self.selection_changed.emit(self.drone_id, state)
)
# ID 顯示
id_label = QLabel(self.display_id)
id_label.setStyleSheet("""
font-weight: bold;
font-size: 14px;
color: #7FFFD4;
min-width: 80px;
""")
header_layout.addWidget(self.checkbox)
header_layout.addWidget(id_label)
header_layout.addStretch()
info_layout.addWidget(header)
# 第一行：狀態 (模式 + ARM狀態)
status_row = self._create_status_row()
info_layout.addWidget(status_row)
# 第二行：電池
battery_row = self._create_battery_row()
info_layout.addWidget(battery_row)
# 第三行：位置 + 高度
position_row = self._create_position_row()
info_layout.addWidget(position_row)
# 第四行：航向 + 速度
nav_row = self._create_nav_row()
info_layout.addWidget(nav_row)
return info_widget
def _create_status_row(self):
"""創建狀態行"""
status_row = QWidget()
status_layout = QHBoxLayout(status_row)
status_layout.setContentsMargins(0, 0, 0, 0)
status_title = QLabel("狀態：")
status_title.setStyleSheet("color: #888; min-width: 50px;")
self.mode_label = QLabel("--")
self.mode_label.setObjectName(f"{self.drone_id}_mode")
self.mode_label.setStyleSheet("color: #DDD;")
self.armed_label = QLabel("--")
self.armed_label.setObjectName(f"{self.drone_id}_armed")
self.armed_label.setStyleSheet("color: #DDD;")
status_layout.addWidget(status_title)
status_layout.addWidget(self.mode_label)
status_layout.addWidget(self.armed_label)
status_layout.addStretch()
return status_row
def _create_battery_row(self):
"""創建電池行"""
battery_row = QWidget()
battery_layout = QHBoxLayout(battery_row)
battery_layout.setContentsMargins(0, 0, 0, 0)
battery_title = QLabel("電池：")
battery_title.setStyleSheet("color: #888; min-width: 50px;")
self.battery_label = QLabel("--")
self.battery_label.setObjectName(f"{self.drone_id}_battery")
self.battery_label.setStyleSheet("color: #DDD;")
battery_layout.addWidget(battery_title)
battery_layout.addWidget(self.battery_label)
battery_layout.addStretch()
return battery_row
def _create_position_row(self):
"""創建位置行"""
position_row = QWidget()
position_layout = QHBoxLayout(position_row)
position_layout.setContentsMargins(0, 0, 0, 0)
position_title = QLabel("位置：")
position_title.setStyleSheet("color: #888; min-width: 50px;")
self.local_label = QLabel("--")
self.local_label.setObjectName(f"{self.drone_id}_local")
self.local_label.setStyleSheet("color: #DDD;")
altitude_title = QLabel("高度：")
altitude_title.setStyleSheet("color: #888; margin-left: 10px;")
self.altitude_label = QLabel("--")
self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
self.altitude_label.setStyleSheet("color: #DDD;")
position_layout.addWidget(position_title)
position_layout.addWidget(self.local_label)
position_layout.addWidget(altitude_title)
position_layout.addWidget(self.altitude_label)
position_layout.addStretch()
return position_row
def _create_nav_row(self):
"""創建導航行"""
nav_row = QWidget()
nav_layout = QHBoxLayout(nav_row)
nav_layout.setContentsMargins(0, 0, 0, 0)
heading_title = QLabel("航向：")
heading_title.setStyleSheet("color: #888; min-width: 50px;")
self.heading_label = QLabel("--")
self.heading_label.setObjectName(f"{self.drone_id}_heading")
self.heading_label.setStyleSheet("color: #DDD;")
speed_title = QLabel("速度：")
speed_title.setStyleSheet("color: #888; margin-left: 10px;")
self.groundspeed_label = QLabel("--")
self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed")
self.groundspeed_label.setStyleSheet("color: #DDD;")
nav_layout.addWidget(heading_title)
nav_layout.addWidget(self.heading_label)
nav_layout.addWidget(speed_title)
nav_layout.addWidget(self.groundspeed_label)
nav_layout.addStretch()
return nav_row
def _create_control_section(self):
"""創建控制按鈕區域"""
control_widget = QWidget()
control_layout = QVBoxLayout(control_widget)
control_layout.setContentsMargins(0, 0, 0, 0)
control_layout.setSpacing(6)
control_widget.setFixedWidth(80)
btn_style = """
QPushButton {
background-color: #444;
color: #DDD;
border: none;
border-radius: 4px;
font-size: 11px;
}
QPushButton:hover {
background-color: #555;
}
"""
# 模式切換按鈕
mode_btn = QPushButton("切換模式")
mode_btn.setStyleSheet(btn_style)
mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id))
# 解鎖按鈕
arm_btn = QPushButton("解鎖")
arm_btn.setStyleSheet(btn_style)
arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id))
# 起飛按鈕
takeoff_btn = QPushButton("起飛")
takeoff_btn.setStyleSheet(btn_style)
takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id))
# Setpoint 按鈕
setpoint_btn = QPushButton("Setpoint")
setpoint_btn.setStyleSheet(btn_style)
setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id))
control_layout.addWidget(mode_btn)
control_layout.addWidget(arm_btn)
control_layout.addWidget(takeoff_btn)
control_layout.addWidget(setpoint_btn)
return control_widget
def update_field(self, field, text, color=None):
"""更新指定欄位的值"""
label = self.findChild(QLabel, f"{self.drone_id}_{field}")
if label and label.text() != text:
label.setText(text)
if color:
label.setStyleSheet(f"color: {color};")
def get_checkbox(self):
"""獲取勾選框"""
return self.checkbox
def set_checked(self, checked):
"""設置勾選狀態"""
self.checkbox.setChecked(checked)
def is_checked(self):
"""獲取勾選狀態"""
return self.checkbox.isChecked()
class SocketGroupPanel(QWidget):
# 定義信號
group_selection_changed = pyqtSignal(str, int) # socket_id, state
def __init__(self, socket_id, color='#AAAAAA', parent=None):
super().__init__(parent)
self.socket_id = socket_id
self.color = color
self._init_ui()
def _init_ui(self):
"""初始化UI"""
self.setObjectName(f"socket_group_{self.socket_id}")
self.setStyleSheet("""
background-color: #1E1E1E;
border-radius: 12px;
""")
layout = QVBoxLayout(self)
layout.setContentsMargins(10, 10, 10, 10)
layout.setSpacing(6)
# Socket 分組標題行 - 包含勾選框
title_row = QWidget()
title_layout = QHBoxLayout(title_row)
title_layout.setContentsMargins(0, 0, 0, 0)
# 分組勾選框
self.group_checkbox = QCheckBox()
self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
self.group_checkbox.setStyleSheet(f"""
QCheckBox {{ color: #DDD; }}
QCheckBox::indicator {{
width: 14px;
height: 14px;
border: 2px solid #888888;
border-radius: 3px;
background: transparent;
}}
QCheckBox::indicator:checked {{
background-color: {self.color};
border: 2px solid #888888;
}}
QCheckBox::indicator:indeterminate {{
background-color: #666;
border: 2px solid #888888;
}}
""")
self.group_checkbox.stateChanged.connect(
lambda state: self.group_selection_changed.emit(self.socket_id, state)
)
# Socket 分組標題
title_label = QLabel(f"Socket {self.socket_id}")
title_label.setStyleSheet(f"""
font-weight: bold;
font-size: 16px;
color: {self.color};
margin-bottom: 8px;
padding: 4px 8px;
border-radius: 6px;
""")
title_layout.addWidget(self.group_checkbox)
title_layout.addWidget(title_label)
title_layout.addStretch()
layout.addWidget(title_row)
# 創建子容器用於放置該 socket 下的所有無人機面板
self.drones_container = QWidget()
self.drones_layout = QVBoxLayout(self.drones_container)
self.drones_layout.setContentsMargins(0, 0, 0, 0)
self.drones_layout.setSpacing(4)
layout.addWidget(self.drones_container)
def add_drone_panel(self, panel):
"""添加無人機面板到分組"""
self.drones_layout.addWidget(panel)
def clear_drones(self):
"""清空所有無人機面板"""
while self.drones_layout.count():
item = self.drones_layout.takeAt(0)
if item.widget():
item.widget().setParent(None)
def get_checkbox(self):
"""獲取分組勾選框"""
return self.group_checkbox
def set_checked(self, checked):
"""設置分組勾選狀態"""
self.group_checkbox.setChecked(checked)
def set_check_state(self, state):
"""設置分組勾選狀態（支持半選）"""
self.group_checkbox.setCheckState(state)

1112
src/unitdev01/unitdev01/gui.py
View File

File diff suppressed because it is too large Load Diff

292
src/unitdev01/unitdev01/map_layout.py
Unescape Escape View File

@ -0,0 +1,292 @@
#!/usr/bin/env python3
from PyQt6.QtWebEngineWidgets import QWebEngineView
from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
from PyQt6.QtWebChannel import QWebChannel
class DroneMap:
"""無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
def __init__(self):
"""初始化地圖"""
self.map_view = QWebEngineView()
self.map_loaded = False
self.pending_map_updates = {}
# 創建橋接對象
self.bridge = MapBridge()
# 設置 QWebChannel
self.channel = QWebChannel()
self.channel.registerObject('bridge', self.bridge)
self.map_view.page().setWebChannel(self.channel)
# 設置地圖 HTML
inline_html = '''
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<link rel="stylesheet" href="https://unpkg.com/leaflet/dist/leaflet.css" />
<script src="https://unpkg.com/leaflet/dist/leaflet.js"></script>
<script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
<script src="qrc:///qtwebchannel/qwebchannel.js"></script>
<style>
html, body, #map { height: 100%; margin: 0; }
.map-controls {
position: absolute;
top: 10px;
right: 10px;
z-index: 1000;
}
.control-button {
padding: 8px 12px;
background-color: #f44336;
color: white;
border: none;
border-radius: 4px;
cursor: pointer;
font-size: 12px;
box-shadow: 0 2px 5px rgba(0,0,0,0.2);
}
.control-button:hover {
background-color: #d32f2f;
}
</style>
</head>
<body>
<div id="map"></div>
<div class="map-controls">
<button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
</div>
<script>
var bridge;
new QWebChannel(qt.webChannelTransport, function(channel) {
bridge = channel.objects.bridge;
});
var map = L.map('map').setView([0, 0], 19);
L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
maxZoom: 19,
attribution: '© OpenStreetMap contributors'
}).addTo(map);
// 地圖點擊事件
map.on('click', function(e) {
if (bridge) {
bridge.emitGpsSignal(e.latlng.lat, e.latlng.lng);
console.log('點擊位置:', e.latlng.lat, e.latlng.lng);
}
});
var arrowIcon = L.icon({
iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png',
iconSize: [40, 40],
iconAnchor: [20, 20]
});
function getColorBySocketId(id) {
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) {
const color = getColorBySocketId(id);
const sysid = id.split('_')[1];
return L.divIcon({
className: 'drone-id',
html: `<div style="
position: relative;
left: 2px;
background-color: ${color};
color: black;
width: 16px;
height: 16px;
border-radius: 50%;
display: flex;
align-items: center;
justify-content: center;
font-weight: bold;
font-size: 10px;
">${sysid}</div>`,
iconSize: [20, 20],
iconAnchor: [10, 10]
});
}
var markers = {};
var idLabels = {};
var trajectories = {};
var trajectoryLines = {};
var focusedId = null;
var initialized = false;
var trajectoryGroup = L.layerGroup().addTo(map);
function initTrajectory(id) {
if (!trajectories[id]) {
trajectories[id] = [];
const color = getColorBySocketId(id);
trajectoryLines[id] = L.polyline([], {
color: color,
weight: 3,
opacity: 0.7,
smoothFactor: 1
}).addTo(trajectoryGroup);
}
}
function addTrajectoryPoint(id, lat, lon) {
initTrajectory(id);
const point = [lat, lon];
trajectories[id].push(point);
if (trajectories[id].length > 1000) {
trajectories[id].shift();
}
trajectoryLines[id].setLatLngs([...trajectories[id]]);
}
function focusOn(id) {
if (!markers[id]) return;
focusedId = id;
var latlng = markers[id].getLatLng();
map.flyTo(latlng, map.getZoom());
}
setInterval(() => {
if (focusedId && markers[focusedId]) {
var latlng = markers[focusedId].getLatLng();
map.panTo(latlng);
}
}, 1000);
function updateDrone(lat, lon, id, heading) {
if (markers[id]) {
const lastPos = markers[id].getLatLng();
const distance = lastPos.distanceTo([lat, lon]);
if (distance > 1) {
addTrajectoryPoint(id, lat, lon);
}
markers[id]
.setLatLng([lat, lon])
.setRotationAngle(heading);
idLabels[id].setLatLng([lat, lon]);
} else {
initTrajectory(id);
addTrajectoryPoint(id, lat, lon);
markers[id] = L.marker([lat, lon], {
icon: arrowIcon,
rotationAngle: heading,
rotationOrigin: 'center'
})
.on('click', function () {
focusOn(id);
})
.addTo(map);
idLabels[id] = L.marker([lat, lon], {
icon: createIdIcon(id),
zIndexOffset: 1000
})
.on('click', function() {
focusOn(id);
})
.addTo(map);
if (!initialized || id < focusedId) {
focusOn(id);
initialized = true;
}
}
}
function clearAllTrajectories() {
trajectories = {};
Object.values(trajectoryLines).forEach(line => {
trajectoryGroup.removeLayer(line);
});
trajectoryLines = {};
console.log('所有軌跡已清除');
}
</script>
</body>
</html>
'''
self.map_view.setHtml(inline_html)
self.map_view.loadFinished.connect(self._on_map_loaded)
# 設置地圖更新計時器
self.map_update_timer = QTimer()
self.map_update_timer.timeout.connect(self.update_map_positions)
self.map_update_timer.start(200) # 每 200ms 更新一次
def _on_map_loaded(self, ok: bool):
"""地圖加載完成回調"""
if ok:
self.map_loaded = True
else:
print("⚠️ 地圖加載失敗")
def update_drone_position(self, drone_id, lat, lon, heading):
"""更新無人機位置（加入待處理隊列）"""
self.pending_map_updates[drone_id] = (lat, lon, heading)
def update_map_positions(self):
"""批量更新地圖上的無人機位置"""
if not self.map_loaded or not self.pending_map_updates:
return
# 批量執行所有待更新的位置
js_commands = []
for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
if js_commands:
combined_js = "\n".join(js_commands)
self.map_view.page().runJavaScript(combined_js)
# 清空待更新緩存
self.pending_map_updates.clear()
def clear_trajectories(self):
"""清除所有軌跡"""
if self.map_loaded:
self.map_view.page().runJavaScript("clearAllTrajectories();")
def focus_on_drone(self, drone_id):
"""聚焦到指定無人機"""
if self.map_loaded:
self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
def get_widget(self):
"""獲取地圖 widget"""
return self.map_view
def get_gps_signal(self):
"""獲取 GPS 信號"""
return self.bridge.gps_signal
class MapBridge(QObject):
"""JavaScript 和 Python 之間的橋接類"""
gps_signal = pyqtSignal(float, float) # lat, lon
def __init__(self):
super().__init__()
@pyqtSlot(float, float)
def emitGpsSignal(self, lat, lon):
"""供 JavaScript 調用的方法"""
self.gps_signal.emit(lat, lon)

98
src/unitdev01/unitdev01/mavlink.py
Unescape Escape View File

@ -21,13 +21,15 @@ class MAVLinkWorker(QThread):
param_signal = pyqtSignal(str, int)
kbps_signal = pyqtSignal(str, float)
def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
def __init__(self, connection_string="udp:127.0.0.1:14560 ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'):
super().__init__()
self.namespaces = ['UAV1', 'UAV2', 'UAV3']
self.connection = mavutil.mavlink_connection(usb, baud=57600)
self.sysids = [1, 5, 10]
self.namespaces = {}
self.namespaces = [f"UAV{sysid}" for sysid in self.sysids]
self.connection = mavutil.mavlink_connection(connection_string, baud=115200)
self.connection.wait_heartbeat()
for sysid in self.namespaces:
sysid = int(sysid.replace('UAV', ''))
for sysid in self.sysids:
self.set_sr_params(sysid)
self.running = True
@ -42,8 +44,8 @@ class MAVLinkWorker(QThread):
self.total_bytes_received = {}
self.throughput_KBps = {}
for namespace in self.namespaces:
self.request_param(namespace, "SR1_EXTRA1")
for sysid in self.sysids:
self.request_param(sysid, "SR1_EXTRA1")
self.connection.mav.timesync_send(
0, #tc1
int(time.time() * 1e9) #ts1
@ -52,7 +54,7 @@ class MAVLinkWorker(QThread):
def run(self):
while self.running:
try:
msg = self.connection.recv_msg()
msg = self.connection.recv_msg() # Debugging line to see received messages
current_time = time.time()
if not msg:
continue
@ -64,7 +66,6 @@ class MAVLinkWorker(QThread):
if msg_type =="BAD_DATA":
continue
if sysid not in self.total_bytes_received:
self.total_bytes_received[sysid] = 0
self.throughput_KBps[sysid] = 0
@ -162,7 +163,7 @@ class MAVLinkWorker(QThread):
elif msg_type == 'PARAM_VALUE':
param_name = "SR1_EXTRA1"
if msg.param_id == param_name:
self.param_signal.emit(namespace, msg.param_value)
self.param_signal.emit(namespace, int(msg.param_value))
except Exception as e:
print(f"Error reading message: {e}")
@ -171,8 +172,11 @@ class MAVLinkWorker(QThread):
self.running = False
self.connection.close()
def get_sysid(self, namespace):
return int(namespace.replace('UAV', ''))
def set_mode(self, namespace, mode):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
if mode == 'STABILIZE':
mode = 0
elif mode == 'AUTO':
@ -188,7 +192,7 @@ class MAVLinkWorker(QThread):
)
def arm(self, namespace, arm):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # Component ID
@ -199,7 +203,7 @@ class MAVLinkWorker(QThread):
)
def takeoff(self, namespace, altitude):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # Component ID
@ -210,7 +214,7 @@ class MAVLinkWorker(QThread):
)
def land(self, namespace):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # Component ID
@ -221,7 +225,7 @@ class MAVLinkWorker(QThread):
)
def set_local_position(self, namespace, x, y, z):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
self.connection.mav.set_position_target_local_ned_send(
0, sysid, 1, # time_boot_ms, sysid, compid
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame
@ -233,7 +237,7 @@ class MAVLinkWorker(QThread):
)
def reboot_drone(self, namespace):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # target_component (一般為1)
@ -244,7 +248,7 @@ class MAVLinkWorker(QThread):
)
def set_param(self, namespace, param_name, value):
sysid = int(namespace.replace('UAV', ''))
sysid = self.get_sysid(namespace)
try:
self.connection.mav.param_set_send(
sysid,
@ -256,8 +260,7 @@ class MAVLinkWorker(QThread):
except Exception as e:
print(f"Failed to set parameter {param_name}: {e}")
def request_param(self, namespace, param_name):
sysid = int(namespace.replace('UAV', ''))
def request_param(self, sysid, param_name):
try:
self.connection.mav.param_request_read_send(
sysid, # 系統 ID
@ -268,43 +271,57 @@ class MAVLinkWorker(QThread):
except Exception as e:
print(f"Failed to set parameter {param_name}: {e}")
'''
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 2, 4]
freqs = [0, 1, 4]
params = {
"SR1_ADSB": freqs[0],
"SR1_EXT_STAT": freqs[1],
"SR1_EXTRA1": freqs[2],
"SR1_EXTRA2": freqs[2],
"SR1_EXTRA2": freqs[1],
"SR1_EXTRA3": freqs[1],
"SR1_POSITION": freqs[1],
"SR1_POSITION": freqs[2],
"SR1_RAW_SENS": freqs[1],
"SR1_RC_CHAN": freqs[1]
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
'''
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 1, 1]
params = {
"SERIAL1_BAUD": 38
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 3, 3]
params = {
"SR1_ADSB": freqs[0],
"SR1_EXT_STAT": freqs[0],
"SR1_EXT_STAT": freqs[1],
"SR1_EXTRA1": freqs[2],
"SR1_EXTRA2": freqs[2],
"SR1_EXTRA3": freqs[0],
"SR1_EXTRA3": freqs[1],
"SR1_POSITION": freqs[1],
"SR1_RAW_SENS": freqs[1],
"SR1_RC_CHAN": freqs[0]
"SR1_RC_CHAN": freqs[1]
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
'''
def init_drone(self, namespace):
sysid = self.get_sysid(namespace)
self.set_local_position(sysid, 0, 0, 0)
class DroneControlApp(QMainWindow):
def __init__(self):
super().__init__()
self.workers = MAVLinkWorker()
self.sysids = self.workers.sysids
self.namespaces = self.workers.namespaces
self.initUI()
@ -438,6 +455,15 @@ class DroneControlApp(QMainWindow):
self.rebootButton.clicked.connect(self.reboot_drone)
main_layout.addWidget(self.rebootButton)
mission_layout = QHBoxLayout()
self.initButton = QPushButton("重設位置")
self.initButton.clicked.connect(self.init_drone)
self.startButton = QPushButton("開始任務")
self.startButton.clicked.connect(self.start_mission)
mission_layout.addWidget(self.initButton)
mission_layout.addWidget(self.startButton)
main_layout.addLayout(mission_layout)
# 設置主佈局
central_widget = QWidget(self)
central_widget.setLayout(main_layout)
@ -563,6 +589,24 @@ class DroneControlApp(QMainWindow):
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def init_drone(self):
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.init_drone(namespace)
print(f"重設位置：{namespace}")
except Exception as e:
QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}")
def start_mission(self):
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.start_mission(namespace)
print(f"開始任務：{namespace}")
except Exception as e:
QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}")
def main():
app = QtWidgets.QApplication(sys.argv)
window = DroneControlApp()

619
src/unitdev01/unitdev01/xbee.py
Unescape Escape View File

@ -1,619 +0,0 @@
import sys
import time
import math
import serial
import struct
from PyQt5 import QtWidgets
from PyQt5.QtCore import QThread, pyqtSignal
from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox
from pymavlink import mavutil
from pymavlink.dialects.v20 import ardupilotmega as mavlink2
class PacketCapture:
def __init__(self):
self.data = bytearray()
def write(self, b):
self.data.extend(b)
return len(b)
class MAVLinkWorker(QThread):
state_signal = pyqtSignal(str, str)
battery_signal = pyqtSignal(str, float)
gps_signal = pyqtSignal(str, float, float)
gps_status_signal = pyqtSignal(str, int, int)
local_position_signal = pyqtSignal(str, float, float, float)
imu_signal = pyqtSignal(str, float)
hdg_signal = pyqtSignal(str, float)
groundspeed_signal = pyqtSignal(str, float)
ping_signal = pyqtSignal(str, float)
loss_signal = pyqtSignal(str, float)
frequency_signal = pyqtSignal(str, float)
param_signal = pyqtSignal(str, int)
kbps_signal = pyqtSignal(str, float)
def __init__(self):
super().__init__()
self.namespaces = ['UAV1', 'UAV2', 'UAV3']
self.connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)
self.connection.wait_heartbeat()
for sysid in self.namespaces:
sysid = int(sysid.replace('UAV', ''))
self.set_sr_params(sysid)
self.running = True
# 用於計算頻率丟包
self.message_count = {}
self.frequency = {}
self.start_time = {}
self.seq_numbers = {}
self.packet_loss_count = {}
self.total_packet_count = {}
self.loss_percentage = {}
self.total_bytes_received = {}
self.throughput_KBps = {}
for namespace in self.namespaces:
self.request_param(namespace, "SR1_EXTRA1")
self.connection.mav.timesync_send(
0, #tc1
int(time.time() * 1e9) #ts1
)
def run(self):
while self.running:
try:
msg = self.connection.recv_msg()
current_time = time.time()
if not msg:
continue
sysid = msg.get_srcSystem()
if sysid == 0:
continue
namespace = f"UAV{sysid}"
msg_type = msg.get_type()
if msg_type =="BAD_DATA":
continue
if sysid not in self.total_bytes_received:
self.total_bytes_received[sysid] = 0
self.throughput_KBps[sysid] = 0
# 計算訊息大小
msg_bytes = msg.get_msgbuf() # 取得封包的 bytes
if msg_bytes:
self.total_bytes_received[sysid] += len(msg_bytes)
# Packet loss calculation
if sysid not in self.seq_numbers:
self.seq_numbers[sysid] = msg.get_seq() # Initialize sequence number tracking
self.packet_loss_count[sysid] = 0
self.total_packet_count[sysid] = 1
else:
current_seq = msg.get_seq()
expected_seq = (self.seq_numbers[sysid] + 1) % 256
self.total_packet_count[sysid] += 1
if current_seq != expected_seq: # Packet(s) lost
lost_packets = (current_seq - expected_seq) % 256
self.packet_loss_count[sysid] += lost_packets
self.total_packet_count[sysid] += lost_packets
self.seq_numbers[sysid] = current_seq
self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100
self.loss_signal.emit(namespace, self.loss_percentage[sysid])
# Frequency calculation
if sysid not in self.message_count:
self.message_count[sysid] = 0
self.start_time[sysid] = current_time
self.message_count[sysid] += 1
# 每隔一段時間更新
elapsed_time = current_time - self.start_time[sysid]
if elapsed_time >= 1:
# 每秒頻率
self.frequency[sysid] = self.message_count[sysid] / elapsed_time
self.frequency_signal.emit(namespace, self.frequency[sysid])
# 發送 timesync request
self.connection.mav.timesync_send(
0, #tc1
int(current_time * 1e9) #ts1
)
#吞吐量
self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time
self.kbps_signal.emit(namespace, self.throughput_KBps[sysid])
self.start_time[sysid] = current_time
self.message_count[sysid] = 0
self.total_bytes_received[sysid] = 0
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
self.state_signal.emit(namespace, mode)
elif msg_type == "BATTERY_STATUS":
voltage = msg.voltages[0] / 1000
self.battery_signal.emit(namespace, voltage)
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
longitude = msg.lon / 1e7
self.gps_signal.emit(namespace, latitude, longitude)
elif msg_type == "GPS_RAW_INT":
fix_type = msg.fix_type
satellites_visible = msg.satellites_visible
self.gps_status_signal.emit(namespace, fix_type, satellites_visible)
elif msg_type == "LOCAL_POSITION_NED":
x = msg.y
y = msg.x
z = -msg.z
self.local_position_signal.emit(namespace, x, y, z)
elif msg_type == "ATTITUDE":
pitch = math.degrees(msg.pitch)
self.imu_signal.emit(namespace, pitch)
elif msg_type == "VFR_HUD":
groundspeed = msg.groundspeed
heading = msg.heading
self.hdg_signal.emit(namespace, heading)
self.groundspeed_signal.emit(namespace, groundspeed)
elif msg_type == "TIMESYNC":
round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6
if(round_trip_time<1e6):
self.ping_signal.emit(namespace, round_trip_time)
elif msg_type == 'PARAM_VALUE':
param_name = "SR1_EXTRA1"
if msg.param_id == param_name:
self.param_signal.emit(namespace, msg.param_value)
except Exception as e:
print(f"Error reading message: {e}")
def stop(self):
self.running = False
self.connection.close()
def build_api_tx_frame(self, data: bytes, frame_id=0x01):
frame_type = 0x10
dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # Broadcast
dest_addr16 = b'\xFF\xFE'
broadcast_radius = 0x00
options = 0x00
frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
frame += dest_addr64 + dest_addr16
frame += struct.pack(">BB", broadcast_radius, options) + data
checksum = 0xFF - (sum(frame) & 0xFF)
return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
def send_command(self, msg):
# Create the packet and send via serial port
PORT = "/dev/ttyUSB0"
BAUD = 57600
ser = serial.Serial(PORT, BAUD)
capture = PacketCapture()
mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1)
mav.version = 2
mav.send(msg)
api_frame = self.build_api_tx_frame(capture.data)
ser.write(api_frame)
print("RAW HEX:", capture.data.hex())
def set_mode(self, namespace, mode):
sysid = int(namespace.replace('UAV', ''))
if mode == 'STABILIZE':
mode = 0
elif mode == 'AUTO':
mode = 3
elif mode == 'GUIDED':
mode = 4
elif mode == 'LOITER':
mode = 5
msg = self.connection.mav.command_long_encode(
target_system=sysid,
target_component=1,
command=mavutil.mavlink.MAV_CMD_DO_SET_MODE,
confirmation=0,
param1=1, # Custom mode enabled
param2=mode,
param3=0, param4=0, param5=0, param6=0, param7=0
)
self.send_command(msg)
def arm(self, namespace, arm):
sysid = int(namespace.replace('UAV', ''))
msg = self.connection.mav.command_long_encode(
target_system=sysid,
target_component=1,
command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
confirmation=0,
param1=1 if arm else 0, # 1 to arm, 0 to disarm
param2=0, param3=0, param4=0, param5=0, param6=0, param7=0
)
self.send_command(msg)
def takeoff(self, namespace, altitude):
sysid = int(namespace.replace('UAV', ''))
msg = self.connection.mav.command_long_encode(
target_system=sysid,
target_component=1,
command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
confirmation=0,
param1=0, param2=0, param3=0, param4=0, param5=0, param6=0,
param7=altitude
)
self.send_command(msg)
def land(self, namespace):
sysid = int(namespace.replace('UAV', ''))
msg = self.connection.mav.command_long_encode(
target_system=sysid,
target_component=1,
command=mavutil.mavlink.MAV_CMD_NAV_LAND,
confirmation=0,
param1=0, param2=0, param3=0, param4=0, param5=0, param6=0,
param7=0
)
self.send_command(msg)
def set_local_position(self, namespace, x, y, z):
sysid = int(namespace.replace('UAV', ''))
msg = self.connection.mav.set_position_target_local_ned_encode(
0, sysid, 1, # time_boot_ms, sysid, compid
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame
0b110111111000, # Mask
y, x, -z, # Position
0, 0, 0, # Velocity
0, 0, 0, # Acceleration
0, 0 # Yaw, yaw_rate
)
self.send_command(msg)
def reboot_drone(self, namespace):
sysid = int(namespace.replace('UAV', ''))
self.connection.mav.command_long_send(
sysid,
1, # target_component (一般為1)
mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, # Reboot 命令
0, # confirmation
1, # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機)
0, 0, 0, 0, 0, 0 # 其餘參數保留
)
def set_param(self, namespace, param_name, value):
sysid = int(namespace.replace('UAV', ''))
try:
self.connection.mav.param_set_send(
sysid,
1,
param_name.encode('utf-8'),
float(value),
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
except Exception as e:
print(f"Failed to set parameter {param_name}: {e}")
def request_param(self, namespace, param_name):
sysid = int(namespace.replace('UAV', ''))
try:
self.connection.mav.param_request_read_send(
sysid, # 系統 ID
1, # 組件 ID
param_name.encode('utf-8'), # 參數名稱
-1 # 參數索引，-1 表示根據名稱請求
)
except Exception as e:
print(f"Failed to set parameter {param_name}: {e}")
'''
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 2, 4]
params = {
"SR1_ADSB": freqs[0],
"SR1_EXT_STAT": freqs[1],
"SR1_EXTRA1": freqs[2],
"SR1_EXTRA2": freqs[2],
"SR1_EXTRA3": freqs[1],
"SR1_POSITION": freqs[1],
"SR1_RAW_SENS": freqs[1],
"SR1_RC_CHAN": freqs[1]
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
'''
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 1, 1]
params = {
"SR1_ADSB": freqs[0],
"SR1_EXT_STAT": freqs[0],
"SR1_EXTRA1": freqs[2],
"SR1_EXTRA2": freqs[2],
"SR1_EXTRA3": freqs[0],
"SR1_POSITION": freqs[1],
"SR1_RAW_SENS": freqs[1],
"SR1_RC_CHAN": freqs[0]
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
class DroneControlApp(QMainWindow):
def __init__(self):
super().__init__()
self.workers = MAVLinkWorker()
self.namespaces = self.workers.namespaces
self.initUI()
# Connect signals to update the UI
self.workers.state_signal.connect(self.update_state)
self.workers.battery_signal.connect(self.update_battery)
self.workers.gps_signal.connect(self.update_gps)
self.workers.gps_status_signal.connect(self.update_gps_status)
self.workers.local_position_signal.connect(self.update_local_position)
self.workers.imu_signal.connect(self.update_imu)
self.workers.hdg_signal.connect(self.update_hdg)
self.workers.groundspeed_signal.connect(self.update_groundspeed)
self.workers.ping_signal.connect(self.update_ping)
self.workers.loss_signal.connect(self.update_loss)
self.workers.frequency_signal.connect(self.update_frequency)
self.workers.param_signal.connect(self.update_param)
self.workers.kbps_signal.connect(self.update_kbps)
self.workers.start()
def initUI(self):
self.setWindowTitle("多無人機控制介面")
self.setGeometry(100, 100, 800, 600)
# 主佈局
main_layout = QVBoxLayout()
# 無人機選擇區域
uav_layout = QHBoxLayout()
self.uav_checkboxes = {}
for namespace in self.namespaces:
checkbox = QCheckBox(namespace)
checkbox.setChecked(True) # 預設勾選
self.uav_checkboxes[namespace] = checkbox
uav_layout.addWidget(checkbox)
main_layout.addLayout(uav_layout)
# 顯示所有無人機資訊，從左到右顯示
uav_layout = QHBoxLayout()
# 逐個顯示 UAV 的資訊
self.uav_labels = {}
for namespace in self.namespaces:
uav_info_layout = QVBoxLayout() # 每台 UAV 的資訊垂直排列
# 狀態顯示
self.uav_labels[namespace] = {
"status": QLabel("狀態：等待數據..."),
"battery": QLabel("電壓：等待數據..."),
"local_position": QLabel("Local Position：等待數據..."),
"gps": QLabel("GPS位置：等待數據...\n\n"),
"fix": QLabel("Fix Type：等待數據..."),
"satellites": QLabel("衛星數量：等待數據..."),
"groundspeed": QLabel("地面速度：等待數據..."),
"pitch": QLabel("Pitch：等待數據..."),
"heading": QLabel("Heading：等待數據..."),
"ping": QLabel("Ping：等待數據..."),
"loss": QLabel("丟包：等待數據..."),
"frequency": QLabel("頻率：等待數據..."),
"kbps": QLabel("吞吐量：等待數據..."),
"param": QLabel("SR1_EXTRA1：等待數據...")
}
# 把每個資訊添加到該 UAV 的垂直佈局中
for label in self.uav_labels[namespace].values():
uav_info_layout.addWidget(label)
# 將該 UAV 的資訊佈局加到主佈局中（從左到右排列）
uav_layout.addLayout(uav_info_layout)
main_layout.addLayout(uav_layout)
# SR1_EXTRA1參數設置
param_layout = QHBoxLayout()
self.paramInput = QLineEdit()
self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值")
self.setParamButton = QPushButton("設置SR1_EXTRA1")
self.setParamButton.clicked.connect(self.set_SR1_EXTRA1)
param_layout.addWidget(self.paramInput)
param_layout.addWidget(self.setParamButton)
main_layout.addLayout(param_layout)
# 模式切換區域
mode_layout = QHBoxLayout()
self.modeComboBox = QComboBox()
self.modeComboBox.addItems(["GUIDED", "STABILIZE", "AUTO", "LOITER"])
self.modeButton = QPushButton("切換模式")
self.modeButton.clicked.connect(self.change_mode)
mode_layout.addWidget(QLabel("選擇模式："))
mode_layout.addWidget(self.modeComboBox)
mode_layout.addWidget(self.modeButton)
main_layout.addLayout(mode_layout)
# 解鎖按鈕
fly_layout = QHBoxLayout()
self.armButton = QPushButton("解鎖")
self.armButton.clicked.connect(self.arm_drone)
# 起飛按鈕
self.takeoffButton = QPushButton("起飛")
self.takeoffButton.clicked.connect(self.takeoff_drone)
# 降落按鈕
self.landButton = QPushButton("降落")
self.landButton.clicked.connect(self.land_drone)
fly_layout.addWidget(self.armButton)
fly_layout.addWidget(self.takeoffButton)
fly_layout.addWidget(self.landButton)
main_layout.addLayout(fly_layout)
# XYZ 設置欄位
xyz_layout = QHBoxLayout()
self.positionX = QLineEdit()
self.positionX.setPlaceholderText("X")
self.positionY = QLineEdit()
self.positionY.setPlaceholderText("Y")
self.positionZ = QLineEdit()
self.positionZ.setPlaceholderText("Z")
self.setPositionButton = QPushButton("設置位置")
self.setPositionButton.clicked.connect(self.set_local_position)
xyz_layout.addWidget(QLabel("輸入位置："))
xyz_layout.addWidget(self.positionX)
xyz_layout.addWidget(self.positionY)
xyz_layout.addWidget(self.positionZ)
xyz_layout.addWidget(self.setPositionButton)
main_layout.addLayout(xyz_layout)
# 設置 XYZ 的按鈕
#設置重啟按鈕
self.rebootButton = QPushButton("重啟")
self.rebootButton.clicked.connect(self.reboot_drone)
main_layout.addWidget(self.rebootButton)
# 設置主佈局
central_widget = QWidget(self)
central_widget.setLayout(main_layout)
self.setCentralWidget(central_widget)
self.show()
def closeEvent(self, event):
try:
self.workers.stop()
finally:
event.accept()
def update_state(self, namespace, mode):
self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
def update_battery(self, namespace, voltage):
self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V")
def update_gps(self, namespace, latitude, longitude):
self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°")
def update_gps_status(self, namespace, fix_type, satellites_visible):
fix_type_str = {
0: "No Fix",
1: "No Fix",
2: "2D Fix",
3: "3D Fix",
4: "RTK Float",
5: "RTK Fix"
}.get(fix_type, "Unknown")
self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}")
self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}")
def update_local_position(self, namespace, x, y, z):
self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})")
def update_imu(self, namespace, pitch):
self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°")
def update_hdg(self, namespace, heading):
self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°")
def update_groundspeed(self, namespace, groundspeed):
self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s")
def update_ping(self, namespace, ping):
self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms")
def update_loss(self, namespace, loss):
self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%")
def update_frequency(self, namespace, frequency):
self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
def update_kbps(self, namespace, kbps):
self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s")
def update_param(self, namespace, value):
self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
def change_mode(self):
try:
selected_mode = self.modeComboBox.currentText()
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.set_mode(namespace, selected_mode)
except Exception as e:
QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}")
def arm_drone(self):
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.arm(namespace, True)
except Exception as e:
QtWidgets.QMessageBox.warning(self, "錯誤", f"解鎖失敗：{e}")
def takeoff_drone(self):
try:
z_text = self.positionZ.text().strip()
z = float(z_text) if z_text else 5.0
h = 2
for i, namespace in enumerate(self.namespaces):
if self.uav_checkboxes[namespace].isChecked():
self.workers.takeoff(namespace, z + h * i)
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def land_drone(self):
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.land(namespace)
def set_local_position(self):
try:
x = float(self.positionX.text().strip())
y = float(self.positionY.text().strip())
z = float(self.positionZ.text().strip())
h = 2
for i, namespace in enumerate(self.namespaces):
if self.uav_checkboxes[namespace].isChecked():
self.workers.set_local_position(namespace, x, y, z + h * i)
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def reboot_drone(self):
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.reboot_drone(namespace)
except Exception as e:
QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}")
def set_SR1_EXTRA1(self):
param_value = self.paramInput.text().strip()
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.set_param(namespace, "SR1_EXTRA1", param_value)
self.workers.request_param(namespace, "SR1_EXTRA1")
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def main():
app = QtWidgets.QApplication(sys.argv)
window = DroneControlApp()
window.show()
sys.exit(app.exec_())
if __name__ == '__main__':
main()
Write Preview
Loading…
Cancel
Save