from rclpy.node import Node from PyQt6.QtCore import QObject, pyqtSignal import math import re import threading from threading import Lock from concurrent.futures import ThreadPoolExecutor import asyncio import websockets import json import socket import sys import os import traceback from pymavlink import mavutil from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu from std_msgs.msg import Float64, String from mavros_msgs.msg import State, VfrHud from mavros_msgs.srv import CommandBool, CommandTOL # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入） _src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) if _src_path not in sys.path: sys.path.insert(0, _src_path) # 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API） try: from fc_network_apps.longCommand import CommandLongClient except ImportError as e: import traceback print(f"⚠️ 警告: 無法導入 CommandLongClient") print(f" 错误: {e}") print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装") print(f" 完整堆栈跟踪:") traceback.print_exc() CommandLongClient = None class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) class UDPMavlinkReceiver(threading.Thread): """UDP MAVLink 接收器""" def __init__(self, ip, port, signals, connection_name, monitor=None): super().__init__(daemon=True) self.ip = ip self.port = port self.signals = signals self.connection_name = connection_name self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 self.running = False self.sock = None def run(self): """執行 UDP 接收循環""" self.running = True try: print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") # 創建 MAVLink 連接 mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') while self.running: try: msg = mav.recv_match(blocking=True, timeout=1.0) if msg is None: continue self.process_mavlink_message(msg) except socket.timeout: continue except Exception as e: print(f"Error receiving MAVLink message: {e}") except Exception as e: print(f"UDP receiver error: {e}") finally: if self.sock: self.sock.close() def process_mavlink_message(self, msg): """處理 MAVLink 訊息""" try: msg_type = msg.get_type() system_id = msg.get_srcSystem() drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": # 先發送連接類型資訊 self.signals.update_signal.emit('connection_type', drone_id, { 'type': 'UDP' }) mode = mavutil.mode_string_v10(msg) armed = bool(msg.base_mode & 128) self.signals.update_signal.emit('state', drone_id, { 'mode': mode, 'armed': armed }) elif msg_type == "BATTERY_STATUS": voltage = msg.voltages[0] / 1000 self.signals.update_signal.emit('battery', drone_id, { 'voltage': voltage }) elif msg_type == "GLOBAL_POSITION_INT": latitude = msg.lat / 1e7 longitude = msg.lon / 1e7 self.signals.update_signal.emit('gps', drone_id, { 'lat': latitude, 'lon': longitude }) elif msg_type == "GPS_RAW_INT": fix_type = msg.fix_type elif msg_type == "LOCAL_POSITION_NED": x = msg.y y = msg.x z = -msg.z self.signals.update_signal.emit('local_pose', drone_id, { 'x': x, 'y': y, 'z': z }) self.signals.update_signal.emit('altitude', drone_id, { 'altitude': z }) self.signals.update_signal.emit('velocity', drone_id, { 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz }) elif msg_type == "ATTITUDE": # 從 MAVLink 訊息中提取並轉為角度 pitch = math.degrees(msg.pitch) roll = math.degrees(msg.roll) yaw = math.degrees(msg.yaw) self.signals.update_signal.emit('attitude', drone_id, { 'pitch': pitch, 'roll': roll, 'yaw': yaw, 'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed) }) elif msg_type == "VFR_HUD": self.signals.update_signal.emit('hud', drone_id, { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, 'throttle': msg.throttle, 'alt': msg.alt, 'climb': msg.climb }) except Exception as e: print(f"Error processing MAVLink message: {e}") def stop(self): """停止接收器""" self.running = False class SerialMavlinkReceiver(threading.Thread): """串口 MAVLink 接收器""" def __init__(self, port, baudrate, signals, connection_name, monitor=None): super().__init__(daemon=True) self.port = port self.baudrate = baudrate self.signals = signals self.connection_name = connection_name self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 self.running = False self.mav = None def run(self): """執行串口接收循環""" self.running = True try: print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") # 創建 MAVLink 串口連接 self.mav = mavutil.mavlink_connection( self.port, baud=self.baudrate, source_system=255 ) print(f"Waiting for heartbeat from {self.port}...") self.mav.wait_heartbeat() print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}") while self.running: try: msg = self.mav.recv_match(blocking=True, timeout=1.0) if msg is None: continue self.process_mavlink_message(msg) except Exception as e: if self.running: print(f"Error receiving MAVLink message from serial: {e}") except Exception as e: print(f"Serial receiver error: {e}") finally: if self.mav: try: self.mav.close() except: pass def process_mavlink_message(self, msg): """處理 MAVLink 訊息""" try: msg_type = msg.get_type() system_id = msg.get_srcSystem() drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": # 先發送連接類型資訊 self.signals.update_signal.emit('connection_type', drone_id, { 'type': 'Serial' }) mode = mavutil.mode_string_v10(msg) armed = bool(msg.base_mode & 128) self.signals.update_signal.emit('state', drone_id, { 'mode': mode, 'armed': armed }) elif msg_type == "BATTERY_STATUS": voltage = msg.voltages[0] / 1000 self.signals.update_signal.emit('battery', drone_id, { 'voltage': voltage }) elif msg_type == "GLOBAL_POSITION_INT": latitude = msg.lat / 1e7 longitude = msg.lon / 1e7 self.signals.update_signal.emit('gps', drone_id, { 'lat': latitude, 'lon': longitude }) elif msg_type == "GPS_RAW_INT": fix_type = msg.fix_type elif msg_type == "LOCAL_POSITION_NED": x = msg.y y = msg.x z = -msg.z self.signals.update_signal.emit('local_pose', drone_id, { 'x': x, 'y': y, 'z': z }) self.signals.update_signal.emit('altitude', drone_id, { 'altitude': z }) self.signals.update_signal.emit('velocity', drone_id, { 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz }) elif msg_type == "ATTITUDE": # 從 MAVLink 訊息中提取並轉為角度 pitch = math.degrees(msg.pitch) roll = math.degrees(msg.roll) yaw = math.degrees(msg.yaw) self.signals.update_signal.emit('attitude', drone_id, { 'pitch': pitch, 'roll': roll, 'yaw': yaw, 'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed) }) elif msg_type == "VFR_HUD": self.signals.update_signal.emit('hud', drone_id, { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, 'throttle': msg.throttle, 'alt': msg.alt, 'climb': msg.climb }) except Exception as e: print(f"Error processing MAVLink message from serial: {e}") def stop(self): """停止接收器""" self.running = False class WebSocketMavlinkReceiver(threading.Thread): """WebSocket MAVLink 接收器""" def __init__(self, url, signals, connection_name, monitor=None): super().__init__(daemon=True) self.url = url self.signals = signals self.connection_name = connection_name self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False self.max_retries = 5 self.base_delay = 1.0 def run(self): """執行 WebSocket 接收循環""" self.running = True asyncio.set_event_loop(asyncio.new_event_loop()) asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) async def ws_client_loop(self): """WebSocket 連接的主循環""" retry_count = 0 print(f"Starting WebSocket client for {self.connection_name} at {self.url}") while self.running and retry_count < self.max_retries: try: async with websockets.connect(self.url) as websocket: print(f"WebSocket {self.connection_name} connected to {self.url}") retry_count = 0 # 重置重試計數 async for message in websocket: if not self.running: break try: data = json.loads(message) data['_connection_source'] = self.connection_name self.process_websocket_message(data) except json.JSONDecodeError as e: print(f"WebSocket {self.connection_name} JSON decode error: {e}") except Exception as e: print(f"WebSocket {self.connection_name} message processing error: {e}") except websockets.exceptions.ConnectionClosedError: print(f"WebSocket {self.connection_name} connection closed") if self.running: retry_count += 1 if retry_count < self.max_retries: delay = self.base_delay * (2 ** min(retry_count, 4)) print(f"Reconnecting in {delay}s...") await asyncio.sleep(delay) else: break except Exception as e: retry_count += 1 if retry_count < self.max_retries and self.running: delay = self.base_delay * (2 ** min(retry_count, 4)) print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") await asyncio.sleep(delay) else: break print(f"WebSocket client {self.connection_name} stopped") def process_websocket_message(self, data): """處理 WebSocket 訊息""" try: system_id = data.get('system_id') if not system_id: return drone_id = f"s{self.socket_id}_{system_id}" # 模式 if 'mode' in data: # 先發送連接類型資訊 self.signals.update_signal.emit('connection_type', drone_id, { 'type': 'WS' }) self.signals.update_signal.emit('state', drone_id, { 'mode': data['mode'], }) # 電池 if 'battery' in data: self.signals.update_signal.emit('battery', drone_id, { 'percentage': data['battery'] }) # 位置 if 'position' in data: pos = data['position'] self.signals.update_signal.emit('gps', drone_id, { 'lat': pos.get('lat', 0), 'lon': pos.get('lon', 0) }) # Local position - 設定 x, y 為 0.0 self.signals.update_signal.emit('local_pose', drone_id, { 'x': 0.0, 'y': 0.0, 'z': 0.0 }) # Altitude - 設定為 0.0 self.signals.update_signal.emit('altitude', drone_id, { 'altitude': 0.0 }) # 航向 if 'heading' in data: self.signals.update_signal.emit('hud', drone_id, { 'heading': data['heading'], 'groundspeed': 0.0 }) except Exception as e: print(f"WebSocket message processing error: {e}") def stop(self): """停止接收器""" self.running = False class DroneMonitor(Node): # Subscribe to drone ROS2 topics _instance = None # Singleton pattern to prevent duplicate nodes def __init__(self): # Use a unique node name with timestamp to avoid conflicts on restart import time node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}' super().__init__(node_name) self.signals = DroneSignals() self.drone_topics = {} self.lock = Lock() self.arm_clients = {} self.takeoff_clients = {} self.setpoint_pubs = {} self.selected_drones = set() self.latest_data = {} # 定義需要過濾的模式 self.filtered_modes = ['Mode(0x000000c0)'] # WebSocket 接收器列表 self.ws_receivers = [] # 串口接收器列表 # ================================================================================ # 【新增】儲存 GPS 資料的字典 # ================================================================================ self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} # ================================================================================ # ================================================================================ # 【新增】Socket ID 重新分配機制 (從 0 開始) # ================================================================================ self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id} self.socket_id_counter = 0 # 當前分配到的最大socket_id self.socket_id_lock = Lock() # 線程安全鎖 # ================================================================================ # ================================================================================ # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取) # ================================================================================ self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'} self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0} # ================================================================================ self.serial_receivers = [] # ================================================================================ # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀） # ================================================================================ self.command_long_client = None try: self.command_long_client = CommandLongClient() except Exception as e: print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}") self.command_long_client = None # ================================================================================ # 主题检测定时器 self.create_timer(1.0, self.scan_topics) def get_next_socket_id(self): """获取下一个可用的 socket_id（从 0 开始连续分配）""" with self.socket_id_lock: current_id = self.socket_id_counter self.socket_id_counter += 1 return current_id def get_or_assign_socket_id(self, original_socket_id): """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配） 同一個原始 socket_id 會得到同一個分配的 ID """ original_socket_id = str(original_socket_id) with self.socket_id_lock: if original_socket_id not in self.socket_id_mapping: # 分配新的 socket_id self.socket_id_mapping[original_socket_id] = self.socket_id_counter self.socket_id_counter += 1 return self.socket_id_mapping[original_socket_id] def scan_topics(self): topics = self.get_topic_names_and_types() drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)') found_drones = set() for topic_name, _ in topics: if match := drone_pattern.match(topic_name): sys_id, topic_type = match.groups() found_drones.add(sys_id) with self.lock: self.drone_topics.setdefault(sys_id, set()).add(topic_type) for sys_id in found_drones: # 为每个 sys_id 分配 socket_id（如果还没有分配） # 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖 if sys_id not in self.sys_to_socket_id: # 暂时所有 ROS2 topic 共享同一个 socket_id = 0 self.sys_to_socket_id[sys_id] = 0 if not hasattr(self, f'drone_{sys_id}_subs'): self.setup_drone(sys_id) def setup_drone(self, sys_id): # sys_id 格式： sys11, sys12, ... base_topic = f'/fc_network/vehicle/{sys_id}' # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務) self.arm_clients[sys_id] = self.create_client( CommandBool, f'{base_topic}/cmd/arming' ) self.takeoff_clients[sys_id] = self.create_client( CommandTOL, f'{base_topic}/cmd/takeoff' ) # Add setpoint publisher self.setpoint_pubs[sys_id] = self.create_publisher( Point, f'{base_topic}/setpoint_position/local', 10 ) subs = { 'battery': self.create_subscription( BatteryState, f'{base_topic}/battery', lambda msg, sid=sys_id: self.battery_callback(sid, msg), 10 ), 'position': self.create_subscription( NavSatFix, f'{base_topic}/position', lambda msg, sid=sys_id: self.gps_callback(sid, msg), 10 ), 'summary': self.create_subscription( String, f'{base_topic}/summary', lambda msg, sid=sys_id: self.summary_callback(sid, msg), 10 ), 'vfr_hud': self.create_subscription( VfrHud, f'{base_topic}/vfr_hud', lambda msg, sid=sys_id: self.hud_callback(sid, msg), 10 ) } setattr(self, f'drone_{sys_id}_subs', subs) # ================================================================================ # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式） # ================================================================================ MODE_MAPPING = { "STABILIZE": 0, "ACRO": 1, "ALT_HOLD": 2, "AUTO": 3, "GUIDED": 4, "LOITER": 5, "RTL": 6, "CIRCLE": 7, "POSITION": 8, "LAND": 9, "OF_LOITER": 10, "DRIFT": 11, "SPORT": 13, "FLIP": 14, "AUTOTUNE": 15, "POSHOLD": 16, "BRAKE": 17, "THROW": 18, "AVOID_ADSB": 19, "GUIDED_NOGPS": 20, "SMART_RTL": 21, } # ================================================================================ async def set_mode(self, drone_id, mode_name): """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）""" try: # 解析 drone_id 提取 sysid parts = drone_id.split('_') if len(parts) < 2: print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) # 獲取模式對應的 custom_mode 值 custom_mode = self.MODE_MAPPING.get(mode_name) if custom_mode is None: print(f"❌ [SET_MODE] 未知模式: {mode_name}") return False print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})") if not self.command_long_client: print(f"❌ [SET_MODE] CommandLongClient 未初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） result = await self.command_long_client.change_mode_async( target_sysid=sysid, custom_mode=float(custom_mode), target_compid=0, base_mode=1.0, timeout_sec=2.0, ) if result and result.success: print(f"✅ [SET_MODE] 模式切換成功") return True else: print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})") return False except Exception as e: print(f"❌ [SET_MODE] 例外錯誤: {e}") traceback.print_exc() return False async def arm_drone(self, drone_id, arm): """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）""" try: # 解析 drone_id 提取 sysid parts = drone_id.split('_') if len(parts) < 2: print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) action_name = "解鎖" if arm else "上鎖" print(f"\n📢 [ARM] {drone_id} → {action_name}") if not self.command_long_client: print(f"❌ [ARM] CommandLongClient 未初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） result = await self.command_long_client.arm_disarm_async( target_sysid=sysid, arm=arm, target_compid=0, timeout_sec=2.0, ) if result and result.success: print(f"✅ [ARM] {action_name}成功") return True else: print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})") return False except Exception as e: print(f"❌ [ARM] 例外錯誤: {e}") traceback.print_exc() return False async def takeoff_drone(self, drone_id, altitude): """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）""" try: # 解析 drone_id 提取 sysid parts = drone_id.split('_') if len(parts) < 2: print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)") if not self.command_long_client: print(f"❌ [TAKEOFF] CommandLongClient 未初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） result = await self.command_long_client.takeoff_async( target_sysid=sysid, altitude_m=float(altitude), target_compid=0, timeout_sec=2.0, ) if result and result.success: print(f"✅ [TAKEOFF] 起飛成功") return True else: print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})") return False except Exception as e: print(f"❌ [TAKEOFF] 例外錯誤: {e}") traceback.print_exc() return False def send_setpoint(self, drone_id, x, y, z): """Send setpoint position command""" if drone_id not in self.setpoint_pubs: return False msg = Point() msg.x = float(x) msg.y = float(y) msg.z = float(z) self.setpoint_pubs[drone_id].publish(msg) return True def quaternion_to_euler(self, q): sinr_cosp = 2 * (q.w * q.x + q.y * q.z) cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) roll = math.atan2(sinr_cosp, cosr_cosp) sinp = 2 * (q.w * q.y - q.z * q.x) pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) siny_cosp = 2 * (q.w * q.z + q.x * q.y) cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) yaw = math.atan2(siny_cosp, cosy_cosp) return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) # callbacks def attitude_callback(self, drone_id, msg): if hasattr(msg, 'orientation'): roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) self.latest_data[(drone_id, 'attitude')] = { 'roll': roll, 'pitch': pitch, 'yaw': yaw, 'rates': (msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z) } def battery_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return self.latest_data[(actual_drone_id, 'battery')] = { 'voltage': msg.voltage } def state_callback(self, drone_id, msg): mode = msg.mode if mode in self.filtered_modes: return self.latest_data[(drone_id, 'state')] = { 'mode': msg.mode, 'armed': msg.armed } def summary_callback(self, sys_id, msg): """處理 summary topic (JSON 格式)""" try: data = json.loads(msg.data) mode = data.get('mode_name', 'UNKNOWN') if mode in self.filtered_modes: return # 從 summary 獲取原始 socket_id，並映射到分配的 socket_id original_socket_id = data.get('socket_id') if original_socket_id is not None: # 使用原始 socket_id 獲取或分配統一的 socket_id assigned_socket_id = self.get_or_assign_socket_id(original_socket_id) else: # 如果沒有 socket_id，使用 sys_to_socket_id 映射 assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0) sysid = data.get('sysid') if sysid is not None: actual_drone_id = f's{assigned_socket_id}_{sysid}' # ================================================================================ # 【關鍵】保存 sys_id 到 actual_drone_id 的映射 # ================================================================================ self.sys_to_actual_id[sys_id] = actual_drone_id # ================================================================================ else: # 如果沒有 sysid，使用 sys_id 中的數字 sys_num = sys_id.replace('sys', '') actual_drone_id = f's{assigned_socket_id}_{sys_num}' self.sys_to_actual_id[sys_id] = actual_drone_id # 先發送連接類型資訊 self.signals.update_signal.emit('connection_type', actual_drone_id, { 'type': 'ROS2' }) self.latest_data[(actual_drone_id, 'state')] = { 'mode': mode, 'armed': data.get('armed', False), 'socket_id': original_socket_id, 'sysid': sysid, 'vehicle_type': data.get('vehicle_type'), 'autopilot': data.get('autopilot'), 'gps_fix': data.get('gps_fix'), 'gps_fix_type': data.get('gps_fix'), 'connected': data.get('connected') } except json.JSONDecodeError as e: print(f"Error parsing summary JSON for {sys_id}: {e}") except Exception as e: print(f"Error in summary_callback for {sys_id}: {e}") def gps_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return self.latest_data[(actual_drone_id, 'gps')] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude } # ================================================================================ # 【新增】儲存 GPS 資料到 drone_gps 字典 # ================================================================================ self.drone_gps[actual_drone_id] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude } # ================================================================================ def local_vel_callback(self, drone_id, msg): self.latest_data[(drone_id, 'velocity')] = { 'vx': msg.x, 'vy': msg.y, 'vz': msg.z } def altitude_callback(self, drone_id, msg): self.latest_data[(drone_id, 'altitude')] = { 'altitude': msg.data } def local_pose_callback(self, drone_id, msg): self.latest_data[(drone_id, 'local_pose')] = { 'x': msg.x, 'y': msg.y, 'z': msg.z } def hud_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return self.latest_data[(actual_drone_id, 'hud')] = { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, 'throttle': msg.throttle, 'alt': msg.altitude, 'climb': msg.climb } def loss_rate_callback(self, drone_id, msg): self.latest_data[(drone_id, 'loss_rate')] = { 'loss_rate': msg.data } def ping_callback(self, drone_id, msg): self.latest_data[(drone_id, 'ping')] = { 'ping': msg.data } def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): """啟動串口 MAVLink 連接""" connection_name = f"Serial_{port.replace('/', '_')}" receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver.start() self.serial_receivers.append(receiver) print(f"Started serial connection on {port} at {baudrate} baud") return receiver