334 lines
12 KiB
Python
334 lines
12 KiB
Python
# -*- 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())
|
||
|
||
|