wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

Update 'src/unitdev01/unitdev01/mavlink.py'

Browse Source
This commit is contained in:
ken910606
2025-02-22 00:03:11 +08:00
parent b8965c0bd6
commit ee4cf1d07c
1 changed files with 237 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/unitdev01/unitdev01/mavlink.py
+237 -54
View File
@@ -1,4 +1,6 @@
import sys
import time
import math
import rclpy
from PyQt5 import QtWidgets
from PyQt5.QtCore import QThread, pyqtSignal
@@ -14,26 +16,94 @@ class MAVLinkWorker(QThread):
imu_signal = pyqtSignal(str, float)
hdg_signal = pyqtSignal(str, float)
groundspeed_signal = pyqtSignal(str, float)
ping_signal = pyqtSignal(str, float)
loss_signal = pyqtSignal(str, float)
frequency_signal = pyqtSignal(str, float)
param_signal = pyqtSignal(str, int)
def __init__(self, connection_string="udp:127.0.0.1:14550"):
def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
super().__init__()
self.connection = mavutil.mavlink_connection(connection_string)
self.namespaces = ['UAV1', 'UAV4', 'UAV5']
self.connection = mavutil.mavlink_connection(usb, baud=57600)
self.connection.wait_heartbeat()
for sysid in self.namespaces:
sysid = int(sysid.replace('UAV', ''))
self.set_sr_params(sysid)
self.running = True
self.uav_data = {}
# 設置需要監控的訊息類型
self.messages_to_monitor = ["HEARTBEAT", "BATTERY_STATUS", "GLOBAL_POSITION_INT", "GPS_RAW_INT", "LOCAL_POSITION_NED", "ATTITUDE", "VFR_HUD", "TIMESYNC"]
self.message_count = {}
self.frequency = {}
self.start_time = {}
# 用於計算丟包
self.seq_numbers = {}
self.packet_loss_count = {}
self.total_packet_count = {}
self.loss_percentage = {}
for namespace in self.namespaces:
self.request_param(namespace, "SR1_EXTRA1")
def run(self):
while self.running:
try:
msg = self.connection.recv_match(blocking=True)
current_time = time.time()
msg = self.connection.recv_msg()
if not msg:
continue
msg_type = msg.get_type()
sysid = msg.get_srcSystem()
namespace = f"uav{sysid}"
if sysid == 0:
continue
namespace = f"UAV{sysid}"
print(sysid)
msg_type = msg.get_type()
if msg_type =="BAD_DATA":
continue
if sysid not in self.uav_data:
self.uav_data[sysid] = {"sysid": sysid}
print(msg_type)
# Packet loss calculation
if sysid not in self.seq_numbers:
self.seq_numbers[sysid] = msg.get_seq() # Initialize sequence number tracking
self.packet_loss_count[sysid] = 0
self.total_packet_count[sysid] = 1
else:
current_seq = msg.get_seq()
expected_seq = (self.seq_numbers[sysid] + 1) % 256 # MAVLink sequence numbers are modulo 256
self.total_packet_count[sysid] += 1
if current_seq != expected_seq: # Packet(s) lost
lost_packets = (current_seq - expected_seq) % 256
self.packet_loss_count[sysid] += lost_packets
self.total_packet_count[sysid] += lost_packets
self.seq_numbers[sysid] = current_seq
self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100
self.loss_signal.emit(namespace, self.loss_percentage[sysid])
# Frequency calculation
if sysid not in self.message_count:
self.message_count[sysid] = 0
self.start_time[sysid] = current_time
self.message_count[sysid] += 1
# 每隔一段時間更新
if current_time - self.start_time[sysid] >= 1:
# 每秒頻率
self.frequency[sysid] = self.message_count[sysid] / (current_time - self.start_time[sysid])
self.start_time[sysid] = current_time
self.message_count[sysid] = 0
self.frequency_signal.emit(namespace, self.frequency[sysid])
# 發送 timesync request
self.connection.mav.timesync_send(
0, #tc1
int(time.time() * 1e9) #ts1
)
#self.send_heartbeat
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
@@ -60,7 +130,7 @@ class MAVLinkWorker(QThread):
self.local_position_signal.emit(namespace, x, y, z)
elif msg_type == "ATTITUDE":
pitch = msg.pitch
pitch = math.degrees(msg.pitch)
self.imu_signal.emit(namespace, pitch)
elif msg_type == "VFR_HUD":
@@ -69,6 +139,16 @@ class MAVLinkWorker(QThread):
self.hdg_signal.emit(namespace, heading)
self.groundspeed_signal.emit(namespace, groundspeed)
elif msg_type == "TIMESYNC":
round_trip_time = (int(time.time() * 1e9) - msg.ts1) / 1e6
if(round_trip_time<1e6):
self.ping_signal.emit(namespace, round_trip_time)
elif msg.get_type() == 'PARAM_VALUE':
param_name = "SR1_EXTRA1"
if msg.param_id == param_name:
self.param_signal.emit(namespace, msg.param_value)
except Exception as e:
print(f"Error reading message: {e}")
@@ -77,7 +157,7 @@ class MAVLinkWorker(QThread):
self.connection.close()
def set_mode(self, namespace, mode):
sysid = int(namespace[-1])
sysid = int(namespace.replace('UAV', ''))
if mode == 'STABILIZE':
mode = 0
elif mode == 'AUTO':
@@ -86,56 +166,47 @@ class MAVLinkWorker(QThread):
mode = 4
elif mode == 'LOITER':
mode = 5
self.connection.mav.set_mode_send(
self.connection.mav.set_mode_send( #SET_MODE (11) — [DEP]
sysid,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode
)
def arm(self, namespace, arm):
sysid = int(namespace[-1])
# self.connection.mav.command_long_send(
# sysid,
# 1, # Component ID
# mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
# 0, # Confirmation
# 1 if arm else 0,
# 0, 0, 0, 0, 0, 0
# )
sysid = int(namespace.replace('UAV', ''))
self.connection.mav.command_long_send(
sysid,
1, # Component ID
mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
0,
33,
1000000,
0, 0, 0, 0, 0
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, #MAV_CMD_COMPONENT_ARM_DISARM (400)
0, # Confirmation
1 if arm else 0,
0, 0, 0, 0, 0, 0
)
def takeoff(self, namespace, altitude):
sysid = int(namespace[-1])
sysid = int(namespace.replace('UAV', ''))
self.connection.mav.command_long_send(
sysid,
1, # Component ID
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, # Command to execute
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, #MAV_CMD_NAV_TAKEOFF (22)
0, # Confirmation (0 = first transmission)
0, 0, 0, 0, # Parameters 1-4 (Unused in takeoff command)
0, 0, altitude # Latitude, Longitude, Altitude (target_altitude in meters)
)
def land(self, namespace):
sysid = int(namespace[-1])
sysid = int(namespace.replace('UAV', ''))
self.connection.mav.command_long_send(
sysid,
1, # Component ID
mavutil.mavlink.MAV_CMD_NAV_LAND,
mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21)
0,
0, 0, 0, 0,
0, 0, 0
)
def set_local_position(self, namespace, x, y, z):
sysid = int(namespace[-1])
sysid = int(namespace.replace('UAV', ''))
self.connection.mav.set_position_target_local_ned_send(
0, sysid, 1, # time_boot_ms, sysid, compid
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame
@@ -146,12 +217,70 @@ class MAVLinkWorker(QThread):
0, 0 # Yaw, yaw_rate
)
def reboot_drone(self, namespace):
sysid = int(namespace.replace('UAV', ''))
self.connection.mav.command_long_send(
sysid,
1, # target_component (一般為1)
mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, # Reboot 命令
0, # confirmation
1, # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機)
0, 0, 0, 0, 0, 0 # 其餘參數保留
)
def set_param(self, namespace, param_name, value):
sysid = int(namespace.replace('UAV', ''))
try:
self.connection.mav.param_set_send(
sysid,
1,
param_name.encode('utf-8'),
float(value),
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
except Exception as e:
print(f"Failed to set parameter {param_name}: {e}")
def request_param(self, namespace, param_name):
sysid = int(namespace.replace('UAV', ''))
try:
self.connection.mav.param_request_read_send(
sysid, # 系統 ID
1, # 組件 ID
param_name.encode('utf-8'), # 參數名稱
-1 # 參數索引，-1 表示根據名稱請求
)
except Exception as e:
print(f"Failed to set parameter {param_name}: {e}")
def send_heartbeat(self):
self.connection.mav.heartbeat_send(
mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_INVALID,
0, 0, 0
)
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 2, 4]
params = {
"SR1_ADSB": freqs[0],
"SR1_EXT_STAT": freqs[1],
"SR1_EXTRA1": freqs[2],
"SR1_EXTRA2": freqs[2],
"SR1_EXTRA3": freqs[1],
"SR1_POSITION": freqs[1],
"SR1_RAW_SENS": freqs[1],
"SR1_RC_CHAN": freqs[1]
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
class DroneControlApp(QMainWindow):
def __init__(self):
super().__init__()
self.namespaces = ['uav1', 'uav2', 'uav3']
self.workers = MAVLinkWorker()
self.namespaces = self.workers.namespaces
self.initUI()
# Connect signals to update the UI
@@ -163,7 +292,10 @@ class DroneControlApp(QMainWindow):
self.workers.imu_signal.connect(self.update_imu)
self.workers.hdg_signal.connect(self.update_hdg)
self.workers.groundspeed_signal.connect(self.update_groundspeed)
self.workers.ping_signal.connect(self.update_ping)
self.workers.loss_signal.connect(self.update_loss)
self.workers.frequency_signal.connect(self.update_frequency)
self.workers.param_signal.connect(self.update_param)
self.workers.start()
def initUI(self):
@@ -193,15 +325,19 @@ class DroneControlApp(QMainWindow):
# 狀態顯示
self.uav_labels[namespace] = {
"status": QLabel(f"{namespace} 狀態：等待數據..."),
"battery": QLabel(f"{namespace} 電量：等待數據..."),
"gps": QLabel(f"{namespace} GPS位置：等待數據...\n\n"),
"fix": QLabel(f"{namespace} Fix Type：等待數據..."),
"satellites": QLabel(f"{namespace} 衛星數量：等待數據..."),
"local_position": QLabel(f"{namespace} Local Position：等待數據..."),
"groundspeed": QLabel(f"{namespace} 地面速度：等待數據..."),
"pitch": QLabel(f"{namespace} Pitch：等待數據..."),
"heading": QLabel(f"{namespace} Heading：等待數據..."),
"status": QLabel("狀態：等待數據..."),
"battery": QLabel("電量：等待數據..."),
"gps": QLabel("GPS位置：等待數據...\n\n"),
"fix": QLabel("Fix Type：等待數據..."),
"satellites": QLabel("衛星數量：等待數據..."),
"local_position": QLabel("Local Position：等待數據..."),
"groundspeed": QLabel("地面速度：等待數據..."),
"pitch": QLabel("Pitch：等待數據..."),
"heading": QLabel("Heading：等待數據..."),
"ping": QLabel("Ping：等待數據..."),
"loss": QLabel("丟包：等待數據..."),
"frequency": QLabel("頻率：等待數據..."),
"param": QLabel("SR1_EXTRA1：等待數據...")
}
# 把每個資訊添加到該 UAV 的垂直佈局中
@@ -213,6 +349,16 @@ class DroneControlApp(QMainWindow):
main_layout.addLayout(uav_layout)
# SR1_EXTRA1參數設置
param_layout = QHBoxLayout()
self.paramInput = QLineEdit()
self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值")
self.setParamButton = QPushButton("設置SR1_EXTRA1")
self.setParamButton.clicked.connect(self.set_SR1_EXTRA1)
param_layout.addWidget(self.paramInput)
param_layout.addWidget(self.setParamButton)
main_layout.addLayout(param_layout)
# 模式切換區域
mode_layout = QHBoxLayout()
self.modeComboBox = QComboBox()
@@ -244,19 +390,26 @@ class DroneControlApp(QMainWindow):
main_layout.addWidget(self.setPositionButton)
# 解鎖按鈕
fly_layout = QHBoxLayout()
self.armButton = QPushButton("解鎖")
self.armButton.clicked.connect(self.arm_drone)
main_layout.addWidget(self.armButton)
# 起飛按鈕
self.takeoffButton = QPushButton("起飛")
self.takeoffButton.clicked.connect(self.takeoff_drone)
main_layout.addWidget(self.takeoffButton)
# 降落按鈕
self.landButton = QPushButton("降落")
self.landButton.clicked.connect(self.land_drone)
main_layout.addWidget(self.landButton)
fly_layout.addWidget(self.armButton)
fly_layout.addWidget(self.takeoffButton)
fly_layout.addWidget(self.landButton)
main_layout.addLayout(fly_layout)
#設置重啟按鈕
self.rebootButton = QPushButton("重啟")
self.rebootButton.clicked.connect(self.reboot_drone)
main_layout.addWidget(self.rebootButton)
# 設置主佈局
central_widget = QWidget(self)
@@ -271,13 +424,13 @@ class DroneControlApp(QMainWindow):
event.accept()
def update_state(self, namespace, mode):
self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}")
self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
def update_battery(self, namespace, percentage):
self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%")
self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%")
def update_gps(self, namespace, latitude, longitude):
self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°")
self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°")
def update_gps_status(self, namespace, fix_type, satellites_visible):
fix_type_str = {
@@ -288,20 +441,32 @@ class DroneControlApp(QMainWindow):
4: "RTK Float",
5: "RTK Fix"
}.get(fix_type, "Unknown")
self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}")
self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{satellites_visible}")
self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}")
self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}")
def update_local_position(self, namespace, x, y, z):
self.uav_labels[namespace]["local_position"].setText(f"{namespace} Local：({x:.2f}, {y:.2f}, {z:.2f})")
self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})")
def update_imu(self, namespace, pitch):
self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.2f}°")
self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°")
def update_hdg(self, namespace, heading):
self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{heading:.2f}°")
self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°")
def update_groundspeed(self, namespace, groundspeed):
self.uav_labels[namespace]["groundspeed"].setText(f"{namespace} 地面速度：{groundspeed:.2f} m/s")
self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s")
def update_ping(self, namespace, ping):
self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms")
def update_loss(self, namespace, loss):
self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%")
def update_frequency(self, namespace, frequency):
self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
def update_param(self, namespace, value):
self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
def change_mode(self):
try:
@@ -346,6 +511,24 @@ class DroneControlApp(QMainWindow):
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def reboot_drone(self):
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.reboot_drone(namespace)
except Exception as e:
QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}")
def set_SR1_EXTRA1(self):
param_value = self.paramInput.text().strip()
try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
self.workers.set_param(namespace, "SR1_EXTRA1", param_value)
self.workers.request_param(namespace, "SR1_EXTRA1")
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def main(args=None):
rclpy.init(args=args)
app = QtWidgets.QApplication(sys.argv)