From a8b36c391f0b90daaa7a97882067b44dab13abf7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:01:35 +0800 Subject: [PATCH 1/3] Update 'src/unitdev01/unitdev01/interface.py' --- src/unitdev01/unitdev01/interface.py | 499 ++++++++++++++++++--------- 1 file changed, 345 insertions(+), 154 deletions(-) diff --git a/src/unitdev01/unitdev01/interface.py b/src/unitdev01/unitdev01/interface.py index 5699e46..9078880 100644 --- a/src/unitdev01/unitdev01/interface.py +++ b/src/unitdev01/unitdev01/interface.py @@ -1,141 +1,289 @@ import sys -import numpy as np +import time +import math import rclpy -from rclpy.executors import SingleThreadedExecutor from PyQt5 import QtWidgets from PyQt5.QtCore import QThread, pyqtSignal from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox -from std_msgs.msg import Float64 -from mavros_msgs.srv import SetMode, CommandBool, CommandTOL -from mavros_msgs.msg import State, GPSRAW, VfrHud -from sensor_msgs.msg import NavSatFix, BatteryState, Imu -from geometry_msgs.msg import PoseStamped -from mavros.base import SENSOR_QOS - -class ROS2Worker(QThread): - state_signal = pyqtSignal(str, str) # 新增 namespace 參數 - battery_signal = pyqtSignal(str, float) # 新增 namespace 參數 - gps_signal = pyqtSignal(str, float, float) # 新增 namespace 參數 +from pymavlink import mavutil + +class MAVLinkWorker(QThread): + state_signal = pyqtSignal(str, str) + battery_signal = pyqtSignal(str, float) + gps_signal = pyqtSignal(str, float, float) gps_status_signal = pyqtSignal(str, int, int) - local_position_signal = pyqtSignal(str, float, float, float) # 新增 namespace 參數 - imu_signal = pyqtSignal(str, float, float, float, float) + local_position_signal = pyqtSignal(str, float, float, float) + imu_signal = pyqtSignal(str, float) hdg_signal = pyqtSignal(str, float) - groundspeed_signal = pyqtSignal(str, float) # 新增 namespace 與 groundspeed 值 + 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, namespaces): + def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): super().__init__() - namespaces = ['uav1', 'uav2', 'uav3'] - self.node = rclpy.create_node('multi_uav_control_app') - self.executor = SingleThreadedExecutor() # 創建單執行器 - self.executor.add_node(self.node) # 添加節點到執行器 + 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 - for namespace in namespaces: - self.node.create_subscription(State, f'/{namespace}/state', - lambda msg, ns=namespace: self.state_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(BatteryState, f'/{namespace}/battery', - lambda msg, ns=namespace: self.battery_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(NavSatFix, f'/{namespace}/global_position/global', - lambda msg, ns=namespace: self.gps_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(GPSRAW, f'/{namespace}/gpsstatus/gps1/raw', - lambda msg, ns=namespace: self.gps_status_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(PoseStamped, f'/{namespace}/local_position/pose', - lambda msg, ns=namespace: self.local_position_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(Imu, f'/{namespace}/imu/data', - lambda msg, ns=namespace: self.imu_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(Float64, f'/{namespace}/global_position/compass_hdg', - lambda msg, ns=namespace: self.heading_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(VfrHud, f'/{namespace}/vfr_hud', - lambda msg, ns=namespace: self.vfr_hud_callback(msg, ns), qos_profile=SENSOR_QOS) - - self.clients = {namespace: { - "set_mode": self.node.create_client(SetMode, f'/{namespace}/set_mode'), - "arming": self.node.create_client(CommandBool, f'/{namespace}/cmd/arming'), - "takeoff": self.node.create_client(CommandTOL, f'/{namespace}/cmd/takeoff'), - "land": self.node.create_client(CommandTOL, f'/{namespace}/cmd/land'), - "local_position": self.node.create_publisher(PoseStamped, f'/{namespace}/setpoint_position/local', 10) - } for namespace in namespaces} + # 設置需要監控的訊息類型 + 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 and rclpy.ok(): - self.executor.spin_once(timeout_sec=0.1) + while self.running: + try: + current_time = time.time() + msg = self.connection.recv_msg() + 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) + + # 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) + self.state_signal.emit(namespace, mode) + + elif msg_type == "BATTERY_STATUS": + percentage = msg.battery_remaining/100 + self.battery_signal.emit(namespace, percentage) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.gps_signal.emit(namespace, latitude, longitude) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + satellites_visible = msg.satellites_visible + self.gps_status_signal.emit(namespace, fix_type, satellites_visible) + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.local_position_signal.emit(namespace, x, y, z) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.imu_signal.emit(namespace, pitch) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + 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}") def stop(self): self.running = False - self.executor.shutdown() - self.node.destroy_node() - - def state_callback(self, msg, namespace): - self.state_signal.emit(namespace, msg.mode) - - def battery_callback(self, msg, namespace): - self.battery_signal.emit(namespace, msg.percentage) - - def gps_callback(self, msg, namespace): - self.gps_signal.emit(namespace, msg.latitude, msg.longitude) - - def gps_status_callback(self, msg, namespace): - self.gps_status_signal.emit(namespace, msg.fix_type, msg.satellites_visible) + self.connection.close() - def local_position_callback(self, msg, namespace): - self.local_position_signal.emit(namespace, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) - - def imu_callback(self, msg, namespace): - self.imu_signal.emit(namespace, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w) - - def heading_callback(self, msg, namespace): - self.hdg_signal.emit(namespace, msg.data) - - def vfr_hud_callback(self, msg, namespace): - self.groundspeed_signal.emit(namespace, msg.groundspeed) - def set_mode(self, namespace, mode): - req = SetMode.Request() - req.custom_mode = mode - self.clients[namespace]["set_mode"].call_async(req) - self.node.get_logger().info(f'{namespace}: 模式切換: {mode}') - - def arm(self, namespace, arm: bool): - req = CommandBool.Request() - req.value = arm - self.clients[namespace]["arming"].call_async(req) - self.node.get_logger().info(f'{namespace}: {"解鎖" if arm else "上鎖"}') + sysid = int(namespace.replace('UAV', '')) + if mode == 'STABILIZE': + mode = 0 + elif mode == 'AUTO': + mode = 3 + elif mode == 'GUIDED': + mode = 4 + elif mode == 'LOITER': + mode = 5 + 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.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + 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): - req = CommandTOL.Request() - req.altitude = altitude - req.latitude = 0.0 - req.longitude = 0.0 - req.yaw = 0.0 - req.min_pitch = 0.0 - self.clients[namespace]["takeoff"].call_async(req) - self.node.get_logger().info(f'{namespace}: 起飛到高度 {altitude}') + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + 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): - req = CommandTOL.Request() - req.altitude = 0.0 - req.latitude = 0.0 - req.longitude = 0.0 - req.yaw = 0.0 - req.min_pitch = 0.0 - self.clients[namespace]["land"].call_async(req) - self.node.get_logger().info(f'{namespace}: 降落') + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + 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): - pose_msg = PoseStamped() - pose_msg.pose.position.x = x - pose_msg.pose.position.y = y - pose_msg.pose.position.z = z - self.clients[namespace]["local_position"].publish(pose_msg) - self.node.get_logger().info(f'{namespace}: Published local position: X={x}, Y={y}, Z={z}') + 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 + 0b110111111000, # Mask + y, x, -z, # Position + 0, 0, 0, # Velocity + 0, 0, 0, # Acceleration + 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 = ROS2Worker(self.namespaces) + self.workers = MAVLinkWorker() + self.namespaces = self.workers.namespaces self.initUI() - # 訂閱所有數據更新 + + # Connect signals to update the UI self.workers.state_signal.connect(self.update_state) self.workers.battery_signal.connect(self.update_battery) self.workers.gps_signal.connect(self.update_gps) @@ -144,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): @@ -174,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 的垂直佈局中 @@ -194,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() @@ -225,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) @@ -252,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 = { @@ -269,22 +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"Pitch：{pitch:.2f}°") + + def update_hdg(self, namespace, heading): + 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_imu(self, namespace, x, y, z, w): - q = np.array([x, y, z, w]) - pitch = self.quaternion_to_euler(q) - self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.2f}°") + def update_loss(self, namespace, loss): + self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%") - def update_hdg(self, namespace, data): - self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{data:.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: @@ -295,17 +477,6 @@ class DroneControlApp(QMainWindow): except Exception as e: QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}") - def set_local_position(self): - try: - x = float(self.positionX.text().strip()) - y = float(self.positionY.text().strip()) - z = float(self.positionZ.text().strip()) - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_local_position(namespace, x, y, z) - except ValueError: - QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") - def arm_drone(self): try: for namespace in self.namespaces: @@ -329,15 +500,35 @@ class DroneControlApp(QMainWindow): if self.uav_checkboxes[namespace].isChecked(): self.workers.land(namespace) - def quaternion_to_euler(self, q): - # Convert quaternion to Euler angles (roll, pitch, yaw) - x, y, z, w = q + def set_local_position(self): + try: + x = float(self.positionX.text().strip()) + y = float(self.positionY.text().strip()) + z = float(self.positionZ.text().strip()) + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_local_position(namespace, x, y, z) + 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, "錯誤", "請輸入有效的數值！") - # Pitch (y-axis rotation) - pitch_y = np.arcsin(2.0 * (w * y - z * x)) - - return pitch_y - def main(args=None): rclpy.init(args=args) app = QtWidgets.QApplication(sys.argv) From b8965c0bd6d070791d51b0ac721c99eb38091593 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:02:50 +0800 Subject: [PATCH 2/3] Update 'src/unitdev01/unitdev01/interface.py' --- src/unitdev01/unitdev01/interface.py | 499 +++++++++------------------ 1 file changed, 154 insertions(+), 345 deletions(-) diff --git a/src/unitdev01/unitdev01/interface.py b/src/unitdev01/unitdev01/interface.py index 9078880..5699e46 100644 --- a/src/unitdev01/unitdev01/interface.py +++ b/src/unitdev01/unitdev01/interface.py @@ -1,289 +1,141 @@ import sys -import time -import math +import numpy as np import rclpy +from rclpy.executors import SingleThreadedExecutor from PyQt5 import QtWidgets from PyQt5.QtCore import QThread, pyqtSignal from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox -from pymavlink import mavutil - -class MAVLinkWorker(QThread): - state_signal = pyqtSignal(str, str) - battery_signal = pyqtSignal(str, float) - gps_signal = pyqtSignal(str, float, float) +from std_msgs.msg import Float64 +from mavros_msgs.srv import SetMode, CommandBool, CommandTOL +from mavros_msgs.msg import State, GPSRAW, VfrHud +from sensor_msgs.msg import NavSatFix, BatteryState, Imu +from geometry_msgs.msg import PoseStamped +from mavros.base import SENSOR_QOS + +class ROS2Worker(QThread): + state_signal = pyqtSignal(str, str) # 新增 namespace 參數 + battery_signal = pyqtSignal(str, float) # 新增 namespace 參數 + gps_signal = pyqtSignal(str, float, float) # 新增 namespace 參數 gps_status_signal = pyqtSignal(str, int, int) - local_position_signal = pyqtSignal(str, float, float, float) - imu_signal = pyqtSignal(str, float) + local_position_signal = pyqtSignal(str, float, float, float) # 新增 namespace 參數 + imu_signal = pyqtSignal(str, float, float, float, 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) + groundspeed_signal = pyqtSignal(str, float) # 新增 namespace 與 groundspeed 值 - def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): + def __init__(self, namespaces): super().__init__() - 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) + namespaces = ['uav1', 'uav2', 'uav3'] + self.node = rclpy.create_node('multi_uav_control_app') + self.executor = SingleThreadedExecutor() # 創建單執行器 + self.executor.add_node(self.node) # 添加節點到執行器 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 = {} - - for namespace in self.namespaces: - self.request_param(namespace, "SR1_EXTRA1") + for namespace in namespaces: + self.node.create_subscription(State, f'/{namespace}/state', + lambda msg, ns=namespace: self.state_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(BatteryState, f'/{namespace}/battery', + lambda msg, ns=namespace: self.battery_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(NavSatFix, f'/{namespace}/global_position/global', + lambda msg, ns=namespace: self.gps_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(GPSRAW, f'/{namespace}/gpsstatus/gps1/raw', + lambda msg, ns=namespace: self.gps_status_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(PoseStamped, f'/{namespace}/local_position/pose', + lambda msg, ns=namespace: self.local_position_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(Imu, f'/{namespace}/imu/data', + lambda msg, ns=namespace: self.imu_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(Float64, f'/{namespace}/global_position/compass_hdg', + lambda msg, ns=namespace: self.heading_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(VfrHud, f'/{namespace}/vfr_hud', + lambda msg, ns=namespace: self.vfr_hud_callback(msg, ns), qos_profile=SENSOR_QOS) + + self.clients = {namespace: { + "set_mode": self.node.create_client(SetMode, f'/{namespace}/set_mode'), + "arming": self.node.create_client(CommandBool, f'/{namespace}/cmd/arming'), + "takeoff": self.node.create_client(CommandTOL, f'/{namespace}/cmd/takeoff'), + "land": self.node.create_client(CommandTOL, f'/{namespace}/cmd/land'), + "local_position": self.node.create_publisher(PoseStamped, f'/{namespace}/setpoint_position/local', 10) + } for namespace in namespaces} def run(self): - while self.running: - try: - current_time = time.time() - msg = self.connection.recv_msg() - 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) - - # 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) - self.state_signal.emit(namespace, mode) - - elif msg_type == "BATTERY_STATUS": - percentage = msg.battery_remaining/100 - self.battery_signal.emit(namespace, percentage) - - elif msg_type == "GLOBAL_POSITION_INT": - latitude = msg.lat / 1e7 - longitude = msg.lon / 1e7 - self.gps_signal.emit(namespace, latitude, longitude) - - elif msg_type == "GPS_RAW_INT": - fix_type = msg.fix_type - satellites_visible = msg.satellites_visible - self.gps_status_signal.emit(namespace, fix_type, satellites_visible) - - elif msg_type == "LOCAL_POSITION_NED": - x = msg.y - y = msg.x - z = -msg.z - self.local_position_signal.emit(namespace, x, y, z) - - elif msg_type == "ATTITUDE": - pitch = math.degrees(msg.pitch) - self.imu_signal.emit(namespace, pitch) - - elif msg_type == "VFR_HUD": - groundspeed = msg.groundspeed - heading = msg.heading - 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}") + while self.running and rclpy.ok(): + self.executor.spin_once(timeout_sec=0.1) def stop(self): self.running = False - self.connection.close() + self.executor.shutdown() + self.node.destroy_node() + + def state_callback(self, msg, namespace): + self.state_signal.emit(namespace, msg.mode) + + def battery_callback(self, msg, namespace): + self.battery_signal.emit(namespace, msg.percentage) + + def gps_callback(self, msg, namespace): + self.gps_signal.emit(namespace, msg.latitude, msg.longitude) + + def gps_status_callback(self, msg, namespace): + self.gps_status_signal.emit(namespace, msg.fix_type, msg.satellites_visible) + def local_position_callback(self, msg, namespace): + self.local_position_signal.emit(namespace, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + + def imu_callback(self, msg, namespace): + self.imu_signal.emit(namespace, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w) + + def heading_callback(self, msg, namespace): + self.hdg_signal.emit(namespace, msg.data) + + def vfr_hud_callback(self, msg, namespace): + self.groundspeed_signal.emit(namespace, msg.groundspeed) + def set_mode(self, namespace, mode): - sysid = int(namespace.replace('UAV', '')) - if mode == 'STABILIZE': - mode = 0 - elif mode == 'AUTO': - mode = 3 - elif mode == 'GUIDED': - mode = 4 - elif mode == 'LOITER': - mode = 5 - 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.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - 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 - ) + req = SetMode.Request() + req.custom_mode = mode + self.clients[namespace]["set_mode"].call_async(req) + self.node.get_logger().info(f'{namespace}: 模式切換: {mode}') + + def arm(self, namespace, arm: bool): + req = CommandBool.Request() + req.value = arm + self.clients[namespace]["arming"].call_async(req) + self.node.get_logger().info(f'{namespace}: {"解鎖" if arm else "上鎖"}') def takeoff(self, namespace, altitude): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - 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) - ) + req = CommandTOL.Request() + req.altitude = altitude + req.latitude = 0.0 + req.longitude = 0.0 + req.yaw = 0.0 + req.min_pitch = 0.0 + self.clients[namespace]["takeoff"].call_async(req) + self.node.get_logger().info(f'{namespace}: 起飛到高度 {altitude}') def land(self, namespace): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21) - 0, - 0, 0, 0, 0, - 0, 0, 0 - ) + req = CommandTOL.Request() + req.altitude = 0.0 + req.latitude = 0.0 + req.longitude = 0.0 + req.yaw = 0.0 + req.min_pitch = 0.0 + self.clients[namespace]["land"].call_async(req) + self.node.get_logger().info(f'{namespace}: 降落') def set_local_position(self, namespace, x, y, z): - 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 - 0b110111111000, # Mask - y, x, -z, # Position - 0, 0, 0, # Velocity - 0, 0, 0, # Acceleration - 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) + pose_msg = PoseStamped() + pose_msg.pose.position.x = x + pose_msg.pose.position.y = y + pose_msg.pose.position.z = z + self.clients[namespace]["local_position"].publish(pose_msg) + self.node.get_logger().info(f'{namespace}: Published local position: X={x}, Y={y}, Z={z}') class DroneControlApp(QMainWindow): def __init__(self): super().__init__() - self.workers = MAVLinkWorker() - self.namespaces = self.workers.namespaces + self.namespaces = ['uav1', 'uav2', 'uav3'] + self.workers = ROS2Worker(self.namespaces) self.initUI() - - # Connect signals to update the UI + # 訂閱所有數據更新 self.workers.state_signal.connect(self.update_state) self.workers.battery_signal.connect(self.update_battery) self.workers.gps_signal.connect(self.update_gps) @@ -292,10 +144,7 @@ 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): @@ -325,19 +174,15 @@ class DroneControlApp(QMainWindow): # 狀態顯示 self.uav_labels[namespace] = { - "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：等待數據...") + "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：等待數據..."), } # 把每個資訊添加到該 UAV 的垂直佈局中 @@ -349,16 +194,6 @@ 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() @@ -390,26 +225,19 @@ 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) - 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) + main_layout.addWidget(self.landButton) # 設置主佈局 central_widget = QWidget(self) @@ -424,13 +252,13 @@ class DroneControlApp(QMainWindow): event.accept() def update_state(self, namespace, mode): - self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") + self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}") def update_battery(self, namespace, percentage): - self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%") + self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%") def update_gps(self, namespace, latitude, longitude): - self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°") + self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°") def update_gps_status(self, namespace, fix_type, satellites_visible): fix_type_str = { @@ -441,32 +269,22 @@ class DroneControlApp(QMainWindow): 4: "RTK Float", 5: "RTK Fix" }.get(fix_type, "Unknown") - self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}") - self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}") + self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}") + self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{satellites_visible}") def update_local_position(self, namespace, x, y, z): - 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"Pitch：{pitch:.2f}°") - - def update_hdg(self, namespace, heading): - self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°") + self.uav_labels[namespace]["local_position"].setText(f"{namespace} Local：({x:.2f}, {y:.2f}, {z:.2f})") def update_groundspeed(self, namespace, groundspeed): - 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") + self.uav_labels[namespace]["groundspeed"].setText(f"{namespace} 地面速度：{groundspeed:.2f} m/s") - def update_loss(self, namespace, loss): - self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%") + def update_imu(self, namespace, x, y, z, w): + q = np.array([x, y, z, w]) + pitch = self.quaternion_to_euler(q) + self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.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 update_hdg(self, namespace, data): + self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{data:.2f}°") def change_mode(self): try: @@ -477,6 +295,17 @@ class DroneControlApp(QMainWindow): except Exception as e: QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}") + def set_local_position(self): + try: + x = float(self.positionX.text().strip()) + y = float(self.positionY.text().strip()) + z = float(self.positionZ.text().strip()) + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_local_position(namespace, x, y, z) + except ValueError: + QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + def arm_drone(self): try: for namespace in self.namespaces: @@ -500,35 +329,15 @@ class DroneControlApp(QMainWindow): if self.uav_checkboxes[namespace].isChecked(): self.workers.land(namespace) - def set_local_position(self): - try: - x = float(self.positionX.text().strip()) - y = float(self.positionY.text().strip()) - z = float(self.positionZ.text().strip()) - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_local_position(namespace, x, y, z) - 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 quaternion_to_euler(self, q): + # Convert quaternion to Euler angles (roll, pitch, yaw) + x, y, z, w = q + # Pitch (y-axis rotation) + pitch_y = np.arcsin(2.0 * (w * y - z * x)) + + return pitch_y + def main(args=None): rclpy.init(args=args) app = QtWidgets.QApplication(sys.argv) From ee4cf1d07cd75d1214e7a15e81b13cda9885f94c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:03:11 +0800 Subject: [PATCH 3/3] 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)