Compare commits
58 Commits
ad87eda1b4
...
7d0368e7ba
Binary file not shown.
@ -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)
|
||||||
@ -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
|
||||||
File diff suppressed because it is too large
Load Diff
@ -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)
|
||||||
@ -0,0 +1,497 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
from pymavlink import mavutil
|
||||||
|
import time
|
||||||
|
import math
|
||||||
|
import threading
|
||||||
|
import sys
|
||||||
|
|
||||||
|
class DroneController:
|
||||||
|
def __init__(self, connection_string="udp:127.0.0.1:14550", drone_id=0):
|
||||||
|
self.master = mavutil.mavlink_connection(connection_string)
|
||||||
|
self.drone_id = drone_id
|
||||||
|
self.connection_string = connection_string
|
||||||
|
self.master.wait_heartbeat()
|
||||||
|
print(f"無人機 #{self.drone_id} 已建立連接!")
|
||||||
|
|
||||||
|
def set_mode(self, mode):
|
||||||
|
if mode == 'STABILIZE':
|
||||||
|
mode_id = 0
|
||||||
|
elif mode == 'AUTO':
|
||||||
|
mode_id = 3
|
||||||
|
elif mode == 'GUIDED':
|
||||||
|
mode_id = 4
|
||||||
|
elif mode == 'LOITER':
|
||||||
|
mode_id = 5
|
||||||
|
elif mode == 'RTL':
|
||||||
|
mode_id = 6
|
||||||
|
elif mode == 'LAND':
|
||||||
|
mode_id = 9
|
||||||
|
else:
|
||||||
|
print(f"不支援的模式: {mode}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
self.master.mav.set_mode_send(
|
||||||
|
self.master.target_system,
|
||||||
|
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
|
||||||
|
mode_id
|
||||||
|
)
|
||||||
|
|
||||||
|
for _ in range(5):
|
||||||
|
msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
|
||||||
|
if msg and msg.custom_mode == mode_id:
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
def arm(self, arm_state=True):
|
||||||
|
self.master.mav.command_long_send(
|
||||||
|
self.master.target_system,
|
||||||
|
self.master.target_component,
|
||||||
|
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||||
|
0,
|
||||||
|
1 if arm_state else 0, 0, 0, 0, 0, 0, 0
|
||||||
|
)
|
||||||
|
|
||||||
|
for _ in range(5):
|
||||||
|
msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
|
||||||
|
if msg:
|
||||||
|
armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
|
||||||
|
if (arm_state and armed) or (not arm_state and not armed):
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
def takeoff(self, altitude):
|
||||||
|
self.master.mav.command_long_send(
|
||||||
|
self.master.target_system,
|
||||||
|
self.master.target_component,
|
||||||
|
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||||
|
0,
|
||||||
|
0, 0, 0, 0, 0, 0,
|
||||||
|
altitude
|
||||||
|
)
|
||||||
|
|
||||||
|
timeout = time.time() + 30
|
||||||
|
while time.time() < timeout:
|
||||||
|
msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
|
||||||
|
if msg:
|
||||||
|
current_alt = msg.relative_alt / 1000.0
|
||||||
|
if abs(current_alt - altitude) < 0.5:
|
||||||
|
return True
|
||||||
|
time.sleep(0.1)
|
||||||
|
return False
|
||||||
|
|
||||||
|
def goto_position_local(self, x, y, z, vx=0, vy=0, vz=0, yaw=0, yaw_rate=0):
|
||||||
|
z = -z
|
||||||
|
|
||||||
|
type_mask = (
|
||||||
|
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
|
||||||
|
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
|
||||||
|
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE
|
||||||
|
)
|
||||||
|
|
||||||
|
print(f"無人機 #{self.drone_id} 發送位置指令: x={x}, y={y}, z={-z}, vx={vx}, vy={vy}, vz={vz}")
|
||||||
|
|
||||||
|
self.master.mav.set_position_target_local_ned_send(
|
||||||
|
0,
|
||||||
|
self.master.target_system,
|
||||||
|
self.master.target_component,
|
||||||
|
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
||||||
|
type_mask,
|
||||||
|
x, y, z,
|
||||||
|
vx, vy, vz,
|
||||||
|
0, 0, 0,
|
||||||
|
yaw, yaw_rate
|
||||||
|
)
|
||||||
|
|
||||||
|
print(f"無人機 #{self.drone_id} 開始監控位置變化...")
|
||||||
|
start_time = time.time()
|
||||||
|
last_progress_time = start_time
|
||||||
|
min_dist = float('inf')
|
||||||
|
|
||||||
|
while time.time() - start_time < 30:
|
||||||
|
msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
|
||||||
|
if msg:
|
||||||
|
dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2)
|
||||||
|
min_dist = min(min_dist, dist)
|
||||||
|
|
||||||
|
current_time = time.time()
|
||||||
|
if current_time - last_progress_time >= 5:
|
||||||
|
if dist > min_dist + 0.1:
|
||||||
|
print(f"無人機 #{self.drone_id} 偏離目標，重新調整")
|
||||||
|
self.master.mav.set_position_target_local_ned_send(
|
||||||
|
0, self.master.target_system, self.master.target_component,
|
||||||
|
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
||||||
|
type_mask,
|
||||||
|
x, y, z, vx, vy, vz, 0, 0, 0, yaw, yaw_rate
|
||||||
|
)
|
||||||
|
last_progress_time = current_time
|
||||||
|
|
||||||
|
print(f"無人機 #{self.drone_id} 當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m")
|
||||||
|
|
||||||
|
if dist < 0.5:
|
||||||
|
print(f"無人機 #{self.drone_id} 到達目標位置")
|
||||||
|
return True
|
||||||
|
|
||||||
|
vel_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=False)
|
||||||
|
if vel_msg:
|
||||||
|
vel_total = math.sqrt(vel_msg.vx**2 + vel_msg.vy**2 + vel_msg.vz**2)
|
||||||
|
if dist < 1.0 and vel_total < 0.1:
|
||||||
|
print(f"無人機 #{self.drone_id} 已穩定懸停在目標位置附近")
|
||||||
|
return True
|
||||||
|
|
||||||
|
time.sleep(0.1)
|
||||||
|
|
||||||
|
print(f"無人機 #{self.drone_id} 移動超時")
|
||||||
|
return False
|
||||||
|
|
||||||
|
def get_current_state(self):
|
||||||
|
state = {}
|
||||||
|
|
||||||
|
# Mode and armed status
|
||||||
|
hb_msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=0.5)
|
||||||
|
if hb_msg:
|
||||||
|
mode_id = hb_msg.custom_mode
|
||||||
|
mode_name = {
|
||||||
|
0: "STABILIZE",
|
||||||
|
3: "AUTO",
|
||||||
|
4: "GUIDED",
|
||||||
|
5: "LOITER",
|
||||||
|
6: "RTL",
|
||||||
|
9: "LAND"
|
||||||
|
}.get(mode_id, f"Unknown({mode_id})")
|
||||||
|
|
||||||
|
state['mode'] = mode_name
|
||||||
|
state['armed'] = bool(hb_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
|
||||||
|
|
||||||
|
# Position
|
||||||
|
pos_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1.0)
|
||||||
|
if pos_msg:
|
||||||
|
state['position'] = {
|
||||||
|
'x': round(pos_msg.x, 2),
|
||||||
|
'y': round(pos_msg.y, 2),
|
||||||
|
'z': round(-pos_msg.z, 2)
|
||||||
|
}
|
||||||
|
|
||||||
|
# GPS
|
||||||
|
gps_msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1.0)
|
||||||
|
if gps_msg:
|
||||||
|
state['gps_position'] = {
|
||||||
|
'lat': gps_msg.lat / 1e7,
|
||||||
|
'lon': gps_msg.lon / 1e7,
|
||||||
|
'alt': gps_msg.alt / 1000,
|
||||||
|
'relative_alt': gps_msg.relative_alt / 1000
|
||||||
|
}
|
||||||
|
|
||||||
|
return state
|
||||||
|
|
||||||
|
class MultiDroneController:
|
||||||
|
def __init__(self, num_drones=5):
|
||||||
|
self.drones = []
|
||||||
|
self.num_drones = num_drones
|
||||||
|
|
||||||
|
for i in range(num_drones):
|
||||||
|
port = 14550 + i
|
||||||
|
connection_string = f"udp:127.0.0.1:{port}"
|
||||||
|
drone = DroneController(connection_string, i)
|
||||||
|
self.drones.append(drone)
|
||||||
|
print(f"已初始化無人機 #{i} 在端口 {port}")
|
||||||
|
|
||||||
|
def get_drone(self, drone_id):
|
||||||
|
if 0 <= drone_id < len(self.drones):
|
||||||
|
return self.drones[drone_id]
|
||||||
|
return None
|
||||||
|
|
||||||
|
def all_drones_command(self, command, *args):
|
||||||
|
results = []
|
||||||
|
threads = []
|
||||||
|
|
||||||
|
for drone in self.drones:
|
||||||
|
thread = threading.Thread(
|
||||||
|
target=lambda d, cmd, args, results: results.append((d.drone_id, getattr(d, cmd)(*args))),
|
||||||
|
args=(drone, command, args, results)
|
||||||
|
)
|
||||||
|
threads.append(thread)
|
||||||
|
thread.start()
|
||||||
|
|
||||||
|
for thread in threads:
|
||||||
|
thread.join()
|
||||||
|
|
||||||
|
for drone_id, success in results:
|
||||||
|
print(f"無人機 #{drone_id} 執行 {command} 結果: {'成功' if success else '失敗'}")
|
||||||
|
|
||||||
|
return all(success for _, success in results)
|
||||||
|
|
||||||
|
def print_menu():
|
||||||
|
menu = [
|
||||||
|
"\n=== 多機無人機控制界面 ===",
|
||||||
|
"1. 選擇操作的無人機",
|
||||||
|
"2. 對所有無人機執行命令",
|
||||||
|
"3. 查看所有無人機狀態",
|
||||||
|
"4. 退出",
|
||||||
|
"0. 顯示此清單",
|
||||||
|
"=========================="
|
||||||
|
]
|
||||||
|
for item in menu:
|
||||||
|
print(item)
|
||||||
|
|
||||||
|
def print_drone_menu():
|
||||||
|
menu = [
|
||||||
|
"\n=== 單機無人機控制界面 ===",
|
||||||
|
"1. 切換至 GUIDED 模式",
|
||||||
|
"2. 切換至 LAND 模式",
|
||||||
|
"3. 解鎖",
|
||||||
|
"4. 上鎖",
|
||||||
|
"5. 起飛",
|
||||||
|
"6. 飛到指定位置",
|
||||||
|
"7. 切換至 RTL 模式(返航)",
|
||||||
|
"8. 查看當前狀態",
|
||||||
|
"9. 返回主菜單",
|
||||||
|
"0. 顯示此清單",
|
||||||
|
"=========================="
|
||||||
|
]
|
||||||
|
for item in menu:
|
||||||
|
print(item)
|
||||||
|
|
||||||
|
def print_all_drones_menu():
|
||||||
|
menu = [
|
||||||
|
"\n=== 所有無人機控制界面 ===",
|
||||||
|
"1. 全部切換至 GUIDED 模式",
|
||||||
|
"2. 全部切換至 LAND 模式",
|
||||||
|
"3. 全部解鎖",
|
||||||
|
"4. 全部上鎖",
|
||||||
|
"5. 全部起飛",
|
||||||
|
"6. 全部返航",
|
||||||
|
"7. 返回主菜單",
|
||||||
|
"0. 顯示此清單",
|
||||||
|
"=========================="
|
||||||
|
]
|
||||||
|
for item in menu:
|
||||||
|
print(item)
|
||||||
|
|
||||||
|
def control_single_drone(drone):
|
||||||
|
print_drone_menu()
|
||||||
|
current_drone_id = drone.drone_id
|
||||||
|
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
choice = input(f"\n無人機 #{current_drone_id} - 請輸入指令編號: ")
|
||||||
|
|
||||||
|
if choice == '0':
|
||||||
|
print_drone_menu()
|
||||||
|
|
||||||
|
elif choice == '1':
|
||||||
|
if drone.set_mode("GUIDED"):
|
||||||
|
print(f"無人機 #{current_drone_id} 已切換至 GUIDED 模式")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 切換模式失敗")
|
||||||
|
|
||||||
|
elif choice == '2':
|
||||||
|
if drone.set_mode("LAND"):
|
||||||
|
print(f"無人機 #{current_drone_id} 已切換至 LAND 模式")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 切換模式失敗")
|
||||||
|
|
||||||
|
elif choice == '3':
|
||||||
|
if drone.arm(True):
|
||||||
|
print(f"無人機 #{current_drone_id} 已解鎖")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 解鎖失敗")
|
||||||
|
|
||||||
|
elif choice == '4':
|
||||||
|
if drone.arm(False):
|
||||||
|
print(f"無人機 #{current_drone_id} 已上鎖")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 上鎖失敗")
|
||||||
|
|
||||||
|
elif choice == '5':
|
||||||
|
altitude = float(input("請輸入起飛高度(公尺): "))
|
||||||
|
if drone.takeoff(altitude):
|
||||||
|
print(f"無人機 #{current_drone_id} 已到達目標高度: {altitude}公尺")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 起飛失敗")
|
||||||
|
|
||||||
|
elif choice == '6':
|
||||||
|
x = float(input("請輸入X座標(公尺): "))
|
||||||
|
y = float(input("請輸入Y座標(公尺): "))
|
||||||
|
z = float(input("請輸入Z座標(公尺,高度為正值): "))
|
||||||
|
vx = float(input("請輸入X方向速度(公尺/秒，可選): ") or "0")
|
||||||
|
vy = float(input("請輸入Y方向速度(公尺/秒，可選): ") or "0")
|
||||||
|
if drone.goto_position_local(x, y, z, vx, vy):
|
||||||
|
print(f"無人機 #{current_drone_id} 已到達目標位置")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 移動失敗")
|
||||||
|
|
||||||
|
elif choice == '7':
|
||||||
|
if drone.set_mode("RTL"):
|
||||||
|
print(f"無人機 #{current_drone_id} 已切換至 RTL 模式，正在返航")
|
||||||
|
else:
|
||||||
|
print(f"無人機 #{current_drone_id} 切換至 RTL 模式失敗")
|
||||||
|
|
||||||
|
elif choice == '8':
|
||||||
|
state = drone.get_current_state()
|
||||||
|
if state:
|
||||||
|
print(f"\n=== 無人機 #{current_drone_id} 狀態 ===")
|
||||||
|
print(f"模式: {state.get('mode', '未知')}")
|
||||||
|
print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
|
||||||
|
|
||||||
|
if 'gps_position' in state:
|
||||||
|
gps = state['gps_position']
|
||||||
|
print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
|
||||||
|
|
||||||
|
if 'position' in state:
|
||||||
|
pos = state['position']
|
||||||
|
print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
|
||||||
|
print("=============================")
|
||||||
|
else:
|
||||||
|
print(f"無法獲取無人機 #{current_drone_id} 狀態")
|
||||||
|
|
||||||
|
elif choice == '9':
|
||||||
|
print("返回主菜單")
|
||||||
|
break
|
||||||
|
|
||||||
|
else:
|
||||||
|
print("無效的指令，請重新輸入")
|
||||||
|
|
||||||
|
except ValueError as e:
|
||||||
|
print(f"輸入錯誤: {e}")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"發生錯誤: {e}")
|
||||||
|
|
||||||
|
def control_all_drones(controller):
|
||||||
|
print_all_drones_menu()
|
||||||
|
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
choice = input("\n請輸入指令編號: ")
|
||||||
|
|
||||||
|
if choice == '0':
|
||||||
|
print_all_drones_menu()
|
||||||
|
|
||||||
|
elif choice == '1':
|
||||||
|
if controller.all_drones_command("set_mode", "GUIDED"):
|
||||||
|
print("所有無人機已切換至 GUIDED 模式")
|
||||||
|
else:
|
||||||
|
print("部分無人機切換模式失敗")
|
||||||
|
|
||||||
|
elif choice == '2':
|
||||||
|
if controller.all_drones_command("set_mode", "LAND"):
|
||||||
|
print("所有無人機已切換至 LAND 模式")
|
||||||
|
else:
|
||||||
|
print("部分無人機切換模式失敗")
|
||||||
|
|
||||||
|
elif choice == '3':
|
||||||
|
if controller.all_drones_command("arm", True):
|
||||||
|
print("所有無人機已解鎖")
|
||||||
|
else:
|
||||||
|
print("部分無人機解鎖失敗")
|
||||||
|
|
||||||
|
elif choice == '4':
|
||||||
|
if controller.all_drones_command("arm", False):
|
||||||
|
print("所有無人機已上鎖")
|
||||||
|
else:
|
||||||
|
print("部分無人機上鎖失敗")
|
||||||
|
|
||||||
|
elif choice == '5':
|
||||||
|
altitude = float(input("請輸入所有無人機起飛高度(公尺): "))
|
||||||
|
if controller.all_drones_command("takeoff", altitude):
|
||||||
|
print(f"所有無人機已到達目標高度: {altitude}公尺")
|
||||||
|
else:
|
||||||
|
print("部分無人機起飛失敗")
|
||||||
|
|
||||||
|
elif choice == '6':
|
||||||
|
if controller.all_drones_command("set_mode", "RTL"):
|
||||||
|
print("所有無人機已切換至 RTL 模式，正在返航")
|
||||||
|
else:
|
||||||
|
print("部分無人機切換至 RTL 模式失敗")
|
||||||
|
|
||||||
|
elif choice == '7':
|
||||||
|
print("返回主菜單")
|
||||||
|
break
|
||||||
|
|
||||||
|
else:
|
||||||
|
print("無效的指令，請重新輸入")
|
||||||
|
|
||||||
|
except ValueError as e:
|
||||||
|
print(f"輸入錯誤: {e}")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"發生錯誤: {e}")
|
||||||
|
|
||||||
|
def display_all_drone_states(controller):
|
||||||
|
"""顯示所有無人機狀態"""
|
||||||
|
print("\n=== 所有無人機狀態 ===")
|
||||||
|
|
||||||
|
for drone in controller.drones:
|
||||||
|
max_attempts = 5
|
||||||
|
state = None
|
||||||
|
|
||||||
|
for attempt in range(max_attempts):
|
||||||
|
state = drone.get_current_state()
|
||||||
|
if state and 'position' in state:
|
||||||
|
break
|
||||||
|
time.sleep(0.2)
|
||||||
|
|
||||||
|
print(f"\n--- 無人機 #{drone.drone_id} ---")
|
||||||
|
print(f"連接: {drone.connection_string}")
|
||||||
|
|
||||||
|
if not state:
|
||||||
|
print("狀態: 無法獲取完整數據")
|
||||||
|
continue
|
||||||
|
|
||||||
|
print(f"飛行模式: {state.get('mode', '未知')}")
|
||||||
|
print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
|
||||||
|
|
||||||
|
if 'gps_position' in state:
|
||||||
|
gps = state['gps_position']
|
||||||
|
print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
|
||||||
|
print(f"相對高度: {gps['relative_alt']:.2f}m")
|
||||||
|
|
||||||
|
if 'position' in state:
|
||||||
|
pos = state['position']
|
||||||
|
print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
|
||||||
|
|
||||||
|
print("-----------------------")
|
||||||
|
|
||||||
|
print("=========================")
|
||||||
|
|
||||||
|
def main():
|
||||||
|
controller = MultiDroneController(5)
|
||||||
|
print_menu()
|
||||||
|
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
choice = input("\n請輸入指令編號: ")
|
||||||
|
|
||||||
|
if choice == '0':
|
||||||
|
print_menu()
|
||||||
|
|
||||||
|
elif choice == '1':
|
||||||
|
drone_id = int(input("請輸入要操作的無人機ID (0-4): "))
|
||||||
|
drone = controller.get_drone(drone_id)
|
||||||
|
if drone:
|
||||||
|
control_single_drone(drone)
|
||||||
|
else:
|
||||||
|
print(f"無效的無人機ID: {drone_id}")
|
||||||
|
|
||||||
|
elif choice == '2':
|
||||||
|
control_all_drones(controller)
|
||||||
|
|
||||||
|
elif choice == '3':
|
||||||
|
display_all_drone_states(controller)
|
||||||
|
|
||||||
|
elif choice == '4':
|
||||||
|
print("程式結束")
|
||||||
|
break
|
||||||
|
|
||||||
|
else:
|
||||||
|
print("無效的指令，請重新輸入")
|
||||||
|
|
||||||
|
except ValueError as e:
|
||||||
|
print(f"輸入錯誤: {e}")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"發生錯誤: {e}")
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
try:
|
||||||
|
main()
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n程式被使用者中斷")
|
||||||
|
sys.exit(0)
|
||||||
@ -0,0 +1,55 @@
|
|||||||
|
import socket
|
||||||
|
import re
|
||||||
|
|
||||||
|
def start_udp_server(host='0.0.0.0', port=5000, buffer_size=1024):
|
||||||
|
server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
server_socket.bind((host, port))
|
||||||
|
|
||||||
|
print(f"UDP Server running on {host}:{port}")
|
||||||
|
print("Waiting for data...")
|
||||||
|
|
||||||
|
position = None
|
||||||
|
velocity = None
|
||||||
|
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
data, client_address = server_socket.recvfrom(buffer_size)
|
||||||
|
|
||||||
|
print(f"\n----- 接收到的資料 -----")
|
||||||
|
print(f"從 {client_address} 接收:")
|
||||||
|
|
||||||
|
try:
|
||||||
|
decoded_data = data.decode('utf-8')
|
||||||
|
print(f"文本數據: {decoded_data}")
|
||||||
|
|
||||||
|
# 直接使用正則表達式提取數據
|
||||||
|
pos_match = re.search(r'"position":\[([-\d\.\,]+)\]', decoded_data)
|
||||||
|
vel_match = re.search(r'"velocity":\[([-\d\.\,]+)\]', decoded_data)
|
||||||
|
|
||||||
|
if pos_match:
|
||||||
|
position_str = pos_match.group(1)
|
||||||
|
position = [float(x) for x in position_str.split(',')]
|
||||||
|
print(f"Position: {position}")
|
||||||
|
|
||||||
|
if vel_match:
|
||||||
|
velocity_str = vel_match.group(1)
|
||||||
|
velocity = [float(x) for x in velocity_str.split(',')]
|
||||||
|
print(f"Velocity: {velocity}")
|
||||||
|
|
||||||
|
# 這裡可以對 position 和 velocity 進行進一步操作
|
||||||
|
|
||||||
|
except UnicodeDecodeError:
|
||||||
|
print(f"二進制數據: {data.hex()}")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"處理數據時出錯: {e}")
|
||||||
|
|
||||||
|
print(f"數據大小: {len(data)} 字節")
|
||||||
|
print("-----------------------")
|
||||||
|
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("Server shutting down...")
|
||||||
|
finally:
|
||||||
|
server_socket.close()
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
start_udp_server(port=5000)
|
||||||
@ -0,0 +1,340 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
from pymavlink import mavutil
|
||||||
|
import time
|
||||||
|
import sys
|
||||||
|
import json
|
||||||
|
import socket
|
||||||
|
import threading
|
||||||
|
import os
|
||||||
|
from datetime import datetime
|
||||||
|
|
||||||
|
# 固定初始位置設定 (5,5,0)
|
||||||
|
INITIAL_POSITION = [5, 5, 0]
|
||||||
|
|
||||||
|
# MAVLink 連接設定
|
||||||
|
connection_string = 'udp:127.0.0.1:14553'
|
||||||
|
|
||||||
|
# 網路傳輸設定
|
||||||
|
target_ip = '140.120.31.130'
|
||||||
|
target_port = 9053
|
||||||
|
|
||||||
|
# 建立 UDP socket 用於傳送數據
|
||||||
|
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
|
||||||
|
# 封包編號計數器
|
||||||
|
packet_counter = 0
|
||||||
|
|
||||||
|
# 共享變數 - 存儲最新位置
|
||||||
|
current_position = None
|
||||||
|
initial_position = INITIAL_POSITION.copy()
|
||||||
|
position_lock = threading.Lock()
|
||||||
|
|
||||||
|
# 連接到無人機
|
||||||
|
print(f"嘗試連接到 MAVLink: {connection_string}")
|
||||||
|
try:
|
||||||
|
connection = mavutil.mavlink_connection(connection_string, autoreconnect=True)
|
||||||
|
connection.wait_heartbeat()
|
||||||
|
print(f"成功連接到系統 {connection.target_system}!")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"MAVLink 連接失敗: {e}")
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
print(f"使用指定初始位置: 北={initial_position[0]}, 東={initial_position[1]}, 下={initial_position[2]}")
|
||||||
|
|
||||||
|
# 請求位置數據流
|
||||||
|
connection.mav.request_data_stream_send(
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_DATA_STREAM_POSITION,
|
||||||
|
10,
|
||||||
|
1
|
||||||
|
)
|
||||||
|
|
||||||
|
# 日誌記錄函數
|
||||||
|
def start_logging():
|
||||||
|
"""創建並開啟日誌文件"""
|
||||||
|
log_dir = "logs"
|
||||||
|
if not os.path.exists(log_dir):
|
||||||
|
os.makedirs(log_dir)
|
||||||
|
|
||||||
|
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||||
|
log_file = os.path.join(log_dir, f"drone_log_{timestamp}.csv")
|
||||||
|
|
||||||
|
file = open(log_file, "w")
|
||||||
|
file.write("packet_id,timestamp,x,y,z,vx,vy,vz\n") # CSV標頭
|
||||||
|
print(f"日誌記錄已開始: {log_file}")
|
||||||
|
return file
|
||||||
|
|
||||||
|
# 控制函數
|
||||||
|
def set_guided_mode():
|
||||||
|
"""設置為 GUIDED 模式"""
|
||||||
|
connection.mav.command_long_send(
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_CMD_DO_SET_MODE,
|
||||||
|
0,
|
||||||
|
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
|
||||||
|
4,
|
||||||
|
0, 0, 0, 0, 0
|
||||||
|
)
|
||||||
|
ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
|
||||||
|
return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
|
||||||
|
|
||||||
|
def arm_throttle():
|
||||||
|
"""解鎖油門"""
|
||||||
|
connection.mav.command_long_send(
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||||
|
0, 1, 0, 0, 0, 0, 0, 0
|
||||||
|
)
|
||||||
|
ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
|
||||||
|
return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
|
||||||
|
|
||||||
|
def takeoff(altitude):
|
||||||
|
"""起飛到指定高度"""
|
||||||
|
connection.mav.command_long_send(
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||||
|
0, 0, 0, 0, 0, 0, 0, altitude
|
||||||
|
)
|
||||||
|
ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
|
||||||
|
return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
|
||||||
|
|
||||||
|
def set_speed_limit(speed_type, speed):
|
||||||
|
"""
|
||||||
|
設置速度限制
|
||||||
|
|
||||||
|
Parameters:
|
||||||
|
-----------
|
||||||
|
speed_type : int
|
||||||
|
速度類型 (0=空速, 1=地速, 2=爬升速度, 3=下降速度)
|
||||||
|
speed : float
|
||||||
|
速度值 (米/秒)
|
||||||
|
"""
|
||||||
|
connection.mav.command_long_send(
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
|
||||||
|
0, # 確認
|
||||||
|
speed_type, # 參數1: 速度類型
|
||||||
|
speed, # 參數2: 速度 (m/s)
|
||||||
|
-1, # 參數3: 油門 (-1 表示不變)
|
||||||
|
0, 0, 0, 0 # 參數4-7: 未使用
|
||||||
|
)
|
||||||
|
ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
|
||||||
|
success = ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
|
||||||
|
if success:
|
||||||
|
print(f"已設置速度限制為 {speed:.2f} m/s")
|
||||||
|
else:
|
||||||
|
print("設置速度限制失敗")
|
||||||
|
return success
|
||||||
|
|
||||||
|
def goto_position(north, east, down):
|
||||||
|
"""前往指定位置 (世界座標系)"""
|
||||||
|
# 將世界座標轉換為相對座標
|
||||||
|
rel_north = north - initial_position[0]
|
||||||
|
rel_east = east - initial_position[1]
|
||||||
|
rel_down = down - initial_position[2]
|
||||||
|
|
||||||
|
connection.mav.set_position_target_local_ned_send(
|
||||||
|
0,
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
|
||||||
|
0b0000111111111000,
|
||||||
|
rel_north, rel_east, rel_down,
|
||||||
|
0, 0, 0,
|
||||||
|
0, 0, 0,
|
||||||
|
0, 0
|
||||||
|
)
|
||||||
|
# print(f"相對位置: 北={rel_north:.2f}, 東={rel_east:.2f}, 下={rel_down:.2f}")
|
||||||
|
|
||||||
|
def get_current_position():
|
||||||
|
"""獲取存儲的當前位置"""
|
||||||
|
with position_lock:
|
||||||
|
if current_position is None:
|
||||||
|
return None
|
||||||
|
return current_position.copy()
|
||||||
|
|
||||||
|
def control_interface():
|
||||||
|
"""命令行控制界面"""
|
||||||
|
print("無人機控制介面已啟動，輸入命令：")
|
||||||
|
print(" arm - 解鎖油門")
|
||||||
|
print(" takeoff [高度] - 起飛")
|
||||||
|
print(" goto [北] [東] [高度] - 前往位置 (世界座標系)")
|
||||||
|
print(" pos - 顯示當前位置 (相對座標和世界座標)")
|
||||||
|
print(" init - 顯示初始位置")
|
||||||
|
print(" exit - 退出控制")
|
||||||
|
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
cmd = input("命令> ").strip().split()
|
||||||
|
if not cmd:
|
||||||
|
continue
|
||||||
|
|
||||||
|
if cmd[0] == "arm":
|
||||||
|
print("嘗試解鎖油門...")
|
||||||
|
if set_guided_mode() and arm_throttle():
|
||||||
|
print("成功解鎖油門！")
|
||||||
|
else:
|
||||||
|
print("解鎖失敗")
|
||||||
|
|
||||||
|
elif cmd[0] == "takeoff":
|
||||||
|
alt = float(cmd[1]) if len(cmd) > 1 else 3.0
|
||||||
|
print(f"嘗試起飛到 {alt} 米高...")
|
||||||
|
if takeoff(alt):
|
||||||
|
print("起飛命令已發送")
|
||||||
|
else:
|
||||||
|
print("起飛失敗")
|
||||||
|
|
||||||
|
elif cmd[0] == "goto":
|
||||||
|
if len(cmd) >= 4:
|
||||||
|
north = float(cmd[1])
|
||||||
|
east = float(cmd[2])
|
||||||
|
down = -float(cmd[3]) # 高度是負的下
|
||||||
|
print(f"前往世界位置: 北:{north} 東:{east} 下:{-down} 米")
|
||||||
|
goto_position(north, east, down)
|
||||||
|
else:
|
||||||
|
print("用法: goto [北] [東] [高度]")
|
||||||
|
|
||||||
|
elif cmd[0] == "pos":
|
||||||
|
pos = get_current_position()
|
||||||
|
if pos:
|
||||||
|
rel_x, rel_y, rel_z = pos
|
||||||
|
# 計算世界座標
|
||||||
|
world_x = initial_position[0] + rel_x
|
||||||
|
world_y = initial_position[1] + rel_y
|
||||||
|
world_z = initial_position[2] + rel_z
|
||||||
|
# NED 轉 ENU
|
||||||
|
enu_x = rel_y
|
||||||
|
enu_y = rel_x
|
||||||
|
enu_z = -rel_z
|
||||||
|
|
||||||
|
print("\n當前位置:")
|
||||||
|
#print(f" 相對座標 (NED): 北={rel_x:.2f}m, 東={rel_y:.2f}m, 下={rel_z:.2f}m")
|
||||||
|
print(f" 世界座標 : 北={world_x:.2f}m, 東={world_y:.2f}m, 下={-world_z:.2f}m")
|
||||||
|
#print(f" 相對座標 (ENU): 東={enu_x:.2f}m, 北={enu_y:.2f}m, 上={enu_z:.2f}m")
|
||||||
|
#print(f" 高度: {-rel_z:.2f}m")
|
||||||
|
else:
|
||||||
|
print("尚未獲取到位置信息")
|
||||||
|
|
||||||
|
elif cmd[0] == "speed":
|
||||||
|
if len(cmd) > 1:
|
||||||
|
speed = float(cmd[1])
|
||||||
|
set_speed_limit(1, speed) # 1 = 地速
|
||||||
|
else:
|
||||||
|
print("用法: speed [速度m/s]")
|
||||||
|
|
||||||
|
elif cmd[0] == "climbspeed":
|
||||||
|
if len(cmd) > 1:
|
||||||
|
speed = float(cmd[1])
|
||||||
|
set_speed_limit(2, speed) # 2 = 爬升速度
|
||||||
|
set_speed_limit(3, speed) # 3 = 下降速度
|
||||||
|
else:
|
||||||
|
print("用法: climbspeed [速度m/s]")
|
||||||
|
|
||||||
|
elif cmd[0] == "init":
|
||||||
|
print(f"初始位置 (NED): 北={initial_position[0]:.2f}m, 東={initial_position[1]:.2f}m, 下={initial_position[2]:.2f}m")
|
||||||
|
|
||||||
|
elif cmd[0] == "exit":
|
||||||
|
print("退出控制介面")
|
||||||
|
break
|
||||||
|
|
||||||
|
else:
|
||||||
|
print(f"未知命令: {cmd[0]}")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"命令執行錯誤: {e}")
|
||||||
|
|
||||||
|
# 數據傳輸線程函數
|
||||||
|
def data_sending_thread():
|
||||||
|
global packet_counter, current_position
|
||||||
|
|
||||||
|
# 開啟日誌文件
|
||||||
|
log_file = start_logging()
|
||||||
|
|
||||||
|
try:
|
||||||
|
print(f"開始接收數據並傳送到 {target_ip}:{target_port}...")
|
||||||
|
while True:
|
||||||
|
msg = connection.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
|
||||||
|
if msg is not None:
|
||||||
|
# 更新當前位置（線程安全）
|
||||||
|
with position_lock:
|
||||||
|
current_position = [msg.x, msg.y, msg.z]
|
||||||
|
|
||||||
|
# 增加封包編號
|
||||||
|
packet_counter += 1
|
||||||
|
|
||||||
|
# 計算世界座標
|
||||||
|
world_x = initial_position[0] + msg.x
|
||||||
|
world_y = initial_position[1] + msg.y
|
||||||
|
world_z = initial_position[2] + msg.z
|
||||||
|
|
||||||
|
# NED 轉換為 ENU 並使用世界座標
|
||||||
|
enu_position = {
|
||||||
|
"x": world_x, # 東 (從 NED 的 x)
|
||||||
|
"y": world_y, # 北 (從 NED 的 y)
|
||||||
|
"z": -world_z # 上 (從 NED 的 -z)
|
||||||
|
}
|
||||||
|
|
||||||
|
enu_velocity = {
|
||||||
|
"vx": msg.vy, # 東向速度
|
||||||
|
"vy": msg.vx, # 北向速度
|
||||||
|
"vz": -msg.vz # 上向速度
|
||||||
|
}
|
||||||
|
|
||||||
|
# 創建JSON對象 (使用世界座標)
|
||||||
|
json_data = {
|
||||||
|
"packet_id": packet_counter,
|
||||||
|
"timestamp": msg.time_boot_ms,
|
||||||
|
"enemy_position": enu_position,
|
||||||
|
"enemy_velocity": enu_velocity
|
||||||
|
}
|
||||||
|
|
||||||
|
# 寫入日誌文件 (CSV格式)
|
||||||
|
try:
|
||||||
|
log_file.write(f"{packet_counter},{msg.time_boot_ms},{enu_position['x']},{enu_position['y']},{enu_position['z']},{enu_velocity['vx']},{enu_velocity['vy']},{enu_velocity['vz']}\n")
|
||||||
|
log_file.flush() # 確保數據即時寫入文件
|
||||||
|
except Exception as e:
|
||||||
|
print(f"寫入日誌失敗: {e}")
|
||||||
|
|
||||||
|
json_str = json.dumps(json_data)
|
||||||
|
|
||||||
|
try:
|
||||||
|
sock.sendto(json_str.encode('utf-8'), (target_ip, target_port))
|
||||||
|
except Exception as e:
|
||||||
|
print(f"傳送數據失敗: {e}")
|
||||||
|
|
||||||
|
time.sleep(0.1)
|
||||||
|
except Exception as e:
|
||||||
|
print(f"數據傳輸線程錯誤: {e}")
|
||||||
|
finally:
|
||||||
|
# 確保關閉日誌文件
|
||||||
|
log_file.close()
|
||||||
|
print("日誌文件已關閉")
|
||||||
|
|
||||||
|
# 創建並啟動數據發送線程
|
||||||
|
sender_thread = threading.Thread(target=data_sending_thread)
|
||||||
|
sender_thread.daemon = True
|
||||||
|
sender_thread.start()
|
||||||
|
|
||||||
|
# 啟動命令行控制界面
|
||||||
|
try:
|
||||||
|
control_interface()
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n程序已被用戶中斷")
|
||||||
|
finally:
|
||||||
|
if connection:
|
||||||
|
connection.mav.request_data_stream_send(
|
||||||
|
connection.target_system,
|
||||||
|
connection.target_component,
|
||||||
|
mavutil.mavlink.MAV_DATA_STREAM_ALL,
|
||||||
|
0, 0
|
||||||
|
)
|
||||||
|
print("數據流已停止")
|
||||||
|
sock.close()
|
||||||
|
print("網絡連接已關閉")
|
||||||
|
print(f"總共傳送了 {packet_counter} 個封包")
|
||||||
|
print("日誌文件已儲存")
|
||||||
Binary file not shown.
Loading…
Reference in New Issue