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())
|
|||
|
|
|
|||
|
|
|