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 try: import serial except ImportError: serial = None from pymavlink import mavutil from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped from sensor_msgs.msg import BatteryState, NavSatFix, Imu from std_msgs.msg import Float64, String from mavros_msgs.msg import State, VfrHud from nav_msgs.msg import Odometry from mavros_msgs.srv import CommandBool, CommandTOL def _log(level, message): print(f"[{level}] {message}") # 確保 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 _log("WARN", "無法導入 CommandLongClient") _log("ERROR", f"錯誤: {e}") _log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整") traceback.print_exc() CommandLongClient = None # 導入 fc_network_apps 的 navigation（PositionTargetGlobalInt / Offboard goto） try: from fc_network_apps.navigation import PositionTargetGlobalIntClient except ImportError as e: import traceback _log("WARN", "無法導入 PositionTargetGlobalIntClient") _log("ERROR", f"錯誤: {e}") traceback.print_exc() PositionTargetGlobalIntClient = None try: from fc_interfaces.msg import AttitudeRaw except ImportError as e: _log("WARN", "無法導入 AttitudeRaw") _log("ERROR", f"錯誤: {e}") AttitudeRaw = None try: from fc_interfaces.msg import GnssRaw except ImportError as e: _log("WARN", "無法導入 GnssRaw") _log("ERROR", f"錯誤: {e}") GnssRaw = None try: from fc_interfaces.msg import SystemDiagnosticsRaw except ImportError as e: _log("WARN", f"SystemDiagnosticsRaw 尚不可用，略過 sys_diags 訂閱: {e}") SystemDiagnosticsRaw = None try: from fc_interfaces.msg import FcNetworkLog except ImportError as e: _log("WARN", f"FcNetworkLog 尚不可用，略過 FC Network 紀錄訂閱: {e}") FcNetworkLog = None class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) class JsonTelemetryProcessor: """共用 WebSocket JSON telemetry 轉換器。 Canonical JSON fields: { "system_id": 1, "mode": "GUIDED", "battery": 85, "position": {"lat": 24.0, "lon": 120.0}, "heading": 90 } Serial JSON also accepts the compact UAV.py shape: {"s": 1, "m": "GUIDED", "a": 1, "b": 85, "h": 10.0, "v": 4.2, "p": [24.0, 120.0], "ypr": [90.0, 0.0, 0.0], "g": 3, "d": [0.8, 1.2]} """ def _emit_json_connection_type(self, drone_id): self.signals.update_signal.emit('connection_type', drone_id, { 'type': self.source_type }) def process_json_telemetry_message(self, data): """處理 WebSocket JSON 格式的遙測資料。""" try: if isinstance(data, list): for item in data: if isinstance(item, dict): self.process_json_telemetry_message(item) return if not isinstance(data, dict): return system_id = data.get('system_id', data.get('sysid', data.get('s'))) if system_id is None: return drone_id = f"s{self.socket_id}_{system_id}" self._emit_json_connection_type(drone_id) mode = data.get('mode', data.get('mode_name', data.get('m'))) state = {} if mode is not None: state['mode'] = mode if 'armed' in data: state['armed'] = data.get('armed') elif 'a' in data: state['armed'] = bool(data.get('a')) if state: self.signals.update_signal.emit('state', drone_id, state) if 'battery' in data: battery = data['battery'] if isinstance(battery, dict): battery_data = {} if 'percentage' in battery: battery_data['percentage'] = battery.get('percentage') elif 'percent' in battery: battery_data['percentage'] = battery.get('percent') if 'voltage' in battery: battery_data['voltage'] = battery.get('voltage') elif 'voltage_v' in battery: battery_data['voltage'] = battery.get('voltage_v') if battery_data: self.signals.update_signal.emit('battery', drone_id, battery_data) else: self.signals.update_signal.emit('battery', drone_id, { 'percentage': battery }) elif 'battery_percentage' in data or 'battery_voltage' in data: battery_data = {} if 'battery_percentage' in data: battery_data['percentage'] = data.get('battery_percentage') if 'battery_voltage' in data: battery_data['voltage'] = data.get('battery_voltage') self.signals.update_signal.emit('battery', drone_id, battery_data) elif 'b' in data: self.signals.update_signal.emit('battery', drone_id, { 'percentage': data.get('b') }) pos = data.get('position') if isinstance(pos, dict): gps_data = { 'lat': pos.get('lat', pos.get('latitude', 0)), 'lon': pos.get('lon', pos.get('longitude', 0)), 'alt': pos.get('alt', pos.get('altitude', 0)) } self.signals.update_signal.emit('gps', drone_id, gps_data) elif 'lat' in data or 'latitude' in data: self.signals.update_signal.emit('gps', drone_id, { 'lat': data.get('lat', data.get('latitude', 0)), 'lon': data.get('lon', data.get('longitude', 0)), 'alt': data.get('alt', data.get('altitude', 0)) }) elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2: dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else [] gps_data = { 'lat': data['p'][0], 'lon': data['p'][1], 'alt': data.get('h', 0) } if 'g' in data: gps_data['fix_type'] = data.get('g') if len(dop) >= 1: gps_data['eph'] = dop[0] gps_data['hdop'] = dop[0] if len(dop) >= 2: gps_data['epv'] = dop[1] gps_data['vdop'] = dop[1] self.signals.update_signal.emit('gps', drone_id, gps_data) local = data.get('local_position', data.get('local_pose', data.get('local'))) if isinstance(local, dict): x = local.get('x', 0.0) y = local.get('y', 0.0) z = local.get('z', 0.0) 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 }) elif isinstance(pos, dict): alt = pos.get('alt', pos.get('altitude', 0.0)) self.signals.update_signal.emit('local_pose', drone_id, { 'x': 0.0, 'y': 0.0, 'z': alt }) self.signals.update_signal.emit('altitude', drone_id, { 'altitude': alt }) elif 'h' in data: height = data.get('h', 0.0) self.signals.update_signal.emit('local_pose', drone_id, { 'x': 0.0, 'y': 0.0, 'z': height }) self.signals.update_signal.emit('altitude', drone_id, { 'altitude': height }) elif ( isinstance(data.get('p'), (list, tuple)) or 'lat' in data or 'latitude' in data ): self.signals.update_signal.emit('local_pose', drone_id, { 'x': 0.0, 'y': 0.0, 'z': 0.0 }) self.signals.update_signal.emit('altitude', drone_id, { 'altitude': 0.0 }) velocity = data.get('velocity') if isinstance(velocity, dict): self.signals.update_signal.emit('velocity', drone_id, { 'vx': velocity.get('vx', velocity.get('x', 0.0)), 'vy': velocity.get('vy', velocity.get('y', 0.0)), 'vz': velocity.get('vz', velocity.get('z', 0.0)) }) elif 'v' in data: self.signals.update_signal.emit('velocity', drone_id, { 'vx': data.get('v', 0.0), 'vy': 0.0, 'vz': 0.0 }) attitude = data.get('attitude') if isinstance(attitude, dict): self.signals.update_signal.emit('attitude', drone_id, { 'roll': attitude.get('roll', 0.0), 'pitch': attitude.get('pitch', 0.0), 'yaw': attitude.get('yaw', 0.0), 'rates': attitude.get('rates', (0.0, 0.0, 0.0)) }) elif isinstance(data.get('ypr'), (list, tuple)) and len(data.get('ypr')) >= 3: yaw, pitch, roll = data['ypr'][0], data['ypr'][1], data['ypr'][2] self.signals.update_signal.emit('attitude', drone_id, { 'roll': roll, 'pitch': pitch, 'yaw': yaw, 'rates': (0.0, 0.0, 0.0) }) hud = data.get('hud', {}) if not isinstance(hud, dict): hud = {} if 'heading' in data: hud['heading'] = data.get('heading') elif 'y' in data: hud['heading'] = data.get('y') elif isinstance(data.get('ypr'), (list, tuple)) and len(data.get('ypr')) >= 1: hud['heading'] = data['ypr'][0] if 'v' in data and 'groundspeed' not in hud: hud['groundspeed'] = data.get('v') if 'h' in data and 'alt' not in hud and 'altitude' not in hud: hud['alt'] = data.get('h') if hud: self.signals.update_signal.emit('hud', drone_id, { 'heading': hud.get('heading', 0.0), 'groundspeed': hud.get('groundspeed', 0.0), 'airspeed': hud.get('airspeed', 0.0), 'throttle': hud.get('throttle', 0.0), 'alt': hud.get('alt', hud.get('altitude', 0.0)), 'climb': hud.get('climb', 0.0) }) except Exception as e: print(f"{self.source_type} JSON telemetry processing error: {e}") 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._socket_id_released = False 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() self._release_socket_id() 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 relative_alt = msg.relative_alt / 1000.0 self.signals.update_signal.emit('gps', drone_id, { 'lat': latitude, 'lon': longitude, 'alt': relative_alt, }) 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 self._release_socket_id() def _release_socket_id(self): if self.monitor and not self._socket_id_released: self.monitor.release_socket_id(self.socket_id) self._socket_id_released = True class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): """串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。""" 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._socket_id_released = False self.source_type = 'Serial' self.running = False self.serial_conn = None self.mav_parser = mavutil.mavlink.MAVLink(None) self.mav_parser.robust_parsing = True self.json_buffer = [] self.json_depth = 0 self.json_in_string = False self.json_escape = False self._detected_protocols = set() def run(self): """執行串口接收循環。""" self.running = True try: print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)") if serial is None: raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線") self.serial_conn = serial.Serial( self.port, self.baudrate, timeout=0.2 ) while self.running: try: chunk = self.serial_conn.read(256) if not chunk: continue for raw_byte in chunk: byte = bytes([raw_byte]) self._process_json_byte(byte) self._process_mavlink_byte(byte) except Exception as e: if self.running: print(f"Error receiving serial telemetry: {e}") except Exception as e: print(f"Serial receiver error: {e}") finally: if self.serial_conn: try: self.serial_conn.close() except Exception: pass self._release_socket_id() def _process_mavlink_byte(self, byte): try: msg = self.mav_parser.parse_char(byte) if msg is None: return if 'MAVLink' not in self._detected_protocols: print(f"Serial {self.connection_name} detected MAVLink") self._detected_protocols.add('MAVLink') self.process_mavlink_message(msg) except Exception: # MAVLink parser is deliberately fed the whole stream, including JSON bytes. return def _process_json_byte(self, byte): try: char = byte.decode('utf-8') except UnicodeDecodeError: if self.json_buffer: self._reset_json_framing() return if not self.json_buffer: if char.isspace(): return if char not in ('{', '['): return self.json_depth = 1 self.json_in_string = False self.json_escape = False self.json_buffer = [char] return self.json_buffer.append(char) if self.json_in_string: if self.json_escape: self.json_escape = False elif char == '\\': self.json_escape = True elif char == '"': self.json_in_string = False return if char == '"': self.json_in_string = True elif char in ('{', '['): self.json_depth += 1 elif char in ('}', ']'): self.json_depth -= 1 if self.json_depth > 0: return payload = ''.join(self.json_buffer) self._reset_json_framing() try: data = json.loads(payload) if 'JSON' not in self._detected_protocols: print(f"Serial {self.connection_name} detected JSON") self._detected_protocols.add('JSON') self.process_json_telemetry_message(data) except json.JSONDecodeError as e: print(f"Serial {self.connection_name} JSON decode error: {e}") def _reset_json_framing(self): self.json_buffer = [] self.json_depth = 0 self.json_in_string = False self.json_escape = False 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 relative_alt = msg.relative_alt / 1000.0 self.signals.update_signal.emit('gps', drone_id, { 'lat': latitude, 'lon': longitude, 'alt': relative_alt, }) 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 if self.serial_conn: try: self.serial_conn.close() except Exception: pass self._release_socket_id() def _release_socket_id(self): if self.monitor and not self._socket_id_released: self.monitor.release_socket_id(self.socket_id) self._socket_id_released = True class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): """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._socket_id_released = False self.source_type = 'WS' 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) if isinstance(data, dict): 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") self._release_socket_id() def process_websocket_message(self, data): """處理 WebSocket 訊息""" self.process_json_telemetry_message(data) def stop(self): """停止接收器""" self.running = False self._release_socket_id() def _release_socket_id(self): if self.monitor and not self._socket_id_released: self.monitor.release_socket_id(self.socket_id) self._socket_id_released = True 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.active_socket_ids = set() # 目前通訊連線使用中的 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 字典（為每個 drone 維護獨立的 client） # ================================================================================ # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件 self.command_long_clients = {} # {drone_id: CommandLongClient} self.client_lock = Lock() # 保護 clients 字典的訪問 self.client_counter = 0 # 用於生成唯一的 client 節點名稱 self.executor = None # 將在 gui.py 中設置，用於添加新的 clients # ================================================================================ # ================================================================================ # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto） # ================================================================================ self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient} self.pos_client_counter = 0 # ================================================================================ # 主题检测定时器 self.create_timer(1.0, self.scan_topics) # FC Network 使用專用結構化紀錄 topic，不再解析 /rosout。 self.fc_network_log_sub = None if FcNetworkLog is not None: self.fc_network_log_sub = self.create_subscription( FcNetworkLog, '/fc_network/logs', self.fc_network_log_callback, 200 ) def get_next_socket_id(self): """取得目前最小的未使用 socket_id（從 0 開始）。""" with self.socket_id_lock: socket_id = 0 while socket_id in self.active_socket_ids: socket_id += 1 self.active_socket_ids.add(socket_id) return socket_id def release_socket_id(self, socket_id): """釋放通訊連線使用的 socket_id，讓後續連線可重用最小空缺。""" try: socket_id = int(socket_id) except (TypeError, ValueError): return with self.socket_id_lock: if socket_id in getattr(self, 'socket_id_mapping', {}).values(): return if socket_id in getattr(self, 'sys_to_socket_id', {}).values(): return self.active_socket_ids.discard(socket_id) def get_or_assign_socket_id(self, original_socket_id): """ROS2 socket_id 映射。 有原始 socket_id=N 時優先使用 N；若 N 已被其他通訊占用， 才改用目前最小未使用 ID。同一個原始 socket_id 會得到同一個映射。 """ original_socket_id = str(original_socket_id) with self.socket_id_lock: if original_socket_id not in self.socket_id_mapping: try: preferred_socket_id = int(original_socket_id) except (TypeError, ValueError): preferred_socket_id = None if ( preferred_socket_id is not None and preferred_socket_id >= 0 and preferred_socket_id not in self.active_socket_ids ): socket_id = preferred_socket_id else: socket_id = 0 while socket_id in self.active_socket_ids: socket_id += 1 self.active_socket_ids.add(socket_id) self.socket_id_mapping[original_socket_id] = socket_id return self.socket_id_mapping[original_socket_id] def get_or_create_client(self, drone_id): """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件""" with self.client_lock: if drone_id not in self.command_long_clients: try: # 生成唯一的 client 節點名稱 self.client_counter += 1 unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}" client = CommandLongClient(node_name=unique_name) self.command_long_clients[drone_id] = client _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})") # 將新 client 添加到主執行器（這樣它的回調才能被處理） if self.executor: self.executor.add_node(client) _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") except TypeError: # 舊版 CommandLongClient 不支持 node_name 參數，使用預設 client = CommandLongClient() self.command_long_clients[drone_id] = client _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)") if self.executor: self.executor.add_node(client) _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") except Exception as e: _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}") return None return self.command_long_clients[drone_id] def get_or_create_position_client(self, drone_id): """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。""" if PositionTargetGlobalIntClient is None: return None with self.client_lock: if drone_id not in self.position_target_clients: try: self.pos_client_counter += 1 unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}" client = PositionTargetGlobalIntClient(node_name=unique_name) self.position_target_clients[drone_id] = client _log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})") if self.executor: self.executor.add_node(client) _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器") except Exception as e: _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}") return None return self.position_target_clients[drone_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: self.sys_to_socket_id[sys_id] = self.get_next_socket_id() subs_attr = f'drone_{sys_id}_subs' if not hasattr(self, subs_attr): self.setup_drone(sys_id) else: # 檢查既有訂閱是否包含 position_gnss / position_ned / attitude，如果不包含就添加（兼容舊訂閱） subs = getattr(self, subs_attr, {}) if isinstance(subs, dict) and 'position_gnss' not in subs and GnssRaw is not None: base_topic = f'/fc_network/vehicle/{sys_id}' try: position_gnss_sub = self.create_subscription( GnssRaw, f'{base_topic}/position_gnss', lambda msg, sid=sys_id: self.gps_callback(sid, msg), 10 ) subs['position_gnss'] = position_gnss_sub setattr(self, subs_attr, subs) except Exception: pass if isinstance(subs, dict) and 'position_ned' not in subs: base_topic = f'/fc_network/vehicle/{sys_id}' try: pos_ned_sub = self.create_subscription( Odometry, f'{base_topic}/position_ned', lambda msg, sid=sys_id: self.position_ned_callback(sid, msg), 10 ) subs['position_ned'] = pos_ned_sub setattr(self, subs_attr, subs) # 明確保存更新後的字典 except Exception as e: pass if isinstance(subs, dict) and 'attitude' not in subs and AttitudeRaw is not None: base_topic = f'/fc_network/vehicle/{sys_id}' try: attitude_sub = self.create_subscription( AttitudeRaw, f'{base_topic}/attitude', lambda msg, sid=sys_id: self.attitude_callback(sid, msg), 10 ) subs['attitude'] = attitude_sub setattr(self, subs_attr, subs) except Exception: pass if isinstance(subs, dict) and 'sys_diags' not in subs and SystemDiagnosticsRaw is not None: base_topic = f'/fc_network/vehicle/{sys_id}' try: sys_diags_sub = self.create_subscription( SystemDiagnosticsRaw, f'{base_topic}/sys_diags', lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg), 10 ) subs['sys_diags'] = sys_diags_sub setattr(self, subs_attr, subs) except Exception: pass if isinstance(subs, dict) and 'status_text' not in subs: base_topic = f'/fc_network/vehicle/{sys_id}' try: status_text_sub = self.create_subscription( String, f'{base_topic}/status_text', lambda msg, sid=sys_id: self.status_text_callback(sid, msg), 10 ) subs['status_text'] = status_text_sub setattr(self, subs_attr, subs) except Exception: pass 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 ), '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 ), 'position_ned': self.create_subscription( Odometry, f'{base_topic}/position_ned', lambda msg, sid=sys_id: self.position_ned_callback(sid, msg), 10 ) } if GnssRaw is not None: subs['position_gnss'] = self.create_subscription( GnssRaw, f'{base_topic}/position_gnss', lambda msg, sid=sys_id: self.gps_callback(sid, msg), 10 ) else: subs['position'] = self.create_subscription( NavSatFix, f'{base_topic}/position', lambda msg, sid=sys_id: self.gps_callback(sid, msg), 10 ) if AttitudeRaw is not None: subs['attitude'] = self.create_subscription( AttitudeRaw, f'{base_topic}/attitude', lambda msg, sid=sys_id: self.attitude_callback(sid, msg), 10 ) if SystemDiagnosticsRaw is not None: subs['sys_diags'] = self.create_subscription( SystemDiagnosticsRaw, f'{base_topic}/sys_diags', lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg), 10 ) subs['status_text'] = self.create_subscription( String, f'{base_topic}/status_text', lambda msg, sid=sys_id: self.status_text_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: _log("ERROR", 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: _log("ERROR", f"[SET_MODE] 未知模式: {mode_name}") return False _log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})") # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件） client = self.get_or_create_client(drone_id) if not client: _log("ERROR", "[SET_MODE] CommandLongClient 無法初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） result = await client.change_mode_async( target_sysid=sysid, custom_mode=float(custom_mode), target_compid=0, base_mode=1.0, timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性 ) if result and result.success: _log("INFO", f"[SET_MODE] {drone_id} 模式切換成功") return True else: _log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})") return False except Exception as e: _log("ERROR", 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: _log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) action_name = "解鎖" if arm else "上鎖" _log("INFO", f"[ARM] {drone_id} -> {action_name}") # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件） client = self.get_or_create_client(drone_id) if not client: _log("ERROR", "[ARM] CommandLongClient 無法初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） result = await client.arm_disarm_async( target_sysid=sysid, arm=arm, target_compid=0, timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性 ) if result and result.success: _log("INFO", f"[ARM] {drone_id} {action_name}成功") return True else: _log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})") return False except Exception as e: _log("ERROR", 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: _log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) _log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)") # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件） client = self.get_or_create_client(drone_id) if not client: _log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） result = await client.takeoff_async( target_sysid=sysid, altitude_m=float(altitude), target_compid=0, timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性 ) if result and result.success: _log("INFO", f"[TAKEOFF] {drone_id} 起飛成功") return True else: _log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})") return False except Exception as e: _log("ERROR", f"[TAKEOFF] 例外錯誤: {e}") traceback.print_exc() return False async def reboot_drone(self, drone_id): """使用 CommandLongClient 執行飛控重啟。""" try: parts = drone_id.split('_') if len(parts) < 2: _log("ERROR", f"[REBOOT] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) _log("INFO", f"[REBOOT] {drone_id} -> 飛控重啟") client = self.get_or_create_client(drone_id) if not client: _log("ERROR", "[REBOOT] CommandLongClient 無法初始化") return False result = await client.reboot_autopilot_async( target_sysid=sysid, target_compid=0, timeout_sec=5.0, ) if result and result.success: _log("INFO", f"[REBOOT] {drone_id} 重啟命令已送出") return True _log("ERROR", f"[REBOOT] 重啟失敗 (message={result.message if result else 'None'})") return False except Exception as e: _log("ERROR", f"[REBOOT] 例外錯誤: {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, sys_id, msg): """處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。""" actual_drone_id = self.sys_to_actual_id.get(sys_id, None) if actual_drone_id is None: return try: if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'): data = { 'roll': math.degrees(msg.roll), 'pitch': math.degrees(msg.pitch), 'yaw': math.degrees(msg.yaw), 'rates': ( getattr(msg, 'rollspeed', 0.0), getattr(msg, 'pitchspeed', 0.0), getattr(msg, 'yawspeed', 0.0), ) } elif hasattr(msg, 'orientation'): roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) data = { 'roll': roll, 'pitch': pitch, 'yaw': yaw, 'rates': ( msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, ) } else: return self.latest_data[(actual_drone_id, 'attitude')] = data except Exception as e: print(f"Error parsing attitude msg for {sys_id}: {e}") 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 sys_diags_callback(self, sys_id, msg): """轉送 /fc_network/vehicle/sysN/sys_diags 到 GUI 紀錄區。""" stamp = getattr(msg, 'stamp', None) data = { 'stamp': { 'sec': getattr(stamp, 'sec', 0), 'nanosec': getattr(stamp, 'nanosec', 0), }, 'sensors_install_mask': int(msg.sensors_install_mask), 'sensors_enabled_mask': int(msg.sensors_enabled_mask), 'sensors_health_mask': int(msg.sensors_health_mask), 'mcu_load': int(msg.mcu_load), 'mcu_load_percent': float(msg.mcu_load) / 10.0, 'bus_error_rate': int(msg.bus_error_rate), 'bus_error_rate_percent': float(msg.bus_error_rate) / 10.0, 'bus_error_count': int(msg.bus_error_count), 'errors_count1': int(msg.errors_count1), 'errors_count2': int(msg.errors_count2), 'errors_count3': int(msg.errors_count3), 'errors_count4': int(msg.errors_count4), } self.signals.update_signal.emit('sys_diags', sys_id, data) def status_text_callback(self, sys_id, msg): """轉送飛控 STATUSTEXT（飛行檢查、警告與錯誤）。""" raw = msg.data.strip() if not raw: return match = re.match(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$', raw) if match: vehicle_timestamp = float(match.group(1)) severity = int(match.group(2)) text = match.group(3) else: vehicle_timestamp = None severity = -1 text = raw self.signals.update_signal.emit('status_text', sys_id, { 'severity': severity, 'text': text, 'vehicle_timestamp': vehicle_timestamp, 'raw': raw, }) def fc_network_log_callback(self, msg): """接收 /fc_network/logs 的結構化紀錄。""" stamp = getattr(msg, 'stamp', None) self.signals.update_signal.emit('fc_network_log', msg.source or 'fc_network', { 'stamp': { 'sec': getattr(stamp, 'sec', 0), 'nanosec': getattr(stamp, 'nanosec', 0), }, 'level': int(msg.level), 'source': msg.source, 'event_code': msg.event_code, 'sysid': int(msg.sysid), 'message': msg.message, }) 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: fallback_socket_id = self.sys_to_socket_id.pop(sys_id, None) if fallback_socket_id is not None: self.release_socket_id(fallback_socket_id) # 使用原始 socket_id 獲取或分配統一的 socket_id assigned_socket_id = self.get_or_assign_socket_id(original_socket_id) self.sys_to_socket_id[sys_id] = assigned_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 _log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)") # 先發送連接類型資訊 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 gps_data = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude } if hasattr(msg, 'fix_type'): gps_data['fix_type'] = msg.fix_type if hasattr(msg, 'satellites_visible'): gps_data['satellites_visible'] = msg.satellites_visible if hasattr(msg, 'eph'): gps_data['eph'] = msg.eph if hasattr(msg, 'epv'): gps_data['epv'] = msg.epv self.latest_data[(actual_drone_id, 'gps')] = gps_data # ================================================================================ # 【新增】儲存 GPS 資料到 drone_gps 字典 # ================================================================================ self.drone_gps[actual_drone_id] = gps_data.copy() # ================================================================================ 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 position_ned_callback(self, sys_id, msg): """處理 position_ned topic (nav_msgs/msg/Odometry) NED 座標系中的位置（在 msg.pose.pose.position）： - x: 北向位移 (m) - y: 東向位移 (m) - z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1) """ try: # 使用映射獲取實際的 drone_id actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return # 從 Odometry 消息中提取位置數據 (msg.pose.pose.position) # Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist x = msg.pose.pose.position.y # NED 座標系中交換 x/y（與 local_pose 對齐） y = msg.pose.pose.position.x z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度（z 為負表示向下） vx = msg.twist.twist.linear.y vy = msg.twist.twist.linear.x vz = -msg.twist.twist.linear.z # 儲存高度信息 self.latest_data[(actual_drone_id, 'altitude')] = { 'altitude': z } # 儲存本地位置信息 self.latest_data[(actual_drone_id, 'local_pose')] = { 'x': x, 'y': y, 'z': z } # 儲存速度資訊，供總覽頁「XY速度」欄位顯示 self.latest_data[(actual_drone_id, 'velocity')] = { 'vx': vx, 'vy': vy, 'vz': vz } # 發送信號給 GUI 更新高度顯示 self.signals.update_signal.emit('altitude', actual_drone_id, { 'altitude': z }) except Exception as e: pass 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=115200): """啟動串口遙測連接（自動辨識 MAVLink / JSON）""" connection_name = f"Serial_{port.replace('/', '_')}" receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver.start() self.serial_receivers.append(receiver) _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)") return receiver