From ee4cf1d07cd75d1214e7a15e81b13cda9885f94c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:03:11 +0800 Subject: [PATCH] Update 'src/unitdev01/unitdev01/mavlink.py' --- src/unitdev01/unitdev01/mavlink.py | 291 +++++++++++++++++++++++------ 1 file changed, 237 insertions(+), 54 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index 430864e..9078880 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -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)