# -*- coding: utf-8 -*- """ 协议适配器实现 — 完整版 MAVLinkAdapter / ROSAdapter / SimulationAdapter """ from __future__ import annotations import math import random import time from typing import Dict, List, Optional from pymavlink import mavutil from uas.uas_control.base_adapter import BaseProtocolAdapter, ProtocolType from uas.uas_control.command import Command, CommandResult, CommandType from uas.uas_control.telemetry import ( UAVTelemetry, ConnectionState ) # ───────────────────────────────────────────────────────────────── # MAVLink 适配器 # ───────────────────────────────────────────────────────────────── class MAVLinkAdapter(BaseProtocolAdapter): """ MAVLink v1/v2 协议适配器（ArduPilot / PX4） config 字段： connection_string : "udp:127.0.0.1:14550" | "tcp:host:port" | "/dev/ttyUSB0:57600" system_id : 地面站 sysid（默认 255） component_id : 地面站 compid（默认 1） source_system : 目标飞控 sysid（默认 1） dialect : "ardupilotmega" | "common"（默认 ardupilotmega） 实际部署需安装：pip install pymavlink 本实现在无 pymavlink 时自动降级为模拟模式。 """ # CommandType → MAVLink CMD ID _CMD_MAP: Dict[CommandType, int] = { CommandType.ARM: 400, # MAV_CMD_COMPONENT_ARM_DISARM CommandType.DISARM: 400, CommandType.TAKEOFF: 22, # MAV_CMD_NAV_TAKEOFF CommandType.LAND: 21, # MAV_CMD_NAV_LAND CommandType.RTL: 20, # MAV_CMD_NAV_RETURN_TO_LAUNCH CommandType.GOTO: 16, # MAV_CMD_NAV_WAYPOINT CommandType.SET_MODE: mavutil.mavlink.MAV_CMD_DO_SET_MODE, # MAV_CMD_DO_SET_MODE CommandType.LOITER: 17, # MAV_CMD_NAV_LOITER_UNLIM CommandType.EMERGENCY_STOP: 400, CommandType.CAMERA_SHOOT: 203, # MAV_CMD_DO_DIGICAM_CONTROL CommandType.GIMBAL_CONTROL: 205, # MAV_CMD_DO_MOUNT_CONTROL CommandType.PAYLOAD_ON: 2800, CommandType.PAYLOAD_OFF: 2800, CommandType.SET_VELOCITY: -1, # 使用 SET_POSITION_TARGET } _MODE_MAP: Dict[str, float] = { "MANUAL": 0.0, "GUIDE": 4.0, "OFFBOARD": 6.0, "LOITER": 0.0, "AUTO": 0.0 } def __init__( self, device_id: str, connection_string: str = "udp:127.0.0.1:14550", system_id: int = 255, component_id: int = 1, source_system: int = 1, dialect: str = "ardupilotmega", parent = None, ): config = dict( connection_string = connection_string, system_id = system_id, component_id = component_id, source_system = source_system, dialect = dialect, ) super().__init__(device_id, ProtocolType.MAVLINK, config, parent) self._mav = None # pymavlink MAVLink 实例 self._vehicle = None # 连接句柄 self._last_hb_time = 0.0 self._sim_mode = False # pymavlink 不可用时降级仿真 # 尝试导入 pymavlink try: from pymavlink import mavutil self._mavutil = mavutil self.logger.info("pymavlink 已加载") except ImportError: self._mavutil = None self._sim_mode = True self.logger.warning( "pymavlink 未安装，MAVLinkAdapter 以模拟模式运行" ) # ── 连接管理 ────────────────────────────────────────────────── def connect(self) -> bool: cs = self.config["connection_string"] if self._sim_mode or self._mavutil is None: self.logger.info(f"[模拟] MAVLink 连接: {cs}") self._set_connected(True) return True try: self._vehicle = self._mavutil.mavlink_connection( cs, source_system = self.config["system_id"], source_component = self.config["component_id"], dialect = self.config["dialect"], ) # 等待心跳（超时 5s） msg = self._vehicle.wait_heartbeat(timeout=5) if msg is None: raise self._set_connected(True) self.logger.info( f"MAVLink 心跳收到: sysid={self._vehicle.target_system} heartbeat={msg} " f"compid={self._vehicle.target_component}" ) return True except Exception as e: self.logger.error(f"MAVLink 连接失败: {e}") self.error_occurred.emit(str(e)) return False def disconnect(self) -> bool: if self._vehicle and not self._sim_mode: try: self._vehicle.close() except Exception: pass self._vehicle = None self._set_connected(False) return True def get_connection(self) -> object: return self._vehicle # ── 指令发送 ────────────────────────────────────────────────── def send_command(self, cmd: Command) -> CommandResult: unsupported = self._check_command_supported(cmd) if unsupported: return unsupported if self._sim_mode: return self._sim_send(cmd) try: return self._mav_send(cmd) except Exception as e: return CommandResult.fail(cmd, f"MAVLink 发送异常: {e}") def _mav_send(self, cmd: Command) -> CommandResult: """真实 MAVLink 发送""" ct = cmd.command_type p = cmd.params # SET_VELOCITY 使用 SET_POSITION_TARGET_LOCAL_NED if ct == CommandType.SET_VELOCITY: self._vehicle.mav.set_position_target_local_ned_send( 0, # time_boot_ms self._vehicle.target_system, self._vehicle.target_component, mavutil_frame := 1, # MAV_FRAME_LOCAL_NED 0b0000111111000111, # type_mask: 仅速度有效 0, 0, 0, # x y z（忽略） p.get("vx", 0), p.get("vy", 0), p.get("vz", 0), 0, 0, 0, # ax ay az（忽略） 0, 0, # yaw yaw_rate（忽略） ) return CommandResult.ok(cmd, "SET_VELOCITY 已发送") # 通用 command_long cmd_id = self._CMD_MAP.get(ct, -1) if cmd_id < 0: return CommandResult.fail(cmd, f"无 MAVLink 映射: {ct.value}") param1 = param2 = param3 = param4 = 0.0 param5 = param6 = param7 = 0.0 if ct == CommandType.ARM: param1 = 1.0 elif ct == CommandType.DISARM: param1 = 0.0 elif ct == CommandType.EMERGENCY_STOP: param1 = 0.0; param2 = 21196.0 # force disarm magic elif ct == CommandType.TAKEOFF: param7 = p.get("altitude", 10.0) elif ct == CommandType.GOTO: param5 = p.get("lat", 0.0) param6 = p.get("lon", 0.0) param7 = p.get("alt", 0.0) elif ct == CommandType.SET_MODE: param1 = 1.0 # MAV_MODE_FLAG_CUSTOM_MODE_ENABLED param2 = self._MODE_MAP.get(p['mode'], 0) self._vehicle.mav.command_long_send( self._vehicle.target_system, self._vehicle.target_component, cmd_id, 0, param1, param2, param3, param4, param5, param6, param7, ) # 等待 ACK（超时 cmd.timeout_s） ack = self._vehicle.recv_match( type="COMMAND_ACK", blocking=True, timeout=cmd.timeout_s ) if ack is None: return CommandResult.timeout(cmd) if ack.result == 0: # MAV_RESULT_ACCEPTED return CommandResult.ok(cmd, f"MAVLink ACK: ACCEPTED") return CommandResult.fail( cmd, f"MAVLink ACK: result={ack.result}" ) def _sim_send(self, cmd: Command) -> CommandResult: """模拟发送（无真实硬件时）""" time.sleep(random.uniform(0.01, 0.05)) self.logger.debug(f"[模拟] MAVLink 发送: {cmd.command_type.value}") return CommandResult.ok( cmd, f"[模拟] MAVLink {cmd.command_type.value} 已执行" ) # ── 遥测拉取 ────────────────────────────────────────────────── def get_telemetry(self) -> Optional[UAVTelemetry]: if self._sim_mode: return self._sim_telemetry() if not self._vehicle: return None try: return self._parse_mavlink_telemetry() except Exception as e: self.logger.error(f"遥测解析异常: {e}") return None def _parse_mavlink_telemetry(self) -> UAVTelemetry: """解析 MAVLink 消息为 UAVTelemetry""" t = UAVTelemetry( device_id = self.device_id, timestamp = time.time(), connection_state = ConnectionState.CONNECTED.value, ) # GLOBAL_POSITION_INT msg = self._vehicle.recv_match( type="GLOBAL_POSITION_INT", blocking=False ) if msg: t.latitude = msg.lat / 1e7 t.longitude = msg.lon / 1e7 t.altitude = msg.alt / 1000.0 t.relative_altitude = msg.relative_alt / 1000.0 t.vx = msg.vx / 100.0 t.vy = msg.vy / 100.0 t.vz = msg.vz / 100.0 t.heading = msg.hdg / 100.0 t.ground_speed = math.hypot(t.vx, t.vy) # ATTITUDE msg = self._vehicle.recv_match( type="ATTITUDE", blocking=False ) if msg: t.roll = math.degrees(msg.roll) t.pitch = math.degrees(msg.pitch) # SYS_STATUS msg = self._vehicle.recv_match( type="SYS_STATUS", blocking=False ) if msg: t.battery_voltage = msg.voltage_battery / 1000.0 t.battery_current = msg.current_battery / 100.0 t.battery_percent = msg.battery_remaining # GPS_RAW_INT msg = self._vehicle.recv_match( type="GPS_RAW_INT", blocking=False ) if msg: t.gps_fix_type = msg.fix_type t.satellites_visible = msg.satellites_visible t.hdop = msg.eph / 100.0 t.vdop = msg.epv / 100.0 # HEARTBEAT msg = self._vehicle.recv_match( type="HEARTBEAT", blocking=False ) if msg: t.armed = bool( msg.base_mode & 0x80 # MAV_MODE_FLAG_SAFETY_ARMED ) return t def _sim_telemetry(self) -> UAVTelemetry: """生成模拟遥测数据""" t = time.time() return UAVTelemetry( device_id = self.device_id, timestamp = t, connection_state = ConnectionState.CONNECTED.value, latitude = 39.9 + math.sin(t * 0.01) * 0.01, longitude = 116.4 + math.cos(t * 0.01) * 0.01, altitude = 100.0 + math.sin(t * 0.05) * 5, relative_altitude = 100.0, heading = (t * 2) % 360, ground_speed = 15.0 + random.uniform(-1, 1), battery_voltage = 22.4 - t * 0.0001, battery_percent = max(0, int(100 - t * 0.01)), gps_fix_type = 3, satellites_visible = 14, armed = True, flight_mode = "AUTO", signal_strength = 85, ) def supported_commands(self) -> List[CommandType]: return list(self._CMD_MAP.keys())