update gui

chiyu
ken910606 3 months ago
parent 44d63979b5
commit 8fdbbbc5dc

@ -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):
@ -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)

@ -397,6 +397,12 @@ class DroneMonitor(Node):
# ================================================================================
self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
# ================================================================================
# ================================================================================
# 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取)
# ================================================================================
self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'}
# ================================================================================
self.serial_receivers = []
# 主题检测定时器
@ -553,7 +559,13 @@ class DroneMonitor(Node):
}
def battery_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'battery')] = {
# 使用映射獲取實際的 drone_id
actual_drone_id = self.sys_to_actual_id.get(drone_id, None)
# 如果還沒有從 summary 獲取到映射,則不處理
if actual_drone_id is None:
return
self.latest_data[(actual_drone_id, 'battery')] = {
'voltage': msg.voltage
}
@ -580,6 +592,15 @@ class DroneMonitor(Node):
if socket_id is not None and sysid is not None:
# 使用 socket_id 作為前綴
actual_drone_id = f's{socket_id}_{sysid}'
# ================================================================================
# 【關鍵】保存 sys_id 到 actual_drone_id 的映射
# ================================================================================
sys_key = f'sys{sysid}'
self.sys_to_actual_id[sys_key] = actual_drone_id
# 也保存原始的 s0_ 格式到實際 ID 的映射
self.sys_to_actual_id[drone_id] = actual_drone_id
# ================================================================================
else:
actual_drone_id = drone_id
@ -600,7 +621,13 @@ class DroneMonitor(Node):
print(f"Error in summary_callback for {drone_id}: {e}")
def gps_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'gps')] = {
# 使用映射獲取實際的 drone_id
actual_drone_id = self.sys_to_actual_id.get(drone_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
@ -609,7 +636,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
@ -636,7 +663,13 @@ class DroneMonitor(Node):
}
def hud_callback(self, drone_id, msg):
self.latest_data[(drone_id, 'hud')] = {
# 使用映射獲取實際的 drone_id
actual_drone_id = self.sys_to_actual_id.get(drone_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,

@ -44,25 +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,
"longitude": 3,
"latitude": 4,
"altitude": 5,
"local": 6,
"velocity": 7,
"groundspeed": 8,
"heading": 9,
"roll": 10,
"pitch": 11,
"yaw": 12,
"loss_rate": 13,
"ping": 14
}
self.socket_colors = {
'0': '#00BFFF', # 天藍色 (DeepSkyBlue)
@ -86,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 = []
# ================================================================================
# 【新增】初始化任務規劃器
@ -122,17 +104,20 @@ class ControlStationUI(QMainWindow):
self.left_tab.addTab(scroll, "無人載具")
# — 分頁 2Overview Table
self.overview_table = OverviewTable(self.info_types, self.info_type_map)
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, "通訊")
@ -269,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
@ -427,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'])
@ -592,15 +641,45 @@ 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:
@ -948,6 +1027,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):
"""
顯示規劃的航點在終端輸出

@ -214,6 +214,9 @@ class DroneMap:
rotationOrigin: 'center'
})
.on('click', function () {
if (bridge) {
bridge.emitDroneClicked(id);
}
focusOn(id);
})
.addTo(map);
@ -223,6 +226,9 @@ class DroneMap:
zIndexOffset: 1000
})
.on('click', function() {
if (bridge) {
bridge.emitDroneClicked(id);
}
focusOn(id);
})
.addTo(map);
@ -423,9 +429,14 @@ class DroneMap:
"""獲取 GPS 信號"""
return self.bridge.gps_signal
def get_drone_clicked_signal(self):
"""獲取無人機點擊信號"""
return self.bridge.drone_clicked
class MapBridge(QObject):
"""JavaScript 和 Python 之間的橋接類"""
gps_signal = pyqtSignal(float, float) # lat, lon
drone_clicked = pyqtSignal(str) # drone_id
def __init__(self):
super().__init__()
@ -434,3 +445,8 @@ 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)

@ -5,11 +5,38 @@ from PyQt6.QtCore import Qt
class OverviewTable(QTableWidget):
"""總覽表格,顯示所有無人機的狀態資訊"""
def __init__(self, info_types, info_type_map, parent=None):
# 默認的資訊類型和映射
DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向",
"空速", "油門", "HUD高", "爬升率", "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
self.info_type_map = info_type_map
# 使用提供的或默認的資訊類型
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 = {} # 存儲無人機面板的引用
# 初始化表格

Loading…
Cancel
Save