172 lines
6.2 KiB
Python
172 lines
6.2 KiB
Python
from __future__ import annotations
|
||
|
||
import math
|
||
import random
|
||
import time
|
||
from typing import Any, Dict, List
|
||
|
||
from uas.uas_control.base_adapter import BaseProtocolAdapter, ProtocolType
|
||
from uas.uas_control.command import Command, CommandResult, CommandType
|
||
from uas.uas_control.telemetry import (
|
||
BaseTelemetry, UAVTelemetry, RobotDogTelemetry, ConnectionState
|
||
)
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 仿真适配器（无需真实硬件，用于测试）
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class SimulationAdapter(BaseProtocolAdapter):
|
||
"""
|
||
仿真协议适配器
|
||
|
||
无需任何外部依赖，完全在内存中模拟设备行为。
|
||
支持所有 CommandType，适合单元测试和集成演示。
|
||
|
||
config 字段：
|
||
device_type : "controller" | "robot_dog"
|
||
fail_rate : 指令随机失败概率（0.0~1.0，默认 0.0）
|
||
latency_ms : 模拟通信延迟（ms，默认 20）
|
||
"""
|
||
|
||
_ALL_COMMANDS = list(CommandType)
|
||
|
||
def __init__(
|
||
self,
|
||
device_id: str,
|
||
device_type: str = "controller",
|
||
fail_rate: float = 0.0,
|
||
latency_ms: float = 20.0,
|
||
parent = None,
|
||
):
|
||
config = dict(
|
||
device_type = device_type,
|
||
fail_rate = fail_rate,
|
||
latency_ms = latency_ms,
|
||
)
|
||
super().__init__(device_id, ProtocolType.SIMULATION, config, parent)
|
||
self._state: Dict[str, Any] = {
|
||
"armed": False,
|
||
"mode": "STABILIZE",
|
||
"lat": 39.9,
|
||
"lon": 116.4,
|
||
"alt": 0.0,
|
||
"heading": 0.0,
|
||
"speed": 0.0,
|
||
"battery": 100,
|
||
"gait": "STAND",
|
||
}
|
||
|
||
def connect(self) -> bool:
|
||
self.logger.info(
|
||
f"[仿真] 设备连接: {self.device_id} "
|
||
f"type={self.config['device_type']}"
|
||
)
|
||
self._set_connected(True)
|
||
return True
|
||
|
||
def disconnect(self) -> bool:
|
||
self._set_connected(False)
|
||
return True
|
||
|
||
def send_command(self, cmd: Command) -> CommandResult:
|
||
# 模拟延迟
|
||
lat_s = self.config["latency_ms"] / 1000.0
|
||
time.sleep(lat_s + random.uniform(0, lat_s * 0.2))
|
||
|
||
# 模拟随机失败
|
||
if random.random() < self.config["fail_rate"]:
|
||
return CommandResult.fail(cmd, "[仿真] 随机故障注入")
|
||
|
||
# 更新内部状态
|
||
ct = cmd.command_type
|
||
p = cmd.params
|
||
|
||
if ct == CommandType.ARM:
|
||
self._state["armed"] = True
|
||
elif ct == CommandType.DISARM:
|
||
self._state["armed"] = False
|
||
elif ct == CommandType.EMERGENCY_STOP:
|
||
self._state["armed"] = False
|
||
self._state["speed"] = 0.0
|
||
elif ct == CommandType.TAKEOFF:
|
||
self._state["alt"] = p.get("altitude", 10.0)
|
||
self._state["speed"] = p.get("speed", 3.0)
|
||
self._state["mode"] = "GUIDED"
|
||
elif ct == CommandType.LAND:
|
||
self._state["alt"] = 0.0
|
||
self._state["speed"] = 0.0
|
||
self._state["mode"] = "LAND"
|
||
elif ct == CommandType.RTL:
|
||
self._state["mode"] = "RTL"
|
||
elif ct == CommandType.GOTO:
|
||
self._state["lat"] = p.get("lat", self._state["lat"])
|
||
self._state["lon"] = p.get("lon", self._state["lon"])
|
||
self._state["alt"] = p.get("alt", self._state["alt"])
|
||
self._state["speed"] = p.get("speed", 5.0)
|
||
elif ct == CommandType.SET_VELOCITY:
|
||
vx = p.get("vx", 0); vy = p.get("vy", 0)
|
||
self._state["speed"] = math.hypot(vx, vy)
|
||
self._state["heading"] = (
|
||
math.degrees(math.atan2(vy, vx)) + 360
|
||
) % 360
|
||
elif ct == CommandType.SET_MODE:
|
||
self._state["mode"] = p.get("mode", "STABILIZE")
|
||
elif ct in (CommandType.SIT, CommandType.STAND,
|
||
CommandType.WALK, CommandType.TROT):
|
||
self._state["gait"] = ct.value.upper()
|
||
self._state["speed"] = (
|
||
p.get("speed", 1.0)
|
||
if ct in (CommandType.WALK, CommandType.TROT) else 0.0
|
||
)
|
||
|
||
return CommandResult.ok(
|
||
cmd, f"[仿真] {ct.value} 执行成功",
|
||
data=dict(self._state)
|
||
)
|
||
|
||
def get_telemetry(self) -> BaseTelemetry:
|
||
t = time.time()
|
||
s = self._state
|
||
dt = self.config["device_type"]
|
||
|
||
# 模拟位置漂移
|
||
s["lat"] += math.sin(t * 0.01) * 0.00001
|
||
s["lon"] += math.cos(t * 0.01) * 0.00001
|
||
s["battery"] = max(0, int(100 - self.uptime_s * 0.05))
|
||
|
||
base = dict(
|
||
device_id = self.device_id,
|
||
timestamp = t,
|
||
connection_state = ConnectionState.CONNECTED.value,
|
||
latitude = s["lat"],
|
||
longitude = s["lon"],
|
||
altitude = s["alt"],
|
||
heading = s["heading"],
|
||
ground_speed = s["speed"],
|
||
battery_percent = s["battery"],
|
||
armed = s["armed"],
|
||
mode = s["mode"],
|
||
gps_fix_type = 3,
|
||
satellites_visible = 14,
|
||
signal_strength = 90,
|
||
)
|
||
|
||
if dt == "controller":
|
||
return UAVTelemetry(**{
|
||
k: v for k, v in base.items()
|
||
if k in UAVTelemetry.__dataclass_fields__
|
||
}, flight_mode=s["mode"])
|
||
else:
|
||
return RobotDogTelemetry(**{
|
||
k: v for k, v in base.items()
|
||
if k in RobotDogTelemetry.__dataclass_fields__
|
||
}, gait_mode=s["gait"])
|
||
|
||
def supported_commands(self) -> List[CommandType]:
|
||
return self._ALL_COMMANDS
|
||
|
||
def inject_fault(self, fail_rate: float) -> None:
|
||
"""动态注入故障率（测试用）"""
|
||
self.config["fail_rate"] = max(0.0, min(1.0, fail_rate))
|
||
self.logger.warning(f"故障注入: fail_rate={fail_rate:.0%}") |