From f7f725a2abc4362dd27880ebf885c310cf9828f5 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 9 Apr 2025 20:35:56 +0800 Subject: [PATCH] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/mavlink.py | 147 ++++++++++++++++++----------- 1 file changed, 90 insertions(+), 57 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index 9078880..b047a0e 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -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()