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

Upload files to 'src/unitdev01/unitdev01'

Browse Source
ken910606 1 year ago
parent d21bc6b069
commit f7f725a2ab
1 changed files with 90 additions and 57 deletions
Show Stats Download Patch File Download Diff File

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

@ -1,7 +1,6 @@
import sys
import time
import math
import rclpy
from PyQt5 import QtWidgets
from PyQt5.QtCore import QThread, pyqtSignal
from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox
@ -20,10 +19,11 @@ class MAVLinkWorker(QThread):
loss_signal = pyqtSignal(str, float)
frequency_signal = pyqtSignal(str, float)
param_signal = pyqtSignal(str, int)
kbps_signal = pyqtSignal(str, float)
def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
super().__init__()
self.namespaces = ['UAV1', 'UAV4', 'UAV5']
self.namespaces = ['UAV1', 'UAV2', 'UAV3']
self.connection = mavutil.mavlink_connection(usb, baud=57600)
self.connection.wait_heartbeat()
for sysid in self.namespaces:
@ -31,38 +31,48 @@ class MAVLinkWorker(QThread):
self.set_sr_params(sysid)
self.running = True
# 設置需要監控的訊息類型
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 = {}
self.total_bytes_received = {}
self.throughput_KBps = {}
for namespace in self.namespaces:
self.request_param(namespace, "SR1_EXTRA1")
self.connection.mav.timesync_send(
0, #tc1
int(time.time() * 1e9) #ts1
)
def run(self):
while self.running:
try:
current_time = time.time()
msg = self.connection.recv_msg()
current_time = time.time()
if not msg:
continue
sysid = msg.get_srcSystem()
if sysid == 0:
continue
namespace = f"UAV{sysid}"
print(sysid)
msg_type = msg.get_type()
if msg_type =="BAD_DATA":
continue
print(msg_type)
if sysid not in self.total_bytes_received:
self.total_bytes_received[sysid] = 0
self.throughput_KBps[sysid] = 0
# 計算訊息大小
msg_bytes = msg.get_msgbuf() # 取得封包的 bytes
if msg_bytes:
self.total_bytes_received[sysid] += len(msg_bytes)
# Packet loss calculation
if sysid not in self.seq_numbers:
@ -71,7 +81,7 @@ class MAVLinkWorker(QThread):
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
expected_seq = (self.seq_numbers[sysid] + 1) % 256
self.total_packet_count[sysid] += 1
if current_seq != expected_seq: # Packet(s) lost
@ -91,27 +101,32 @@ class MAVLinkWorker(QThread):
self.message_count[sysid] += 1
# 每隔一段時間更新
if current_time - self.start_time[sysid] >= 1:
elapsed_time = current_time - self.start_time[sysid]
if elapsed_time >= 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[sysid] = self.message_count[sysid] / elapsed_time
self.frequency_signal.emit(namespace, self.frequency[sysid])
# 發送 timesync request
self.connection.mav.timesync_send(
0, #tc1
int(time.time() * 1e9) #ts1
int(current_time * 1e9) #ts1
)
#self.send_heartbeat
#吞吐量
self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time
self.kbps_signal.emit(namespace, self.throughput_KBps[sysid])
self.start_time[sysid] = current_time
self.message_count[sysid] = 0
self.total_bytes_received[sysid] = 0
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
self.state_signal.emit(namespace, mode)
elif msg_type == "BATTERY_STATUS":
percentage = msg.battery_remaining/100
self.battery_signal.emit(namespace, percentage)
voltage = msg.voltages[0] / 1000
self.battery_signal.emit(namespace, voltage)
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
@ -140,11 +155,11 @@ class MAVLinkWorker(QThread):
self.groundspeed_signal.emit(namespace, groundspeed)
elif msg_type == "TIMESYNC":
round_trip_time = (int(time.time() * 1e9) - msg.ts1) / 1e6
round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6
if(round_trip_time<1e6):
self.ping_signal.emit(namespace, round_trip_time)
elif msg.get_type() == 'PARAM_VALUE':
elif msg_type == 'PARAM_VALUE':
param_name = "SR1_EXTRA1"
if msg.param_id == param_name:
self.param_signal.emit(namespace, msg.param_value)
@ -253,13 +268,7 @@ class MAVLinkWorker(QThread):
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]
@ -275,6 +284,22 @@ class MAVLinkWorker(QThread):
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
'''
def set_sr_params(self, sysid):
""" 直接設置 MAVLink 訊息頻率 """
freqs = [0, 1, 1]
params = {
"SR1_ADSB": freqs[0],
"SR1_EXT_STAT": freqs[0],
"SR1_EXTRA1": freqs[2],
"SR1_EXTRA2": freqs[2],
"SR1_EXTRA3": freqs[0],
"SR1_POSITION": freqs[1],
"SR1_RAW_SENS": freqs[1],
"SR1_RC_CHAN": freqs[0]
}
for param, value in params.items():
self.set_param(f"UAV{sysid}", param, value)
class DroneControlApp(QMainWindow):
def __init__(self):
@ -296,6 +321,7 @@ class DroneControlApp(QMainWindow):
self.workers.loss_signal.connect(self.update_loss)
self.workers.frequency_signal.connect(self.update_frequency)
self.workers.param_signal.connect(self.update_param)
self.workers.kbps_signal.connect(self.update_kbps)
self.workers.start()
def initUI(self):
@ -326,17 +352,18 @@ class DroneControlApp(QMainWindow):
# 狀態顯示
self.uav_labels[namespace] = {
"status": QLabel("狀態：等待數據..."),
"battery": QLabel("電量：等待數據..."),
"battery": QLabel("電壓：等待數據..."),
"local_position": QLabel("Local Position：等待數據..."),
"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("頻率：等待數據..."),
"kbps": QLabel("吞吐量：等待數據..."),
"param": QLabel("SR1_EXTRA1：等待數據...")
}
@ -370,25 +397,6 @@ class DroneControlApp(QMainWindow):
mode_layout.addWidget(self.modeButton)
main_layout.addLayout(mode_layout)
# XYZ 設置欄位
xyz_layout = QHBoxLayout()
self.positionX = QLineEdit()
self.positionX.setPlaceholderText("X")
self.positionY = QLineEdit()
self.positionY.setPlaceholderText("Y")
self.positionZ = QLineEdit()
self.positionZ.setPlaceholderText("Z")
xyz_layout.addWidget(QLabel("輸入位置："))
xyz_layout.addWidget(self.positionX)
xyz_layout.addWidget(self.positionY)
xyz_layout.addWidget(self.positionZ)
main_layout.addLayout(xyz_layout)
# 設置 XYZ 的按鈕
self.setPositionButton = QPushButton("設置位置")
self.setPositionButton.clicked.connect(self.set_local_position)
main_layout.addWidget(self.setPositionButton)
# 解鎖按鈕
fly_layout = QHBoxLayout()
self.armButton = QPushButton("解鎖")
@ -406,6 +414,25 @@ class DroneControlApp(QMainWindow):
fly_layout.addWidget(self.landButton)
main_layout.addLayout(fly_layout)
# XYZ 設置欄位
xyz_layout = QHBoxLayout()
self.positionX = QLineEdit()
self.positionX.setPlaceholderText("X")
self.positionY = QLineEdit()
self.positionY.setPlaceholderText("Y")
self.positionZ = QLineEdit()
self.positionZ.setPlaceholderText("Z")
self.setPositionButton = QPushButton("設置位置")
self.setPositionButton.clicked.connect(self.set_local_position)
xyz_layout.addWidget(QLabel("輸入位置："))
xyz_layout.addWidget(self.positionX)
xyz_layout.addWidget(self.positionY)
xyz_layout.addWidget(self.positionZ)
xyz_layout.addWidget(self.setPositionButton)
main_layout.addLayout(xyz_layout)
# 設置 XYZ 的按鈕
#設置重啟按鈕
self.rebootButton = QPushButton("重啟")
self.rebootButton.clicked.connect(self.reboot_drone)
@ -426,8 +453,8 @@ class DroneControlApp(QMainWindow):
def update_state(self, namespace, mode):
self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
def update_battery(self, namespace, percentage):
self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%")
def update_battery(self, namespace, voltage):
self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V")
def update_gps(self, namespace, latitude, longitude):
self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°")
@ -465,6 +492,9 @@ class DroneControlApp(QMainWindow):
def update_frequency(self, namespace, frequency):
self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
def update_kbps(self, namespace, kbps):
self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s")
def update_param(self, namespace, value):
self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
@ -488,10 +518,12 @@ class DroneControlApp(QMainWindow):
def takeoff_drone(self):
try:
z_text = self.positionZ.text().strip()
altitude = float(z_text) if z_text else 5.0
for namespace in self.namespaces:
z = float(z_text) if z_text else 5.0
h = 2
for i, namespace in enumerate(self.namespaces):
if self.uav_checkboxes[namespace].isChecked():
self.workers.takeoff(namespace, altitude)
self.workers.takeoff(namespace, z + h * i)
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
@ -505,9 +537,11 @@ class DroneControlApp(QMainWindow):
x = float(self.positionX.text().strip())
y = float(self.positionY.text().strip())
z = float(self.positionZ.text().strip())
for namespace in self.namespaces:
h = 2
for i, namespace in enumerate(self.namespaces):
if self.uav_checkboxes[namespace].isChecked():
self.workers.set_local_position(namespace, x, y, z)
self.workers.set_local_position(namespace, x, y, z + h * i)
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
@ -529,8 +563,7 @@ class DroneControlApp(QMainWindow):
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
def main(args=None):
rclpy.init(args=args)
def main():
app = QtWidgets.QApplication(sys.argv)
window = DroneControlApp()
window.show()

Write Preview
Loading…
Cancel
Save