实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
This commit is contained in:
keita
@@ -1,5 +1,5 @@
|
||||
{
|
||||
"_comment": "吊舱↔PX4 桥接配置。heading_servo.enabled=true → 走 L1 航向伺服(TrackingControlLaw)；tracking 段为 L1 控制律参数(与 visual_track_control 同结构，各分组须齐全)。全部数值为台架标定前占位，符号(beta_sign/alpha_sign)与增益须经 S1/S2 标定后回填，标定前不得实飞。",
|
||||
"_comment": "吊舱↔PX4 桥接配置。heading_servo.enabled=true → 走 L1 航向伺服(TrackingControlLaw)；tracking 段为 L1 控制律参数(与 visual_track_control 同结构，各分组须齐全)。S1 台架标定(2026-10-09)：目标在左下时 off_target_x/y 均为负 → 吊舱 X 轴右正(与控制律 β 同向)故 beta_sign=+1；Y 轴上正(与控制律 α 下正相反)故 alpha_sign=-1。厂商已确认本版电机无用 → beta_from_frame/alpha_from_frame 恒 false(框架角路径仅留作将来装电机的预留)。heading.gain 等增益仍为占位，须 S2 干跑验证方向与量级后方可实飞。",
|
||||
|
||||
"pod_serial": {
|
||||
"port": "",
|
||||
@@ -33,7 +33,7 @@
|
||||
"heading_servo": {
|
||||
"enabled": true,
|
||||
"beta_sign": 1,
|
||||
"alpha_sign": 1,
|
||||
"alpha_sign": -1,
|
||||
"beta_from_frame": false,
|
||||
"beta_from_off_target": true,
|
||||
"alpha_from_frame": false,
|
||||
@@ -83,5 +83,12 @@
|
||||
"ay_mps2": 1.0,
|
||||
"az_mps2": 0.5
|
||||
}
|
||||
},
|
||||
|
||||
"ground_inject": {
|
||||
"enabled": false,
|
||||
"host": "0.0.0.0",
|
||||
"port": 14555,
|
||||
"timeout_ms": 5000
|
||||
}
|
||||
}
|
||||
|
||||
@@ -31,6 +31,7 @@ from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
TrackState)
|
||||
|
||||
from .config import BridgeConfig
|
||||
from .ground_link import GroundTarget
|
||||
from .guidance import GuidanceInput, GuidanceLaw
|
||||
from .pod_serial import PodLink, PodSerial
|
||||
from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
|
||||
@@ -48,12 +49,15 @@ class PodBridge:
|
||||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||||
|
||||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None) -> None:
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None,
|
||||
ground_target_fn=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._px4 = px4
|
||||
self._pod = pod
|
||||
self._now_ms = now_ms_fn
|
||||
self._vision_target_fn = vision_target_fn # §2.3 第二判据（可选，S3+ 接入我方视觉）
|
||||
self._ground_target_fn = ground_target_fn # 地面注入坐标访问器（A2，无激光替代表4）
|
||||
self._use_heading_servo = cfg.heading_servo.enabled # 运行时控制律选择（A3 状态机切换）
|
||||
self._guidance = GuidanceLaw(cfg.guidance)
|
||||
self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0）
|
||||
self._attitude_period_ms = max(
|
||||
@@ -114,22 +118,36 @@ class PodBridge:
|
||||
|
||||
def _active_frequency_hz(self) -> float:
|
||||
"""当前生效控制律的循环频率：L1 用 tracking.control，L2 用 guidance。"""
|
||||
if self._cfg.heading_servo.enabled:
|
||||
if self._use_heading_servo:
|
||||
return self._cfg.tracking.control.frequency_hz
|
||||
return self._cfg.guidance.frequency_hz
|
||||
|
||||
def set_heading_servo_enabled(self, enabled: bool) -> None:
|
||||
"""运行时切换控制律（任务状态机 GOTO(L2) ↔ TRACK(L1) 切换，A3）。
|
||||
|
||||
切换时 reset 即将启用的律（清积分/限幅器历史），使指令从零渐变，
|
||||
避免切换瞬间输出跳变。L1 用吊舱角度直入，L2 用地理引导（注入坐标/表4）。
|
||||
"""
|
||||
if enabled == self._use_heading_servo:
|
||||
return
|
||||
self._use_heading_servo = enabled
|
||||
if enabled:
|
||||
self._tracking.reset()
|
||||
else:
|
||||
self._guidance.reset()
|
||||
|
||||
async def step(self, now_ms: int):
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
|
||||
|
||||
按 heading_servo.enabled 在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4 地理引导）间切换（方案 §7）。
|
||||
按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。
|
||||
"""
|
||||
self._ingest_pod_frames(now_ms)
|
||||
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
|
||||
if self._cfg.heading_servo.enabled:
|
||||
if self._use_heading_servo:
|
||||
out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms)
|
||||
else:
|
||||
out = self._guidance.update(
|
||||
@@ -175,6 +193,21 @@ class PodBridge:
|
||||
|
||||
# --- 飞控侧 -------------------------------------------------------------
|
||||
def _build_guidance_input(self, own, now_ms: int) -> GuidanceInput:
|
||||
injected = self._injected_target(now_ms)
|
||||
if injected is not None:
|
||||
# 地面注入坐标优先（本吊舱无激光 → 表4 永不出，目标坐标改走地面站）
|
||||
return GuidanceInput(
|
||||
own_lat_deg=own.latitude_deg,
|
||||
own_lon_deg=own.longitude_deg,
|
||||
own_yaw_deg=math.degrees(own.yaw_rad),
|
||||
own_position_valid=own.position_valid,
|
||||
own_position_time_ms=own.position_time_ms, # position 流专用时间戳
|
||||
target_lat_deg=injected.latitude_deg,
|
||||
target_lon_deg=injected.longitude_deg,
|
||||
target_valid=True, # 注入坐标已验证新鲜
|
||||
target_time_ms=injected.time_ms,
|
||||
)
|
||||
# 回退表4（有激光吊舱 / 未接入地面站）
|
||||
tgt = self._latest_target
|
||||
locked = self._pod_locked(now_ms)
|
||||
return GuidanceInput(
|
||||
@@ -189,6 +222,26 @@ class PodBridge:
|
||||
target_time_ms=tgt[1] if tgt else 0,
|
||||
)
|
||||
|
||||
def _injected_target(self, now_ms: int) -> Optional[GroundTarget]:
|
||||
"""地面注入坐标（新鲜有效则返回，否则 None → 回退表4 / 无目标）。
|
||||
|
||||
本吊舱无激光 → 表4 永不出，目标坐标改由地面站注入（预定流程「地面给坐标」）。
|
||||
未启用 / 未注入 / 超龄 → None（下游按无目标刹车）。"""
|
||||
if not self._cfg.ground_inject.enabled:
|
||||
return None
|
||||
if self._ground_target_fn is None:
|
||||
return None
|
||||
target = self._ground_target_fn()
|
||||
if target is None or target.time_ms <= 0:
|
||||
return None
|
||||
if is_stale(target.time_ms, now_ms, self._cfg.ground_inject.timeout_ms):
|
||||
return None
|
||||
return target
|
||||
|
||||
def pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控（公共接口，供编排层/状态机复用，A3）。"""
|
||||
return self._pod_locked(now_ms)
|
||||
|
||||
def _pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控：表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1
|
||||
bit2=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。
|
||||
|
||||
@@ -20,6 +20,7 @@ from visual_track_control.config import TrackingConfig
|
||||
from visual_track_control.config import load_from_dict as _load_tracking_dict
|
||||
|
||||
from .guidance import GuidanceConfig
|
||||
from .ground_link import GroundInjectConfig
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -77,6 +78,7 @@ class BridgeConfig:
|
||||
guidance: GuidanceConfig = field(default_factory=GuidanceConfig)
|
||||
heading_servo: HeadingServoConfig = field(default_factory=HeadingServoConfig)
|
||||
tracking: TrackingConfig = field(default_factory=TrackingConfig)
|
||||
ground_inject: GroundInjectConfig = field(default_factory=GroundInjectConfig)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
@@ -294,6 +296,23 @@ def _load_tracking(root: dict) -> TrackingConfig:
|
||||
return _load_tracking_dict(s)
|
||||
|
||||
|
||||
def _load_ground_inject(root: dict) -> GroundInjectConfig:
|
||||
"""地面坐标注入配置（预定流程「地面给坐标」，A2）。"""
|
||||
s = _require_section(root, "ground_inject")
|
||||
allowed = {f.name for f in dataclasses.fields(GroundInjectConfig)}
|
||||
_reject_unknown(s, allowed, "ground_inject")
|
||||
c = GroundInjectConfig()
|
||||
c.enabled = _get_bool(s, "ground_inject", "enabled", c.enabled)
|
||||
c.host = _get_string(s, "ground_inject", "host", c.host)
|
||||
c.port = int(_in_range(
|
||||
_get_integer(s, "ground_inject", "port", c.port),
|
||||
1, 65535, "ground_inject.port"))
|
||||
c.timeout_ms = int(_positive(
|
||||
_get_integer(s, "ground_inject", "timeout_ms", c.timeout_ms),
|
||||
"ground_inject.timeout_ms"))
|
||||
return c
|
||||
|
||||
|
||||
def load_from_json(path: str) -> BridgeConfig:
|
||||
"""从 JSON 文件一次性加载桥接配置（启动期调用，调参重启生效）。"""
|
||||
try:
|
||||
@@ -314,4 +333,5 @@ def load_from_json(path: str) -> BridgeConfig:
|
||||
cfg.guidance = _load_guidance(root)
|
||||
cfg.heading_servo = _load_heading_servo(root)
|
||||
cfg.tracking = _load_tracking(root)
|
||||
cfg.ground_inject = _load_ground_inject(root)
|
||||
return cfg
|
||||
|
||||
@@ -0,0 +1,135 @@
|
||||
"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。
|
||||
|
||||
本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
|
||||
本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标
|
||||
缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。
|
||||
|
||||
设计要点：
|
||||
* UDP 无连接监听，每包 JSON {"lat":..,"lon":..,"alt":..}（可选 "ts" 仅诊断，不用）。
|
||||
* 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
|
||||
（两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
|
||||
* parse_payload 为纯函数（可单测）；socket / 事件循环拆到 run()，且 socket
|
||||
延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import logging
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable, Optional
|
||||
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _is_number(value) -> bool:
|
||||
"""数值（排除 bool）。"""
|
||||
return isinstance(value, (int, float)) and not isinstance(value, bool)
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundTarget:
|
||||
"""地面注入的目标坐标（带本地接收时刻）。
|
||||
|
||||
字段与 protocol.TargetGeo 语义对齐（经纬度 °、绝对高度 m），但不含表4 的
|
||||
时间字段——那些由吊舱解算生成，地面注入不提供。
|
||||
"""
|
||||
|
||||
latitude_deg: float = 0.0
|
||||
longitude_deg: float = 0.0
|
||||
abs_alt_m: float = 0.0
|
||||
time_ms: int = 0 # 本地接收时刻（单调毫秒），0 = 从未注入
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundInjectConfig:
|
||||
"""地面坐标注入配置（全外置；数值占位，待落地时回填）。"""
|
||||
|
||||
enabled: bool = False # 是否启用地面注入（骨架期默认关，走表4 原路径）
|
||||
host: str = "0.0.0.0" # UDP 监听地址
|
||||
port: int = 14555 # UDP 监听端口（避开 MAVSDK SITL 14540）
|
||||
timeout_ms: int = 5000 # 注入坐标超龄门限（超龄判无效 → 下游刹车）
|
||||
|
||||
|
||||
class GroundLink:
|
||||
"""地面坐标注入接收器。
|
||||
|
||||
用法（在 runner 外部以 asyncio task 运行）：
|
||||
link = GroundLink(cfg, now_ms_fn)
|
||||
task = asyncio.create_task(link.run(stop_event))
|
||||
PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: GroundInjectConfig,
|
||||
now_ms_fn: Callable[[], int]) -> None:
|
||||
self._cfg = cfg
|
||||
self._now_ms = now_ms_fn
|
||||
self._target = GroundTarget()
|
||||
|
||||
def latest(self) -> GroundTarget:
|
||||
"""返回当前注入目标（可能 time_ms=0 表示从未注入）。"""
|
||||
return self._target
|
||||
|
||||
def inject(self, target: GroundTarget) -> None:
|
||||
"""直接注入目标并本地打戳（测试 / 地面站本地注入用，与 UDP run() 等价）。"""
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
|
||||
def is_valid(self, now_ms: int) -> bool:
|
||||
"""注入坐标是否新鲜有效（未注入 / 超龄 → False）。"""
|
||||
t = self._target
|
||||
if t.time_ms <= 0:
|
||||
return False
|
||||
return not is_stale(t.time_ms, now_ms, self._cfg.timeout_ms)
|
||||
|
||||
@staticmethod
|
||||
def parse_payload(payload: bytes) -> Optional[GroundTarget]:
|
||||
"""解析地面站 JSON 包（纯函数，可单测）。返回不含 time_ms 的目标；
|
||||
非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
|
||||
try:
|
||||
obj = json.loads(payload.decode("utf-8"))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError):
|
||||
return None
|
||||
if not isinstance(obj, dict):
|
||||
return None
|
||||
lat = obj.get("lat")
|
||||
lon = obj.get("lon")
|
||||
if not _is_number(lat) or not _is_number(lon):
|
||||
return None
|
||||
alt = obj.get("alt", 0.0)
|
||||
if not _is_number(alt):
|
||||
return None
|
||||
return GroundTarget(latitude_deg=float(lat), longitude_deg=float(lon),
|
||||
abs_alt_m=float(alt))
|
||||
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""UDP 接收循环：每收一包合法 JSON 更新注入目标并本地打戳。"""
|
||||
import socket # 延迟导入：仅真正跑 socket 时才需要
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
sock.bind((self._cfg.host, self._cfg.port))
|
||||
sock.setblocking(False)
|
||||
loop = asyncio.get_running_loop()
|
||||
logger.info("[GroundLink] 监听 UDP %s:%d", self._cfg.host, self._cfg.port)
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
data, _addr = await loop.sock_recvfrom(sock, 1024)
|
||||
except asyncio.CancelledError:
|
||||
raise
|
||||
except OSError as exc:
|
||||
logger.warning("[GroundLink] 接收异常: %s", exc)
|
||||
await asyncio.sleep(0.05)
|
||||
continue
|
||||
target = self.parse_payload(data)
|
||||
if target is None:
|
||||
continue # 非法包静默丢弃（热路径不打日志）
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
logger.debug("[GroundLink] 更新注入目标 (%f, %f, %fm)",
|
||||
target.latitude_deg, target.longitude_deg,
|
||||
target.abs_alt_m)
|
||||
finally:
|
||||
sock.close()
|
||||
@@ -1,7 +1,7 @@
|
||||
"""px4_gateway.py —— PX4(MAVSDK) 侧网关：读飞控遥测 + 发 offboard 速度指令。
|
||||
|
||||
职责（桥接的飞控侧）：
|
||||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_body），缓存为
|
||||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_ned），缓存为
|
||||
OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
|
||||
* 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
|
||||
（不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
|
||||
@@ -98,9 +98,11 @@ class Px4Gateway:
|
||||
self._state.position_time_ms = now # 仅此流刷新，姿态高频流不得代刷
|
||||
|
||||
def update_attitude(self, e: Any) -> None:
|
||||
self._state.roll_rad = float(e.roll_rad)
|
||||
self._state.pitch_rad = float(e.pitch_rad)
|
||||
self._state.yaw_rad = float(e.yaw_rad)
|
||||
"""MAVSDK attitude_euler() 返回 EulerAngle(roll_deg/pitch_deg/yaw_deg，度)，
|
||||
转弧度存入 OwnState（下游控制律统一用弧度 NED）。"""
|
||||
self._state.roll_rad = math.radians(float(e.roll_deg))
|
||||
self._state.pitch_rad = math.radians(float(e.pitch_deg))
|
||||
self._state.yaw_rad = math.radians(float(e.yaw_deg))
|
||||
self._state.has_attitude = True
|
||||
self._state.time_ms = self._now_ms()
|
||||
|
||||
@@ -110,9 +112,11 @@ class Px4Gateway:
|
||||
self._recompute_valid()
|
||||
self._state.time_ms = self._now_ms()
|
||||
|
||||
def update_velocity_body(self, v: Any) -> None:
|
||||
def update_velocity_ned(self, v: Any) -> None:
|
||||
"""MAVSDK velocity_ned() 返回 VelocityNed(north/east/down_m_s)，
|
||||
水平合速度即地速（表3 ground_speed 语义）。"""
|
||||
self._state.ground_speed_mps = math.hypot(
|
||||
float(v.forward_m_s), float(v.right_m_s))
|
||||
float(v.north_m_s), float(v.east_m_s))
|
||||
self._state.time_ms = self._now_ms()
|
||||
|
||||
def own_state(self) -> OwnState:
|
||||
@@ -125,7 +129,9 @@ class Px4Gateway:
|
||||
|
||||
# --- 遥测订阅（异步胶水）----------------------------------------------
|
||||
async def start_telemetry(self) -> None:
|
||||
"""订阅四路遥测流，后台任务持续刷新缓存。"""
|
||||
"""订阅四路遥测流，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
|
||||
if self._tasks:
|
||||
return # 已订阅，避免重复起任务（runner 会先订阅，bridge.start 再触发时跳过）
|
||||
t = self._drone.telemetry
|
||||
self._tasks = [
|
||||
asyncio.ensure_future(self._consume(t.position(), self.update_position)),
|
||||
@@ -133,7 +139,7 @@ class Px4Gateway:
|
||||
self._consume(t.attitude_euler(), self.update_attitude)),
|
||||
asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.velocity_body(), self.update_velocity_body)),
|
||||
self._consume(t.velocity_ned(), self.update_velocity_ned)),
|
||||
]
|
||||
logger.info("[Px4Gateway] 遥测订阅已启动")
|
||||
|
||||
|
||||
@@ -0,0 +1,86 @@
|
||||
"""verify_px4.py —— 仅飞控验证（不接吊舱）：连飞控 + 订阅遥测 + offboard 启停。
|
||||
|
||||
增量流程第④级「仅飞控」：确认飞控侧遥测与 offboard 正常，再进第⑤级地面联调。
|
||||
|
||||
用法：
|
||||
python utils/pod_bridge/verify_px4.py serial:///dev/serial0:57600
|
||||
python utils/pod_bridge/verify_px4.py serial:///dev/serial0:57600 --offboard --seconds 15
|
||||
|
||||
判读标准（每行遥测）：
|
||||
* 连接成功 → 打印「飞控已连接」
|
||||
* 位置流：has_position=True、position_valid=True（fix_type>=3）、lat/lon 非 0
|
||||
* 姿态流：has_attitude=True（yaw 有值）
|
||||
* GPS：fix_type>=3、num_satellites>0
|
||||
* 遥测持续刷新（time_ms / position_time_ms 每拍递增）→ 链路稳定
|
||||
* --offboard：未上锁(armed)时 offboard.start 可能返回 False 属正常，仅验证启停不报错
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path
|
||||
|
||||
from pod_bridge.px4_gateway import Px4Gateway # noqa: E402
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
async def run(conn: str, seconds: int, do_offboard: bool) -> int:
|
||||
px4 = await Px4Gateway.connect(conn, now_ms_fn=_monotonic_ms)
|
||||
await px4.start_telemetry()
|
||||
print(f"-- 遥测订阅已启动，连续打印 {seconds}s …")
|
||||
print(" (has_pos pos_valid fix sat | lat lon rel_alt | yaw | time_ms/pos_ms)")
|
||||
|
||||
for i in range(seconds):
|
||||
await asyncio.sleep(1.0)
|
||||
s = px4.own_state()
|
||||
print(f"[{i:2d}s] pos={int(s.has_position)} valid={int(s.position_valid)} "
|
||||
f"fix={s.fix_type} sat={s.num_satellites:2d} | "
|
||||
f"{s.latitude_deg:.6f} {s.longitude_deg:.6f} {s.relative_altitude_m:5.1f}m | "
|
||||
f"yaw={s.yaw_rad:+.2f} att={int(s.has_attitude)} | "
|
||||
f"t={s.time_ms} pos_t={s.position_time_ms}")
|
||||
|
||||
s = px4.own_state()
|
||||
print("\n-- 判读 --")
|
||||
ok = True
|
||||
if not s.has_position:
|
||||
print(" ✗ 位置流无数据（position 流未到达）"); ok = False
|
||||
if not s.position_valid:
|
||||
print(f" ✗ 位置无效（fix_type={s.fix_type} < 3，GPS 未 3D 定位）"); ok = False
|
||||
if not s.has_attitude:
|
||||
print(" ✗ 姿态流无数据（attitude_euler 未到达）"); ok = False
|
||||
if ok:
|
||||
print(" ✓ 遥测正常：位置/姿态/GPS 均有效，链路稳定")
|
||||
|
||||
if do_offboard:
|
||||
print("\n-- offboard 启停测试 --")
|
||||
started = await px4.start_offboard()
|
||||
print(f" start_offboard -> {started}")
|
||||
if started:
|
||||
await asyncio.sleep(2.0)
|
||||
await px4.send_brake_hover(_monotonic_ms())
|
||||
await px4.stop_offboard()
|
||||
print(" ✓ offboard 启停完成（已零速刹车）")
|
||||
else:
|
||||
print(" ⚠ offboard 启动失败（未 armed 属正常，armed 后重试）")
|
||||
|
||||
await px4.stop_telemetry()
|
||||
return 0 if ok else 1
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="仅飞控验证（不接吊舱）")
|
||||
ap.add_argument("conn", help="飞控连接串，如 serial:///dev/serial0:57600 或 udp://:14540")
|
||||
ap.add_argument("--seconds", type=int, default=10, help="遥测打印时长（秒）")
|
||||
ap.add_argument("--offboard", action="store_true", help="额外测试 offboard 启停")
|
||||
args = ap.parse_args()
|
||||
return asyncio.run(run(args.conn, args.seconds, args.offboard))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,143 @@
|
||||
"""virtual_pod.py —— 模拟吊舱（SITL/台架工具，非飞行组件；类比 sim_fc.py 模拟飞控）。
|
||||
|
||||
按"无电机吊舱"的实测语义合成 表2-2 遥测，实现 PodLink 接口可直插 PodBridge：
|
||||
* 相机刚性固连机体朝前 → 框架角(frame_roll/pitch/yaw)恒 0；
|
||||
* 无激光 → laser_range_m 恒 None；
|
||||
* off_target 按目标相对机体的视线角合成，符号与 S1 台架标定一致：
|
||||
目标偏右 → off_target_x_deg 为正（右正）
|
||||
目标偏下 → off_target_y_deg 为负（上正）
|
||||
* 目标不可见（身后 / 超视场 / 被 set_visible(False)）→ 状态字节1 bit2=0（搜索态）
|
||||
且脱靶量为 0（对齐真实吊舱"丢失时脱靶量恒 0、锁定位不复位"的坑）。
|
||||
|
||||
载机状态由注入的 own_state_fn 提供（CarrierView：NED 位置 + 航向），与 PodBridge
|
||||
解耦——SITL 脚本注入遥测 hub，单测注入固定值。
|
||||
"""
|
||||
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .protocol import Telemetry
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarrierView:
|
||||
"""虚拟吊舱所需的载机视角状态（NED 位置 + 航向）。"""
|
||||
|
||||
north_m: float = 0.0
|
||||
east_m: float = 0.0
|
||||
down_m: float = 0.0
|
||||
yaw_rad: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class VirtualTargetConfig:
|
||||
"""虚拟目标放置与运动参数（锚点机体系偏移 + 水平圆弧）。"""
|
||||
|
||||
ahead_m: float = 30.0
|
||||
right_m: float = 15.0
|
||||
down_m: float = 0.0
|
||||
arc_radius_m: float = 12.0
|
||||
arc_speed_mps: float = 1.0
|
||||
arc_ccw: bool = True
|
||||
|
||||
|
||||
class VirtualTarget:
|
||||
"""虚拟目标：按锚点机头朝向放置，随后在 NED 水平面做匀速圆弧运动。
|
||||
|
||||
几何与 test.py 的 VirtualTarget 一致（去掉了针孔投影部分——吊舱路径
|
||||
直接给角度，不需要检测框）。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: VirtualTargetConfig,
|
||||
anchor_ned: tuple[float, float, float],
|
||||
anchor_yaw_rad: float) -> None:
|
||||
cos_y = math.cos(anchor_yaw_rad)
|
||||
sin_y = math.sin(anchor_yaw_rad)
|
||||
|
||||
self._start_n = anchor_ned[0] + cfg.ahead_m * cos_y - cfg.right_m * sin_y
|
||||
self._start_e = anchor_ned[1] + cfg.ahead_m * sin_y + cfg.right_m * cos_y
|
||||
self._start_d = anchor_ned[2] + cfg.down_m
|
||||
|
||||
self._radius_m = max(0.1, float(cfg.arc_radius_m))
|
||||
self._omega = (float(cfg.arc_speed_mps) / self._radius_m
|
||||
* (1.0 if cfg.arc_ccw else -1.0))
|
||||
|
||||
right_n = -sin_y
|
||||
right_e = cos_y
|
||||
self._center_n = self._start_n + self._radius_m * right_n
|
||||
self._center_e = self._start_e + self._radius_m * right_e
|
||||
self._theta0 = math.atan2(self._start_e - self._center_e,
|
||||
self._start_n - self._center_n)
|
||||
|
||||
self._pos_n = self._start_n
|
||||
self._pos_e = self._start_e
|
||||
self._pos_d = self._start_d
|
||||
self._t0 = time.monotonic()
|
||||
self._visible = True
|
||||
|
||||
@property
|
||||
def visible(self) -> bool:
|
||||
return self._visible
|
||||
|
||||
def set_visible(self, visible: bool) -> None:
|
||||
self._visible = visible
|
||||
|
||||
def update_position(self) -> None:
|
||||
elapsed = max(0.0, time.monotonic() - self._t0)
|
||||
theta = self._theta0 + self._omega * elapsed
|
||||
self._pos_n = self._center_n + self._radius_m * math.cos(theta)
|
||||
self._pos_e = self._center_e + self._radius_m * math.sin(theta)
|
||||
self._pos_d = self._start_d
|
||||
|
||||
@property
|
||||
def pos_ned(self) -> tuple[float, float, float]:
|
||||
return self._pos_n, self._pos_e, self._pos_d
|
||||
|
||||
|
||||
class VirtualPod:
|
||||
"""模拟吊舱：poll() 按当前载机视角合成一帧 表2-2 遥测（锁定/搜索态）。"""
|
||||
|
||||
def __init__(self, target: VirtualTarget, own_state_fn,
|
||||
fov_h_deg: float = 60.0, fov_v_deg: float = 35.0) -> None:
|
||||
self._target = target
|
||||
self._own = own_state_fn
|
||||
self._fov_h = fov_h_deg
|
||||
self._fov_v = fov_v_deg
|
||||
self.attitude_frames_rx = 0 # 收到的表3 帧数（诊断）
|
||||
self.closed = False
|
||||
|
||||
def poll(self) -> list:
|
||||
self._target.update_position()
|
||||
own = self._own()
|
||||
n, e, d = self._target.pos_ned
|
||||
rel_n = n - own.north_m
|
||||
rel_e = e - own.east_m
|
||||
rel_d = d - own.down_m
|
||||
cos_y = math.cos(own.yaw_rad)
|
||||
sin_y = math.sin(own.yaw_rad)
|
||||
fwd = rel_n * cos_y + rel_e * sin_y
|
||||
right = -rel_n * sin_y + rel_e * cos_y
|
||||
|
||||
off_x = math.degrees(math.atan2(right, fwd)) # 右正
|
||||
off_y = math.degrees(math.atan2(-rel_d, fwd)) # 上正（S1 标定）
|
||||
locked = (self._target.visible
|
||||
and fwd > 0.5
|
||||
and abs(off_x) < self._fov_h / 2.0
|
||||
and abs(off_y) < self._fov_v / 2.0)
|
||||
|
||||
return [Telemetry(
|
||||
status1=0b00000100 if locked else 0, # bit2 锁定 / 搜索
|
||||
off_target_x_deg=off_x if locked else 0.0,
|
||||
off_target_y_deg=off_y if locked else 0.0,
|
||||
frame_roll_deg=0.0, # 无电机：框架角恒 0
|
||||
frame_pitch_deg=0.0,
|
||||
frame_yaw_deg=0.0,
|
||||
laser_range_m=None, # 无激光：恒无效
|
||||
)]
|
||||
|
||||
def send(self, data: bytes) -> None:
|
||||
self.attitude_frames_rx += 1 # 表3 载机姿态下发：计数即可，不解析
|
||||
|
||||
def close(self) -> None:
|
||||
self.closed = True
|
||||
@@ -0,0 +1,9 @@
|
||||
"""task_runner —— 任务编排器（预定流程.md 的天空侧任务级状态机）。
|
||||
|
||||
把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段
|
||||
编排成一条可运行的全流程：WAIT → ARM_TAKEOFF → GOTO → TRACK → APPROACH → RTL。
|
||||
|
||||
模块划分：
|
||||
* state_machine.py 纯逻辑任务状态机（无 IO，可单测）
|
||||
* runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环，A3）
|
||||
"""
|
||||
@@ -0,0 +1,149 @@
|
||||
"""log.py —— 任务日志模块：结构化记录任务全流程（事件 + 周期遥测）。
|
||||
|
||||
预定流程 B 阶段：让任意一次 SITL / 实飞都能回溯出完整阶段轨迹与异常。
|
||||
|
||||
分层：
|
||||
* 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
|
||||
低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
|
||||
* 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
|
||||
|
||||
时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
|
||||
文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
|
||||
"""
|
||||
|
||||
import csv
|
||||
import json
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
_TELEMETRY_FIELDS = (
|
||||
"t_s", "state",
|
||||
"pos_valid", "fix_type", "num_satellites",
|
||||
"latitude_deg", "longitude_deg", "relative_altitude_m", "yaw_deg",
|
||||
"target_injected", "distance_to_target_m", "target_locked",
|
||||
"off_target_x_deg", "off_target_y_deg",
|
||||
"mode", "yaw_rate_dps", "vx_body_mps", "vz_mps",
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class TelemetryRecord:
|
||||
"""单周期遥测记录（字段顺序即 CSV 列序）。"""
|
||||
|
||||
t_s: float = 0.0
|
||||
state: str = ""
|
||||
pos_valid: bool = False
|
||||
fix_type: int = 0
|
||||
num_satellites: int = 0
|
||||
latitude_deg: float = 0.0
|
||||
longitude_deg: float = 0.0
|
||||
relative_altitude_m: float = 0.0
|
||||
yaw_deg: float = 0.0
|
||||
target_injected: bool = False
|
||||
distance_to_target_m: float = float("inf")
|
||||
target_locked: bool = False
|
||||
off_target_x_deg: float = 0.0
|
||||
off_target_y_deg: float = 0.0
|
||||
mode: str = ""
|
||||
yaw_rate_dps: float = 0.0
|
||||
vx_body_mps: float = 0.0
|
||||
vz_mps: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskLogConfig:
|
||||
"""任务日志配置（全外置）。"""
|
||||
|
||||
enabled: bool = True
|
||||
dir: str = "logs" # 输出目录（相对工作目录）
|
||||
event_file: str = "task_events.jsonl" # 事件日志
|
||||
telemetry_file: str = "task_telemetry.csv" # 周期遥测日志
|
||||
telemetry_every_n: int = 1 # 每 N 周期记一次遥测（1 = 每周期）
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class TaskLogger:
|
||||
"""结构化任务日志。用法：
|
||||
|
||||
logger = TaskLogger(cfg, now_ms_fn)
|
||||
logger.open() # 建目录 + 写 CSV 表头
|
||||
logger.log_event(now_ms, "state", from_state="wait", ...)
|
||||
logger.log_telemetry(now_ms, record)
|
||||
logger.close()
|
||||
|
||||
enabled=False 或未 open 时，所有 log 方法为空操作（不抛异常、不写文件）。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: TaskLogConfig = TaskLogConfig(),
|
||||
now_ms_fn=_monotonic_ms) -> None:
|
||||
self._cfg = cfg
|
||||
self._now_ms = now_ms_fn
|
||||
self._event_fp = None
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
self._t0_ms: Optional[int] = None
|
||||
self._tick = 0
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
def open(self) -> None:
|
||||
"""创建输出目录并打开两个日志文件；disabled 时跳过。"""
|
||||
if not self._cfg.enabled:
|
||||
return
|
||||
out_dir = Path(self._cfg.dir)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
self._event_fp = (out_dir / self._cfg.event_file).open(
|
||||
"a", encoding="utf-8")
|
||||
self._tel_fp = (out_dir / self._cfg.telemetry_file).open(
|
||||
"a", encoding="utf-8", newline="")
|
||||
self._tel_writer = csv.writer(self._tel_fp)
|
||||
if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
|
||||
self._tel_writer.writerow(_TELEMETRY_FIELDS)
|
||||
self._event_fp.flush()
|
||||
self._tel_fp.flush()
|
||||
|
||||
def close(self) -> None:
|
||||
"""flush 并关闭两个日志文件。"""
|
||||
if self._event_fp is not None:
|
||||
self._event_fp.flush()
|
||||
self._event_fp.close()
|
||||
self._event_fp = None
|
||||
if self._tel_fp is not None:
|
||||
self._tel_fp.flush()
|
||||
self._tel_fp.close()
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
|
||||
# --- 记录 ---------------------------------------------------------------
|
||||
def log_event(self, now_ms: int, event_type: str, **fields) -> None:
|
||||
"""追加一条 JSONL 事件。"""
|
||||
if self._event_fp is None:
|
||||
return
|
||||
record = {"type": event_type, "ts_ms": now_ms,
|
||||
"t_s": self._rel_s(now_ms)}
|
||||
record.update(fields)
|
||||
self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
|
||||
|
||||
def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
|
||||
"""追加一行遥测（按 telemetry_every_n 节流）。"""
|
||||
if self._tel_writer is None:
|
||||
return
|
||||
self._tick += 1
|
||||
if self._tick % max(1, self._cfg.telemetry_every_n) != 0:
|
||||
return
|
||||
rec.t_s = self._rel_s(now_ms)
|
||||
self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])
|
||||
|
||||
# --- 内部 ---------------------------------------------------------------
|
||||
def _rel_s(self, now_ms: int) -> float:
|
||||
if self._t0_ms is None:
|
||||
self._t0_ms = now_ms
|
||||
return (now_ms - self._t0_ms) / 1000.0
|
||||
|
||||
@property
|
||||
def fieldnames(self) -> tuple:
|
||||
return _TELEMETRY_FIELDS
|
||||
@@ -0,0 +1,39 @@
|
||||
"""pod_control.py —— 吊舱目标识别/锁定控制（真机，任务编排的一部分）。
|
||||
|
||||
真机吊舱不会自动输出"锁定"——需要先发控制指令让它进入目标识别 + 自动锁定模式：
|
||||
* 0x07 目标识别开（param_x = 目标类型：0=人/车，1=无人机）
|
||||
* 0x0F 发现目标自动锁定（常驻模式：目标进画面自动锁定，非一次性动作）
|
||||
结束（任务完成/返航）时：
|
||||
* 0x0E 停止跟踪（只停当前这一次跟踪）
|
||||
* 0x08 目标识别关（真正退出识别）
|
||||
|
||||
本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。
|
||||
SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode, ControlCommand
|
||||
|
||||
|
||||
class PodController:
|
||||
"""吊舱目标识别/锁定控制器。
|
||||
|
||||
arm_detection / stop_tracking 为同步方法，经 bridge.send_control 下发 表2-1
|
||||
控制帧（编码由 PodBridge 完成，本类只关心"发什么指令序列"）。
|
||||
"""
|
||||
|
||||
def __init__(self, bridge, target_type: int = 0) -> None:
|
||||
"""bridge：PodBridge（需有 send_control）；target_type：0=人/车，1=无人机。"""
|
||||
self._bridge = bridge
|
||||
self._target_type = int(target_type)
|
||||
|
||||
def arm_detection(self) -> None:
|
||||
"""开启目标识别 + 发现自动锁定（GOTO 阶段进入时调用）。"""
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.TARGET_DETECT_ON, param_x=self._target_type))
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.AUTO_LOCK_ON_FOUND))
|
||||
|
||||
def stop_tracking(self) -> None:
|
||||
"""停止跟踪 + 关闭目标识别（RTL 前调用）。"""
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF))
|
||||
@@ -0,0 +1,296 @@
|
||||
"""runner.py —— 任务编排器：驱动任务状态机跑通 WAIT→…→RTL 全流程。
|
||||
|
||||
「预定流程.md」天空侧的落地：把已通的 L1（航向伺服 TrackingControlLaw）与
|
||||
L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入（GroundLink）与
|
||||
状态机（TaskStateMachine），串成一条可运行的全流程。
|
||||
|
||||
职责边界：
|
||||
* 状态机（TaskStateMachine）只做「状态 + 转移判据」决策；
|
||||
* 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」；
|
||||
* 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）；
|
||||
* 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成，
|
||||
便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。
|
||||
|
||||
生命周期：run() 开头先订阅 PX4 遥测（幂等），用于起飞到位 / 距离计算；
|
||||
GOTO 进入时 start offboard；RTL 进入时安全停机 + 返航。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from typing import Callable, Optional
|
||||
|
||||
from pod_bridge.guidance import haversine_distance_m
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
|
||||
from .log import TaskLogger, TelemetryRecord
|
||||
from .state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
||||
TaskStateMachine)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class TaskRunner:
|
||||
"""任务编排器。依赖（除 sm 外均可注入 mock）：
|
||||
|
||||
cfg BridgeConfig —— ground_inject.timeout_ms 等
|
||||
sm_cfg TaskStateConfig —— 切换半径/超时/起飞高度/居中阈值/循环频率
|
||||
px4 Px4Gateway —— own_state()（姿态/位置/相对高度）
|
||||
bridge PodBridge —— step / set_heading_servo_enabled / pod_locked
|
||||
sm TaskStateMachine —— update()
|
||||
ground GroundLink —— latest()（地面注入坐标）
|
||||
drone MAVSDK System —— action.arm/takeoff/return_to_launch/land/disarm
|
||||
vision_fn callable->bool —— 我方视觉第二判据（可选，require_vision 时用）
|
||||
"""
|
||||
|
||||
def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine,
|
||||
ground, drone, vision_fn: Optional[Callable[[], bool]] = None,
|
||||
now_ms_fn: Callable[[], int] = _monotonic_ms,
|
||||
logger: Optional[TaskLogger] = None,
|
||||
pod_control=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._sm_cfg = sm_cfg
|
||||
self._px4 = px4
|
||||
self._bridge = bridge
|
||||
self._sm = sm
|
||||
self._ground = ground
|
||||
self._drone = drone
|
||||
self._vision_fn = vision_fn
|
||||
self._now = now_ms_fn
|
||||
self._logger = logger # 结构化任务日志（可选，B 阶段）
|
||||
self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机）
|
||||
self._centered_since_ms: Optional[int] = None
|
||||
self._rtl_requested = False
|
||||
self._emergency = False
|
||||
self._rtl_done = False
|
||||
|
||||
# --- 外部控制 -----------------------------------------------------------
|
||||
def request_rtl(self) -> None:
|
||||
"""外部请求返航（如收到地面站返航命令 / 人工按键）。"""
|
||||
self._rtl_requested = True
|
||||
|
||||
def trigger_emergency(self) -> None:
|
||||
"""外部触发紧急终止（丢控 / RC 接管 / 失控）。"""
|
||||
self._emergency = True
|
||||
|
||||
@property
|
||||
def state(self) -> TaskState:
|
||||
return self._sm.state
|
||||
|
||||
@property
|
||||
def rtl_done(self) -> bool:
|
||||
return self._rtl_done
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""主循环：采集快照 → 状态机 → 进入动作/持续动作，直到 RTL 完成或停止。"""
|
||||
await self._px4.start_telemetry() # 幂等：先拿遥测，供起飞到位/距离计算
|
||||
if self._logger is not None:
|
||||
self._logger.open()
|
||||
period_s = 1.0 / self._sm_cfg.loop_frequency_hz
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
now_ms = self._now()
|
||||
inputs = self._collect_inputs(now_ms)
|
||||
tr = self._sm.update(inputs, now_ms)
|
||||
if tr.occurred:
|
||||
logger.info("[Task] %s -> %s (%s)",
|
||||
tr.from_state.value, tr.to_state.value, tr.reason)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
now_ms, "state", from_state=tr.from_state.value,
|
||||
to_state=tr.to_state.value, reason=tr.reason)
|
||||
await self._on_enter(tr.to_state, now_ms)
|
||||
out = await self._tick(now_ms)
|
||||
if self._logger is not None:
|
||||
self._logger.log_telemetry(
|
||||
now_ms, self._build_telemetry_record(now_ms, inputs, out))
|
||||
if self._finished():
|
||||
break
|
||||
await asyncio.sleep(period_s)
|
||||
finally:
|
||||
await self._teardown()
|
||||
if self._logger is not None:
|
||||
self._logger.close()
|
||||
|
||||
# --- 输入采集 -----------------------------------------------------------
|
||||
def _collect_inputs(self, now_ms: int) -> TaskInputs:
|
||||
"""聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。"""
|
||||
own = self._px4.own_state()
|
||||
|
||||
injected = self._ground.latest()
|
||||
target_injected = bool(
|
||||
injected.time_ms > 0
|
||||
and not is_stale(injected.time_ms, now_ms,
|
||||
self._cfg.ground_inject.timeout_ms))
|
||||
|
||||
distance = math.inf
|
||||
if target_injected and own.position_valid:
|
||||
distance = haversine_distance_m(
|
||||
own.latitude_deg, own.longitude_deg,
|
||||
injected.latitude_deg, injected.longitude_deg)
|
||||
|
||||
takeoff_reached = (own.relative_altitude_m
|
||||
>= self._sm_cfg.takeoff_altitude_m)
|
||||
target_locked = self._bridge.pod_locked(now_ms)
|
||||
vision = bool(self._vision_fn and self._vision_fn())
|
||||
centered, centered_s = self._update_centered(now_ms)
|
||||
|
||||
return TaskInputs(
|
||||
target_injected=target_injected,
|
||||
distance_to_target_m=distance,
|
||||
takeoff_reached=takeoff_reached,
|
||||
target_locked=target_locked,
|
||||
vision_has_target=vision,
|
||||
target_centered=centered,
|
||||
centered_duration_s=centered_s,
|
||||
rtl_requested=self._rtl_requested,
|
||||
emergency=self._emergency,
|
||||
)
|
||||
|
||||
def _update_centered(self, now_ms: int) -> tuple[bool, float]:
|
||||
"""脱靶量居中判定 + 持续时长累加（TRACK→APPROACH 判据）。
|
||||
|
||||
无遥测 / 任一轴脱靶量超出 center_threshold_deg → 清零累加。
|
||||
"""
|
||||
tel = self._bridge.latest_telemetry
|
||||
if tel is None:
|
||||
self._centered_since_ms = None
|
||||
return False, 0.0
|
||||
frame, _ = tel
|
||||
centered = (abs(frame.off_target_x_deg) < self._sm_cfg.center_threshold_deg
|
||||
and abs(frame.off_target_y_deg) < self._sm_cfg.center_threshold_deg)
|
||||
if not centered:
|
||||
self._centered_since_ms = None
|
||||
return False, 0.0
|
||||
if self._centered_since_ms is None:
|
||||
self._centered_since_ms = now_ms
|
||||
return True, (now_ms - self._centered_since_ms) / 1000.0
|
||||
|
||||
# --- 状态进入动作 -------------------------------------------------------
|
||||
async def _on_enter(self, state: TaskState, now_ms: int) -> None:
|
||||
if state is TaskState.ARM_TAKEOFF:
|
||||
await self._arm_takeoff()
|
||||
elif state is TaskState.GOTO:
|
||||
if not await self._bridge.start():
|
||||
logger.error("[Task] offboard 启动失败，紧急终止")
|
||||
self._emergency = True
|
||||
return
|
||||
self._bridge.set_heading_servo_enabled(False) # L2 地理引导
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.arm_detection() # 真机：开启目标识别 + 自动锁定
|
||||
elif state is TaskState.TRACK:
|
||||
self._bridge.set_heading_servo_enabled(True) # L1 航向伺服
|
||||
elif state is TaskState.APPROACH:
|
||||
pass # 悬停由 _tick 持续发零速
|
||||
elif state is TaskState.RTL:
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关（bridge 停机前）
|
||||
await self._start_rtl()
|
||||
|
||||
# --- 每周期持续动作 -----------------------------------------------------
|
||||
async def _tick(self, now_ms: int):
|
||||
"""每周期持续动作。返回本拍控制输出（GOTO/TRACK 有，其余 None）。"""
|
||||
state = self._sm.state
|
||||
if state in (TaskState.GOTO, TaskState.TRACK):
|
||||
return await self._bridge.step(now_ms)
|
||||
elif state is TaskState.APPROACH:
|
||||
await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳）
|
||||
elif state is TaskState.RTL:
|
||||
await self._check_landed()
|
||||
return None
|
||||
# WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度）
|
||||
|
||||
def _build_telemetry_record(self, now_ms: int, inputs: TaskInputs,
|
||||
out) -> TelemetryRecord:
|
||||
"""聚合本拍遥测为一帧 TelemetryRecord（供日志 / 页面复用）。"""
|
||||
own = self._px4.own_state()
|
||||
tel = self._bridge.latest_telemetry
|
||||
off_x = off_y = 0.0
|
||||
if tel is not None:
|
||||
frame, _ = tel
|
||||
off_x = frame.off_target_x_deg
|
||||
off_y = frame.off_target_y_deg
|
||||
return TelemetryRecord(
|
||||
state=self._sm.state.value,
|
||||
pos_valid=own.position_valid,
|
||||
fix_type=own.fix_type,
|
||||
num_satellites=own.num_satellites,
|
||||
latitude_deg=own.latitude_deg,
|
||||
longitude_deg=own.longitude_deg,
|
||||
relative_altitude_m=own.relative_altitude_m,
|
||||
yaw_deg=math.degrees(own.yaw_rad),
|
||||
target_injected=inputs.target_injected,
|
||||
distance_to_target_m=inputs.distance_to_target_m,
|
||||
target_locked=inputs.target_locked,
|
||||
off_target_x_deg=off_x,
|
||||
off_target_y_deg=off_y,
|
||||
mode=out.mode.name if out is not None else "",
|
||||
yaw_rate_dps=out.yaw_rate_dps if out is not None else 0.0,
|
||||
vx_body_mps=out.vx_body_mps if out is not None else 0.0,
|
||||
vz_mps=out.vz_mps if out is not None else 0.0,
|
||||
)
|
||||
|
||||
# --- 飞行编排 -----------------------------------------------------------
|
||||
async def _arm_takeoff(self) -> None:
|
||||
try:
|
||||
await self._drone.action.arm()
|
||||
await self._drone.action.set_takeoff_altitude(
|
||||
self._sm_cfg.takeoff_altitude_m)
|
||||
await self._drone.action.takeoff()
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
self._now(), "takeoff",
|
||||
altitude_m=self._sm_cfg.takeoff_altitude_m)
|
||||
except Exception as exc:
|
||||
logger.error("[Task] 起飞失败: %s", exc)
|
||||
self._emergency = True
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "takeoff_failed",
|
||||
error=str(exc))
|
||||
|
||||
async def _start_rtl(self) -> None:
|
||||
try:
|
||||
await self._bridge.stop() # 先零速刹车 + 停 offboard/遥测/串口
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 停机异常: %s", exc)
|
||||
try:
|
||||
await self._drone.action.return_to_launch()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] RTL 失败，尝试降落: %s", exc)
|
||||
try:
|
||||
await self._drone.action.land()
|
||||
except Exception:
|
||||
pass
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "rtl")
|
||||
|
||||
async def _check_landed(self) -> None:
|
||||
"""RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。"""
|
||||
try:
|
||||
async for is_in_air in self._drone.telemetry.in_air():
|
||||
if not is_in_air and not self._rtl_done:
|
||||
self._rtl_done = True
|
||||
try:
|
||||
await self._drone.action.disarm()
|
||||
except Exception:
|
||||
pass
|
||||
break # 只取最新一帧
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# --- 收尾 ---------------------------------------------------------------
|
||||
def _finished(self) -> bool:
|
||||
return self._sm.state is TaskState.RTL and self._rtl_done
|
||||
|
||||
async def _teardown(self) -> None:
|
||||
"""退出主循环（含异常/中途停止）时的安全收尾。"""
|
||||
try:
|
||||
await self._bridge.stop()
|
||||
except Exception:
|
||||
pass
|
||||
@@ -0,0 +1,178 @@
|
||||
"""state_machine.py —— 任务级状态机（纯逻辑，无 IO，可单测）。
|
||||
|
||||
「预定流程.md」天空侧的任务编排：把已通的 L1（航向伺服 TrackingControlLaw）与
|
||||
L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块只做「状态 + 转移判据
|
||||
+ 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份
|
||||
TaskInputs 快照，返回是否转移 + 原因。
|
||||
|
||||
状态链：
|
||||
WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 坐标引导）
|
||||
│ 距目标 < switch_radius
|
||||
│ 且锁定(+可选视觉)
|
||||
▼
|
||||
APPROACH ◀──居中稳定── TRACK（L1 航向伺服）
|
||||
│
|
||||
任意状态 ──超时 / 返航 / 紧急──▶ RTL
|
||||
|
||||
设计要点（铁律对齐）：
|
||||
* 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒），
|
||||
不碰时钟/IO/网络，可确定性单测。
|
||||
* 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航）
|
||||
→ 任何非 RTL 状态立即转 RTL（安全第一）。
|
||||
* GOTO→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉第二判据，
|
||||
避免 bit2 粘滞导致误入追踪。
|
||||
* TRACK 丢目标：若注入坐标仍有效 → 退回 GOTO 重新引导；否则 → RTL。
|
||||
* 阈值/超时全外置（TaskStateConfig），代码不写死数值。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class TaskState(Enum):
|
||||
"""任务状态（.value 为小写字符串，供日志/序列化）。"""
|
||||
|
||||
WAIT = "wait" # 待命：等地面坐标
|
||||
ARM_TAKEOFF = "arm_takeoff" # 解锁 + 起飞到悬停高度
|
||||
GOTO = "goto" # L2 坐标引导（飞向注入坐标）
|
||||
TRACK = "track" # L1 航向伺服（吊舱角度追踪）
|
||||
APPROACH = "approach" # 接近/悬停目标上空
|
||||
RTL = "rtl" # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskStateConfig:
|
||||
"""状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。"""
|
||||
|
||||
switch_radius_m: float = 30.0 # GOTO→TRACK 切换半径（米）
|
||||
require_vision: bool = False # GOTO→TRACK 是否需视觉第二判据（S3 起开）
|
||||
arm_takeoff_timeout_s: float = 60.0 # 起飞阶段超时
|
||||
goto_timeout_s: float = 120.0 # 坐标引导超时
|
||||
track_timeout_s: float = 120.0 # 追踪超时
|
||||
center_hold_s: float = 3.0 # TRACK→APPROACH 居中稳定时长（秒）
|
||||
approach_hover_s: float = 10.0 # APPROACH 悬停时长（到点 → RTL）
|
||||
takeoff_altitude_m: float = 5.0 # 起飞目标高度（runner 起飞 + takeoff_reached 判据）
|
||||
center_threshold_deg: float = 1.0 # 脱靶量居中阈值（|off_target| < 此值判居中）
|
||||
loop_frequency_hz: float = 20.0 # runner 主循环频率（bridge.step 节拍）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskInputs:
|
||||
"""单周期任务快照（调用方在同一控制周期采集并打戳）。
|
||||
|
||||
各布尔量为「已合成好的判据」，由 runner 从 PX4 遥测 / 吊舱遥测 / 地面注入 /
|
||||
视觉源聚合后传入；状态机不做聚合，只做转移决策。
|
||||
"""
|
||||
|
||||
target_injected: bool = False # 地面坐标有效注入（新鲜、未超龄）
|
||||
distance_to_target_m: float = math.inf # 到注入目标的水平距离（GOTO 用）
|
||||
takeoff_reached: bool = False # 已到达起飞目标高度
|
||||
target_locked: bool = False # 吊舱锁定（bit2 + 遥测新鲜）
|
||||
vision_has_target: bool = False # 我方视觉第二判据（可选，require_vision 时必看）
|
||||
target_centered: bool = False # 目标居中（脱靶量长期≈0）
|
||||
centered_duration_s: float = 0.0 # 居中持续时长（外部累加，秒）
|
||||
rtl_requested: bool = False # 人工返航命令
|
||||
emergency: bool = False # 紧急（丢控 / RC 接管 / 失控）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskTransition:
|
||||
"""状态机单周期转移结果。occurred=False 表示维持原状态。"""
|
||||
|
||||
from_state: TaskState = TaskState.WAIT
|
||||
to_state: TaskState = TaskState.WAIT
|
||||
occurred: bool = False
|
||||
reason: str = ""
|
||||
|
||||
|
||||
class TaskStateMachine:
|
||||
"""任务级状态机。update() 单周期返回 TaskTransition，维护内部 current 状态。"""
|
||||
|
||||
def __init__(self, cfg: TaskStateConfig) -> None:
|
||||
self._cfg = cfg
|
||||
self._state = TaskState.WAIT
|
||||
self._entered_ms = 0 # 进入当前状态的单调毫秒（超时/时长判定用）
|
||||
|
||||
@property
|
||||
def state(self) -> TaskState:
|
||||
return self._state
|
||||
|
||||
def reset(self) -> None:
|
||||
"""回到 WAIT 并清零计时（新任务 / 任务终止后复用）。"""
|
||||
self._state = TaskState.WAIT
|
||||
self._entered_ms = 0
|
||||
|
||||
def update(self, inputs: TaskInputs, now_ms: int) -> TaskTransition:
|
||||
"""吃一份快照，返回转移结果；无转移时 occurred=False、to_state 保持原状态。"""
|
||||
state = self._state
|
||||
|
||||
# 全局最高优先级：紧急 / 人工返航 → 任何非 RTL 状态立即终止。
|
||||
if state is not TaskState.RTL:
|
||||
if inputs.emergency:
|
||||
return self._goto(TaskState.RTL, "紧急（丢控/RC 接管）", now_ms)
|
||||
if inputs.rtl_requested:
|
||||
return self._goto(TaskState.RTL, "人工返航命令", now_ms)
|
||||
|
||||
if state is TaskState.WAIT:
|
||||
if inputs.target_injected:
|
||||
return self._goto(TaskState.ARM_TAKEOFF, "收到地面坐标", now_ms)
|
||||
|
||||
elif state is TaskState.ARM_TAKEOFF:
|
||||
if inputs.takeoff_reached:
|
||||
return self._goto(TaskState.GOTO, "已到起飞高度", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.arm_takeoff_timeout_s:
|
||||
return self._goto(TaskState.RTL, "起飞超时", now_ms)
|
||||
|
||||
elif state is TaskState.GOTO:
|
||||
if not inputs.target_injected:
|
||||
return self._goto(TaskState.RTL, "注入坐标失效", now_ms)
|
||||
if (inputs.distance_to_target_m <= self._cfg.switch_radius_m
|
||||
and self._lock_ok(inputs)):
|
||||
return self._goto(TaskState.TRACK, "进入追踪半径且目标锁定", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.goto_timeout_s:
|
||||
return self._goto(TaskState.RTL, "坐标引导超时", now_ms)
|
||||
|
||||
elif state is TaskState.TRACK:
|
||||
if not inputs.target_locked:
|
||||
# 丢目标：坐标仍有效 → 退回 GOTO 重新引导；否则终止返航。
|
||||
if inputs.target_injected:
|
||||
return self._goto(TaskState.GOTO, "目标丢失，退回坐标引导", now_ms)
|
||||
return self._goto(TaskState.RTL, "目标丢失且坐标失效", now_ms)
|
||||
if (inputs.target_centered
|
||||
and inputs.centered_duration_s >= self._cfg.center_hold_s):
|
||||
return self._goto(TaskState.APPROACH, "目标居中稳定", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.track_timeout_s:
|
||||
return self._goto(TaskState.RTL, "追踪超时", now_ms)
|
||||
|
||||
elif state is TaskState.APPROACH:
|
||||
if self._elapsed_s(now_ms) >= self._cfg.approach_hover_s:
|
||||
return self._goto(TaskState.RTL, "悬停完成", now_ms)
|
||||
|
||||
# RTL 为终态；无任何转移条件命中 → 维持原状态。
|
||||
return TaskTransition(from_state=state, to_state=state, occurred=False)
|
||||
|
||||
# --- 内部辅助 -----------------------------------------------------------
|
||||
def _lock_ok(self, inputs: TaskInputs) -> bool:
|
||||
"""GOTO→TRACK 的目标锁定判据（复用 L1 方案 §2.3）。
|
||||
|
||||
需吊舱锁定 bit2；若 require_vision 还需我方视觉第二判据（bit2 粘滞兜底）。
|
||||
"""
|
||||
if not inputs.target_locked:
|
||||
return False
|
||||
if self._cfg.require_vision and not inputs.vision_has_target:
|
||||
return False
|
||||
return True
|
||||
|
||||
def _elapsed_s(self, now_ms: int) -> float:
|
||||
"""进入当前状态的时长（秒）。_entered_ms=0（未打戳）时返回 0。"""
|
||||
if self._entered_ms <= 0:
|
||||
return 0.0
|
||||
return max(0.0, (now_ms - self._entered_ms) / 1000.0)
|
||||
|
||||
def _goto(self, target: TaskState, reason: str, now_ms: int) -> TaskTransition:
|
||||
prev = self._state
|
||||
self._state = target
|
||||
self._entered_ms = now_ms
|
||||
return TaskTransition(from_state=prev, to_state=target,
|
||||
occurred=True, reason=reason)
|
||||
@@ -13,6 +13,7 @@ import pytest
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import BridgeConfig, load_from_json
|
||||
from pod_bridge.ground_link import GroundTarget
|
||||
from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
|
||||
Telemetry, decode_attitude, decode_control)
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
@@ -523,3 +524,75 @@ def test_l1_require_vision_passes_with_source():
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
# --- 地面坐标注入（A2）-------------------------------------------------------
|
||||
def _inject_config(timeout_ms=5000):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = timeout_ms
|
||||
return cfg
|
||||
|
||||
|
||||
def test_ground_inject_priority_over_table4():
|
||||
"""注入坐标优先于表4：吊舱无表4/无锁定（本设备常态），注入新鲜坐标照常引导。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||||
b = PodBridge(_inject_config(), px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert len(px4.velocity_calls) == 1
|
||||
|
||||
|
||||
def test_ground_inject_stale_falls_back_to_brake():
|
||||
"""注入坐标超龄 → 视为无目标 → 刹车（不得朝过期坐标飞）。"""
|
||||
px4 = FakePx4(_valid_state(2000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000) # 1000ms 前注入
|
||||
cfg = _inject_config(timeout_ms=500)
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(2000)) # 差值 1000ms > 500ms → 超龄
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
assert px4.velocity_calls == []
|
||||
|
||||
|
||||
def test_ground_inject_disabled_ignores_injection():
|
||||
"""未启用地面注入 → 忽略注入坐标，回退表4 原路径（无表4 → 刹车）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||||
cfg = BridgeConfig() # ground_inject.enabled=False
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
|
||||
|
||||
# --- 运行时切换控制律（A3：GOTO(L2) ↔ TRACK(L1)）-----------------------------
|
||||
def test_set_heading_servo_enabled_runtime_switch():
|
||||
"""运行时切换控制律：初始 L1 → 切 L2（刹车）→ 切回 L1（速度）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
b = _bridge(px4, pod, _l1_config()) # enabled=True → 初始 L1
|
||||
|
||||
# 初始 L1：锁定 + 脱靶量 → 速度
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
# 切 L2：无表4/无锁定 → 刹车
|
||||
b.set_heading_servo_enabled(False)
|
||||
out = asyncio.run(b.step(1100))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
|
||||
# 切回 L1：重新喂锁定脱靶量 → 速度
|
||||
b.set_heading_servo_enabled(True)
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = asyncio.run(b.step(1200))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
@@ -133,6 +133,17 @@ def test_heading_servo_loads():
|
||||
assert hs.vertical_enabled is True # 决策3：无人机 L1 开垂直
|
||||
|
||||
|
||||
def test_deployed_config_has_calibrated_signs():
|
||||
"""S1 台架标定回归(2026-10-09)：目标在左下时 off_target_x/y 均为负 →
|
||||
吊舱 X 轴右正(与控制律 β 同向) beta_sign=+1；Y 轴上正(与 α 下正相反) alpha_sign=-1。
|
||||
厂商确认本版电机无用 → 框架角路径必须关。改动这些值前必须重新台架标定。"""
|
||||
cfg = load_from_json(str(_DEFAULT_CONFIG))
|
||||
assert cfg.heading_servo.beta_sign == 1
|
||||
assert cfg.heading_servo.alpha_sign == -1
|
||||
assert cfg.heading_servo.beta_from_frame is False
|
||||
assert cfg.heading_servo.alpha_from_frame is False
|
||||
|
||||
|
||||
def test_heading_servo_partial_override(tmp_path):
|
||||
cfg = load_from_json(_write(tmp_path, {
|
||||
"heading_servo": {"beta_sign": -1, "beta_from_frame": True},
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
"""ground_link_test.py —— 地面坐标注入通道单元测试。
|
||||
|
||||
覆盖：parse_payload 解析（合法/缺字段/非法 JSON/非对象/类型错误）、
|
||||
is_valid 新鲜度判定（未注入/新鲜/超龄边界）。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget
|
||||
|
||||
|
||||
# --- parse_payload ----------------------------------------------------------
|
||||
def test_parse_valid_payload():
|
||||
t = GroundLink.parse_payload(
|
||||
b'{"lat": 39.9042, "lon": 116.4074, "alt": 50.0}')
|
||||
assert t is not None
|
||||
assert t.latitude_deg == pytest.approx(39.9042)
|
||||
assert t.longitude_deg == pytest.approx(116.4074)
|
||||
assert t.abs_alt_m == pytest.approx(50.0)
|
||||
assert t.time_ms == 0 # 打戳由接收方（run）负责
|
||||
|
||||
|
||||
def test_parse_default_alt_is_zero():
|
||||
t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0}')
|
||||
assert t is not None
|
||||
assert t.abs_alt_m == 0.0
|
||||
|
||||
|
||||
def test_parse_missing_lat_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_missing_lon_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lat": 1.0}') is None
|
||||
|
||||
|
||||
def test_parse_invalid_json_returns_none():
|
||||
assert GroundLink.parse_payload(b'not json') is None
|
||||
|
||||
|
||||
def test_parse_non_object_returns_none():
|
||||
assert GroundLink.parse_payload(b'[1, 2, 3]') is None
|
||||
|
||||
|
||||
def test_parse_string_coords_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lat": "x", "lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_bool_coords_returns_none():
|
||||
# True 是 bool 不是数值 → 拒绝
|
||||
assert GroundLink.parse_payload(b'{"lat": true, "lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_ignores_ts_field():
|
||||
"""ts 字段仅诊断，不参与解析（新鲜度用本地接收时刻）。"""
|
||||
t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0, "ts": 999}')
|
||||
assert t is not None
|
||||
assert t.latitude_deg == pytest.approx(1.0)
|
||||
|
||||
|
||||
# --- is_valid ---------------------------------------------------------------
|
||||
def _link(timeout_ms=5000):
|
||||
return GroundLink(GroundInjectConfig(timeout_ms=timeout_ms),
|
||||
now_ms_fn=lambda: 0)
|
||||
|
||||
|
||||
def _inject(link, time_ms):
|
||||
link._target = GroundTarget(latitude_deg=1.0, longitude_deg=2.0,
|
||||
time_ms=time_ms)
|
||||
|
||||
|
||||
def test_is_valid_false_when_never_injected():
|
||||
assert _link().is_valid(1000) is False
|
||||
|
||||
|
||||
def test_is_valid_true_when_fresh():
|
||||
link = _link(timeout_ms=5000)
|
||||
_inject(link, 1000)
|
||||
assert link.is_valid(1000) is True
|
||||
assert link.is_valid(4000) is True
|
||||
|
||||
|
||||
def test_is_valid_timeout_boundary():
|
||||
link = _link(timeout_ms=5000)
|
||||
_inject(link, 1000)
|
||||
assert link.is_valid(6000) is True # 恰好 5000ms，未超龄
|
||||
assert link.is_valid(6001) is False # 5001ms，超龄
|
||||
|
||||
|
||||
def test_latest_returns_injected_target():
|
||||
link = _link()
|
||||
_inject(link, 1000)
|
||||
assert link.latest().latitude_deg == pytest.approx(1.0)
|
||||
assert link.latest().time_ms == 1000
|
||||
@@ -5,6 +5,7 @@
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import math
|
||||
from enum import Enum
|
||||
from types import SimpleNamespace
|
||||
|
||||
@@ -62,7 +63,7 @@ class FakeTelemetry:
|
||||
def gps_info(self):
|
||||
return self._stream(self._gps)
|
||||
|
||||
def velocity_body(self):
|
||||
def velocity_ned(self):
|
||||
return self._stream(self._velocity)
|
||||
|
||||
|
||||
@@ -77,8 +78,9 @@ def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0):
|
||||
absolute_altitude_m=aalt, relative_altitude_m=ralt)
|
||||
|
||||
|
||||
def _att(roll=0.1, pitch=0.2, yaw=1.57):
|
||||
return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw)
|
||||
def _att(roll_deg=5.0, pitch_deg=10.0, yaw_deg=90.0):
|
||||
"""MAVSDK EulerAngle 字段是度（roll_deg/pitch_deg/yaw_deg），网关转弧度。"""
|
||||
return SimpleNamespace(roll_deg=roll_deg, pitch_deg=pitch_deg, yaw_deg=yaw_deg)
|
||||
|
||||
|
||||
# --- 枚举转换 ---------------------------------------------------------------
|
||||
@@ -120,12 +122,14 @@ def test_update_attitude_sets_radians():
|
||||
gw.update_attitude(_att())
|
||||
s = gw.own_state()
|
||||
assert s.has_attitude is True
|
||||
assert s.yaw_rad == pytest.approx(1.57)
|
||||
assert s.roll_rad == pytest.approx(math.radians(5.0))
|
||||
assert s.pitch_rad == pytest.approx(math.radians(10.0))
|
||||
assert s.yaw_rad == pytest.approx(math.radians(90.0))
|
||||
|
||||
|
||||
def test_update_velocity_body_ground_speed_is_horizontal_magnitude():
|
||||
def test_update_velocity_ned_ground_speed_is_horizontal_magnitude():
|
||||
gw = Px4Gateway(FakeDrone())
|
||||
gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0,
|
||||
gw.update_velocity_ned(SimpleNamespace(north_m_s=3.0, east_m_s=4.0,
|
||||
down_m_s=-9.9))
|
||||
assert gw.own_state().ground_speed_mps == pytest.approx(5.0) # 不含 down
|
||||
|
||||
@@ -173,9 +177,9 @@ def test_command_delegation_reaches_offboard():
|
||||
def test_start_telemetry_populates_cache_then_stop():
|
||||
telemetry = FakeTelemetry(
|
||||
position=[_pos(lat=22.0, lon=114.0)],
|
||||
attitude=[_att(yaw=0.5)],
|
||||
attitude=[_att(yaw_deg=30.0)],
|
||||
gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
|
||||
velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)],
|
||||
velocity=[SimpleNamespace(north_m_s=2.0, east_m_s=0.0, down_m_s=0.0)],
|
||||
)
|
||||
gw = Px4Gateway(FakeDrone(telemetry))
|
||||
|
||||
@@ -184,7 +188,7 @@ def test_start_telemetry_populates_cache_then_stop():
|
||||
await asyncio.sleep(0.05) # 让后台任务消费预设消息
|
||||
s = gw.own_state()
|
||||
assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0
|
||||
assert s.yaw_rad == pytest.approx(0.5)
|
||||
assert s.yaw_rad == pytest.approx(math.radians(30.0))
|
||||
assert s.position_valid is True
|
||||
assert s.ground_speed_mps == pytest.approx(2.0)
|
||||
await gw.stop_telemetry()
|
||||
@@ -210,7 +214,7 @@ def test_position_timestamp_not_refreshed_by_other_streams():
|
||||
clock[0] = 2000
|
||||
gw.update_attitude(_att())
|
||||
gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9))
|
||||
gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0,
|
||||
gw.update_velocity_ned(SimpleNamespace(north_m_s=1.0, east_m_s=0.0,
|
||||
down_m_s=0.0))
|
||||
s = gw.own_state()
|
||||
assert s.time_ms == 2000 # 诊断时间戳照常刷新
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
"""virtual_pod_bridge_test.py —— VirtualPod × PodBridge × TrackingControlLaw 集成测试。
|
||||
|
||||
用部署配置(pod_bridge.json)把"SITL 闭环段"在无飞控下跑通，端到端复核 S1 标定：
|
||||
目标偏右 → 控制律应输出正 yaw_rate（右转）；
|
||||
目标偏下 → 应输出正 vz（下降）；
|
||||
目标消失 → 应判丢零速刹车。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import load_from_json
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||||
VirtualTargetConfig)
|
||||
|
||||
_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"
|
||||
|
||||
|
||||
class FakePx4:
|
||||
def __init__(self, yaw_rad=0.0):
|
||||
self._yaw = yaw_rad
|
||||
self.clock = 1000 # 测试循环每拍推进，与 step(now_ms) 同步
|
||||
self.velocity_calls = []
|
||||
self.brake_calls = []
|
||||
|
||||
def own_state(self):
|
||||
return OwnState(has_attitude=True, has_position=True, position_valid=True,
|
||||
fix_type=3, yaw_rad=self._yaw, time_ms=self.clock,
|
||||
position_time_ms=self.clock)
|
||||
|
||||
async def start_telemetry(self): pass
|
||||
async def stop_telemetry(self): pass
|
||||
async def start_offboard(self): return True
|
||||
async def stop_offboard(self): pass
|
||||
async def send_velocity_heading(self, out, time_boot_ms=None):
|
||||
self.velocity_calls.append(out); return True
|
||||
async def send_brake_hover(self, time_boot_ms=None):
|
||||
self.brake_calls.append(time_boot_ms); return True
|
||||
|
||||
|
||||
def _bridge(target, own):
|
||||
cfg = load_from_json(str(_DEFAULT_CONFIG)) # 部署配置：L1 enabled、S1 符号
|
||||
pod = VirtualPod(target, lambda: own)
|
||||
px4 = FakePx4(yaw_rad=own.yaw_rad)
|
||||
return PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0), px4
|
||||
|
||||
|
||||
def _static_target(ahead=10.0, right=0.0, down=0.0):
|
||||
cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
|
||||
arc_speed_mps=0.0)
|
||||
return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
|
||||
|
||||
|
||||
def _run_ticks(bridge, px4, n, t0=1000, dt=50):
|
||||
"""跑 n 拍（每拍 50ms，节拍与 20Hz 一致），返回末拍输出。"""
|
||||
out = None
|
||||
for i in range(n):
|
||||
t = t0 + i * dt
|
||||
px4.clock = t
|
||||
out = asyncio.run(bridge.step(t))
|
||||
return out
|
||||
|
||||
|
||||
def test_deployed_config_turns_right_for_right_target():
|
||||
# 目标在右前方：S1 标定 beta_sign=+1 → yaw_rate 应为正（右转）
|
||||
tgt = _static_target(ahead=10.0, right=5.0)
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
out = _run_ticks(bridge, px4, 5)
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps > 0.0
|
||||
assert px4.velocity_calls # 正常速度航向指令
|
||||
assert not px4.brake_calls
|
||||
|
||||
|
||||
def test_deployed_config_descends_for_below_target():
|
||||
# 目标偏下：S1 标定 alpha_sign=-1（吊舱上正→控制律下正）→ vz 应为正（下降）
|
||||
tgt = _static_target(ahead=10.0, down=2.0)
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
out = _run_ticks(bridge, px4, 10)
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.vz_mps > 0.0
|
||||
|
||||
|
||||
def test_deployed_config_slow_approach_forward():
|
||||
# L1 无距离：slow_approach 0.2 m/s 起步（决策3 低限速）
|
||||
tgt = _static_target()
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
out = _run_ticks(bridge, px4, 10)
|
||||
assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
|
||||
|
||||
|
||||
def test_target_lost_brakes_through_pipeline():
|
||||
tgt = _static_target()
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
_run_ticks(bridge, px4, 3)
|
||||
tgt.set_visible(False) # 目标消失 → 吊舱转搜索态 → 判丢刹车
|
||||
out = _run_ticks(bridge, px4, 2, t0=1000 + 3 * 50)
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
assert px4.brake_calls
|
||||
@@ -0,0 +1,124 @@
|
||||
"""virtual_pod_test.py —— 模拟吊舱几何与 S1 符号约定回归测试。
|
||||
|
||||
钉死 S1 台架标定结论（2026-10-09）：目标在左下时 off_target_x/y 均为负，
|
||||
即 X 右正、Y 上正；并验证不可见/超视场 → 搜索态 + 脱靶量归 0（对齐真实
|
||||
吊舱"丢失时脱靶量恒 0"的行为）。
|
||||
"""
|
||||
|
||||
import math
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||||
VirtualTargetConfig)
|
||||
|
||||
|
||||
def _pod(target, own):
|
||||
return VirtualPod(target, lambda: own)
|
||||
|
||||
|
||||
def _static_target(ahead=30.0, right=0.0, down=0.0):
|
||||
"""放在锚点正前方（机头朝北）的静止目标。"""
|
||||
cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
|
||||
arc_speed_mps=0.0)
|
||||
return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
|
||||
|
||||
|
||||
def _carrier(yaw_deg=0.0):
|
||||
return CarrierView(north_m=0.0, east_m=0.0, down_m=0.0,
|
||||
yaw_rad=math.radians(yaw_deg))
|
||||
|
||||
|
||||
def _telemetry(pod):
|
||||
frames = pod.poll()
|
||||
assert len(frames) == 1
|
||||
return frames[0]
|
||||
|
||||
|
||||
# ---- S1 符号约定回归：目标位置 → 脱靶量符号 --------------------------------
|
||||
def test_target_right_gives_positive_off_x():
|
||||
# 目标在右前方：rel_e>0、机头朝北 → 脱靶量 X 应为正（右正）
|
||||
tgt = _static_target(ahead=10.0, right=5.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_x_deg > 0.0
|
||||
assert tel.off_target_x_deg == pytest.approx(math.degrees(math.atan2(5.0, 10.0)),
|
||||
abs=1e-9)
|
||||
|
||||
|
||||
def test_target_left_gives_negative_off_x():
|
||||
tgt = _static_target(ahead=10.0, right=-5.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_x_deg < 0.0
|
||||
|
||||
|
||||
def test_target_below_gives_negative_off_y():
|
||||
# S1 实测：目标偏下 → off_target_y 为负（上正）。11.3° 在垂直视场(35°)内
|
||||
tgt = _static_target(ahead=10.0, down=2.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_y_deg < 0.0
|
||||
assert tel.off_target_y_deg == pytest.approx(-math.degrees(math.atan2(2.0, 10.0)),
|
||||
abs=1e-9)
|
||||
|
||||
|
||||
def test_target_above_gives_positive_off_y():
|
||||
tgt = _static_target(ahead=10.0, down=-2.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_y_deg > 0.0
|
||||
|
||||
|
||||
def test_yaw_rotation_follows_body_frame():
|
||||
# 机头偏东 20°：目标在正北 → 目标在机体左侧 20° → off_x 为负
|
||||
tgt = _static_target(ahead=10.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier(yaw_deg=20.0)))
|
||||
assert tel.off_target_x_deg == pytest.approx(-20.0, abs=1e-9)
|
||||
|
||||
|
||||
# ---- 无电机 / 无激光语义 ---------------------------------------------------
|
||||
def test_frame_angles_always_zero_and_laser_none():
|
||||
tgt = _static_target()
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.frame_yaw_deg == 0.0
|
||||
assert tel.frame_pitch_deg == 0.0
|
||||
assert tel.frame_roll_deg == 0.0
|
||||
assert tel.laser_range_m is None
|
||||
|
||||
|
||||
# ---- 锁定门控 --------------------------------------------------------------
|
||||
def test_visible_target_locks():
|
||||
tel = _telemetry(_pod(_static_target(), _carrier()))
|
||||
assert tel.is_target_locked
|
||||
|
||||
|
||||
def test_hidden_target_searches_with_zero_off():
|
||||
tgt = _static_target()
|
||||
tgt.set_visible(False)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert not tel.is_target_locked
|
||||
assert tel.off_target_x_deg == 0.0 and tel.off_target_y_deg == 0.0
|
||||
|
||||
|
||||
def test_target_behind_searches():
|
||||
# 机头朝北、目标在正南 10m → fwd<0 → 搜索态
|
||||
cfg = VirtualTargetConfig(ahead_m=-10.0, right_m=0.0, down_m=0.0,
|
||||
arc_speed_mps=0.0)
|
||||
tgt = VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert not tel.is_target_locked
|
||||
|
||||
|
||||
def test_target_out_of_fov_searches():
|
||||
# 目标几乎在正右方（β→90°）超出默认 FOV 60° → 搜索态
|
||||
tgt = _static_target(ahead=1.0, right=100.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert not tel.is_target_locked
|
||||
|
||||
|
||||
# ---- PodLink 接口 ----------------------------------------------------------
|
||||
def test_send_counts_and_close_marks():
|
||||
pod = _pod(_static_target(), _carrier())
|
||||
pod.send(b"\x00" * 4)
|
||||
pod.send(b"\x00" * 4)
|
||||
assert pod.attitude_frames_rx == 2
|
||||
pod.close()
|
||||
assert pod.closed
|
||||
@@ -0,0 +1,123 @@
|
||||
"""log_test.py —— 任务日志模块单元测试。
|
||||
|
||||
覆盖：open 建目录/写表头、事件 JSONL、遥测 CSV、telemetry_every_n 节流、
|
||||
disabled 时零副作用、close 收尾。
|
||||
"""
|
||||
|
||||
import csv
|
||||
import json
|
||||
|
||||
import pytest
|
||||
|
||||
from task_runner.log import TaskLogConfig, TaskLogger, TelemetryRecord
|
||||
|
||||
|
||||
def _cfg(tmp_path, **kw):
|
||||
c = TaskLogConfig(dir=str(tmp_path), **kw)
|
||||
return c
|
||||
|
||||
|
||||
def test_open_creates_dir_and_csv_header(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.close()
|
||||
|
||||
assert (tmp_path / "task_events.jsonl").exists()
|
||||
assert (tmp_path / "task_telemetry.csv").exists()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
header = next(csv.reader(f))
|
||||
assert header[0] == "t_s"
|
||||
assert "state" in header
|
||||
assert "distance_to_target_m" in header
|
||||
|
||||
|
||||
def test_log_event_writes_jsonl(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="wait",
|
||||
to_state="arm_takeoff", reason="收到地面坐标")
|
||||
logger.log_event(2000, "emergency", reason="RC 接管")
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_events.jsonl", encoding="utf-8") as f:
|
||||
lines = [json.loads(x) for x in f if x.strip()]
|
||||
assert len(lines) == 2
|
||||
assert lines[0]["type"] == "state"
|
||||
assert lines[0]["from_state"] == "wait"
|
||||
assert lines[0]["to_state"] == "arm_takeoff"
|
||||
assert lines[0]["ts_ms"] == 1000
|
||||
assert lines[0]["t_s"] == 0.0 # 首个事件 t_s = 0
|
||||
assert lines[1]["t_s"] == 1.0 # 第二个事件 t_s = 1.0s
|
||||
|
||||
|
||||
def test_log_telemetry_writes_csv_rows(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
rec = TelemetryRecord(state="goto", pos_valid=True, latitude_deg=47.39,
|
||||
distance_to_target_m=33.5, mode="VELOCITY_HEADING",
|
||||
yaw_rate_dps=-30.0, vx_body_mps=1.7)
|
||||
logger.log_telemetry(1000, rec)
|
||||
logger.log_telemetry(1100, rec)
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 2
|
||||
assert rows[0]["state"] == "goto"
|
||||
assert rows[0]["pos_valid"] == "True"
|
||||
assert rows[0]["distance_to_target_m"] == "33.5"
|
||||
assert rows[0]["t_s"] == "0.0"
|
||||
assert rows[1]["t_s"] == "0.1"
|
||||
|
||||
|
||||
def test_telemetry_throttled_by_every_n(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_every_n=5))
|
||||
logger.open()
|
||||
rec = TelemetryRecord(state="goto")
|
||||
for i in range(12):
|
||||
logger.log_telemetry(1000 + i * 100, rec)
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 2 # 第 5、10 次各记一次
|
||||
|
||||
|
||||
def test_disabled_logs_nothing(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path, enabled=False))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="wait", to_state="arm_takeoff")
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="goto"))
|
||||
logger.close()
|
||||
|
||||
# 未 open（disabled）→ 不建目录、不写文件
|
||||
assert not (tmp_path / "task_events.jsonl").exists()
|
||||
assert not (tmp_path / "task_telemetry.csv").exists()
|
||||
|
||||
|
||||
def test_log_before_open_is_noop(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.log_event(1000, "state", from_state="wait", to_state="arm_takeoff")
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="goto"))
|
||||
logger.close()
|
||||
assert not (tmp_path / "task_events.jsonl").exists()
|
||||
|
||||
|
||||
def test_append_keeps_existing_rows(tmp_path):
|
||||
"""重复 open（追加模式）不清空既有内容，表头不重复写。"""
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="wait"))
|
||||
logger.close()
|
||||
|
||||
logger2 = TaskLogger(_cfg(tmp_path))
|
||||
logger2.open()
|
||||
logger2.log_telemetry(2000, TelemetryRecord(state="goto"))
|
||||
logger2.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 2
|
||||
assert rows[0]["state"] == "wait"
|
||||
assert rows[1]["state"] == "goto"
|
||||
@@ -0,0 +1,45 @@
|
||||
"""pod_control_test.py —— 吊舱目标识别/锁定控制器单元测试。
|
||||
|
||||
覆盖：arm_detection 发 0x07(识别,目标类型)+0x0F(自动锁定)、
|
||||
stop_tracking 发 0x0E(停止)+0x08(识别关)。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode
|
||||
from task_runner.pod_control import PodController
|
||||
|
||||
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.sent = []
|
||||
|
||||
def send_control(self, cmd):
|
||||
self.sent.append(cmd)
|
||||
|
||||
|
||||
def test_arm_detection_sends_detect_and_autolock():
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge, target_type=1)
|
||||
pc.arm_detection()
|
||||
|
||||
assert len(bridge.sent) == 2
|
||||
assert bridge.sent[0].code == ControlCode.TARGET_DETECT_ON
|
||||
assert bridge.sent[0].param_x == 1 # 目标类型 1 = 无人机
|
||||
assert bridge.sent[1].code == ControlCode.AUTO_LOCK_ON_FOUND
|
||||
|
||||
|
||||
def test_arm_detection_default_target_type_is_person_vehicle():
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge) # 默认 target_type=0 = 人/车
|
||||
pc.arm_detection()
|
||||
assert bridge.sent[0].code == ControlCode.TARGET_DETECT_ON
|
||||
assert bridge.sent[0].param_x == 0
|
||||
|
||||
|
||||
def test_stop_tracking_sends_stop_and_detect_off():
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.stop_tracking()
|
||||
|
||||
assert len(bridge.sent) == 2
|
||||
assert bridge.sent[0].code == ControlCode.STOP_TRACK
|
||||
assert bridge.sent[1].code == ControlCode.TARGET_DETECT_OFF
|
||||
@@ -0,0 +1,311 @@
|
||||
"""runner_test.py —— 任务编排器单元测试（mock px4/bridge/ground/drone，无硬件）。
|
||||
|
||||
覆盖：输入采集（注入坐标新鲜/超龄、起飞到位、锁定、居中累加）、
|
||||
状态进入动作（ARM_TAKEOFF 触发起飞、GOTO 切 L2、TRACK 切 L1）。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
from dataclasses import replace
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.config import BridgeConfig
|
||||
from pod_bridge.ground_link import GroundTarget
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
from pod_bridge.protocol import Telemetry
|
||||
from task_runner.runner import TaskRunner
|
||||
from task_runner.state_machine import (TaskState, TaskStateConfig,
|
||||
TaskStateMachine)
|
||||
|
||||
|
||||
# --- mocks ------------------------------------------------------------------
|
||||
class FakePx4:
|
||||
def __init__(self, state=None):
|
||||
self._state = state or OwnState()
|
||||
self.telemetry_started = 0
|
||||
self.offboard_started = False
|
||||
self.brake_calls = []
|
||||
|
||||
def own_state(self):
|
||||
return replace(self._state)
|
||||
|
||||
async def start_telemetry(self):
|
||||
self.telemetry_started += 1
|
||||
|
||||
async def stop_telemetry(self):
|
||||
pass
|
||||
|
||||
async def start_offboard(self):
|
||||
self.offboard_started = True
|
||||
return True
|
||||
|
||||
async def stop_offboard(self):
|
||||
pass
|
||||
|
||||
async def send_velocity_heading(self, out, t=None):
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, t=None):
|
||||
self.brake_calls.append(t)
|
||||
return True
|
||||
|
||||
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.started = False
|
||||
self.stopped = False
|
||||
self.law = None # set_heading_servo_enabled 最后值
|
||||
self.steps = 0
|
||||
self._locked = False
|
||||
self._telemetry = None
|
||||
|
||||
async def start(self):
|
||||
self.started = True
|
||||
return True
|
||||
|
||||
async def stop(self):
|
||||
self.stopped = True
|
||||
|
||||
def set_heading_servo_enabled(self, enabled):
|
||||
self.law = enabled
|
||||
|
||||
def pod_locked(self, now_ms):
|
||||
return self._locked
|
||||
|
||||
async def step(self, now_ms):
|
||||
self.steps += 1
|
||||
|
||||
@property
|
||||
def latest_telemetry(self):
|
||||
return self._telemetry
|
||||
|
||||
|
||||
class FakeGround:
|
||||
def __init__(self, target=None):
|
||||
self._target = target
|
||||
|
||||
def latest(self):
|
||||
return self._target or GroundTarget()
|
||||
|
||||
|
||||
class FakeDrone:
|
||||
def __init__(self):
|
||||
self.armed = False
|
||||
self.took_off = False
|
||||
self.takeoff_alt = 0.0
|
||||
self.rtl = False
|
||||
self.landed = False
|
||||
self.disarmed = False
|
||||
|
||||
class _Action:
|
||||
def __init__(self, d):
|
||||
self._d = d
|
||||
|
||||
async def arm(self):
|
||||
self._d.armed = True
|
||||
|
||||
async def set_takeoff_altitude(self, a):
|
||||
self._d.takeoff_alt = a
|
||||
|
||||
async def takeoff(self):
|
||||
self._d.took_off = True
|
||||
|
||||
async def return_to_launch(self):
|
||||
self._d.rtl = True
|
||||
|
||||
async def land(self):
|
||||
self._d.landed = True
|
||||
|
||||
async def disarm(self):
|
||||
self._d.disarmed = True
|
||||
|
||||
@property
|
||||
def action(self):
|
||||
return FakeDrone._Action(self)
|
||||
|
||||
|
||||
def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
|
||||
return OwnState(latitude_deg=lat, longitude_deg=lon,
|
||||
relative_altitude_m=rel_alt, fix_type=3, num_satellites=12,
|
||||
has_position=True, has_attitude=True, position_valid=True,
|
||||
time_ms=now_ms, position_time_ms=now_ms)
|
||||
|
||||
|
||||
def _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||||
sm_cfg=None, vision_fn=None, pod_control=None):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = 5000
|
||||
return TaskRunner(
|
||||
cfg, sm_cfg or TaskStateConfig(),
|
||||
px4 or FakePx4(_state(1000)),
|
||||
bridge or FakeBridge(),
|
||||
TaskStateMachine(sm_cfg or TaskStateConfig()),
|
||||
ground or FakeGround(),
|
||||
drone or FakeDrone(),
|
||||
vision_fn=vision_fn,
|
||||
now_ms_fn=lambda: 1000,
|
||||
pod_control=pod_control,
|
||||
)
|
||||
|
||||
|
||||
# --- 输入采集 ---------------------------------------------------------------
|
||||
def test_collect_injected_fresh_sets_distance():
|
||||
ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000))
|
||||
px4 = FakePx4(_state(1000))
|
||||
r = _make_runner(px4=px4, ground=ground)
|
||||
|
||||
inp = r._collect_inputs(1000)
|
||||
assert inp.target_injected is True
|
||||
assert inp.distance_to_target_m == pytest.approx(111.19, rel=1e-2) # ~111m
|
||||
assert inp.takeoff_reached is False # rel_alt=0 < 5
|
||||
|
||||
|
||||
def test_collect_injected_stale_is_invalid():
|
||||
ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000))
|
||||
r = _make_runner(ground=ground) # timeout=5000
|
||||
|
||||
inp = r._collect_inputs(7000) # 6000ms 差值 > 5000 → 超龄
|
||||
assert inp.target_injected is False
|
||||
assert inp.distance_to_target_m == float("inf")
|
||||
|
||||
|
||||
def test_collect_takeoff_reached():
|
||||
px4 = FakePx4(_state(1000, rel_alt=5.0))
|
||||
sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
|
||||
r = _make_runner(px4=px4, sm_cfg=sm_cfg)
|
||||
|
||||
assert r._collect_inputs(1000).takeoff_reached is True
|
||||
|
||||
|
||||
def test_collect_locked_from_bridge():
|
||||
bridge = FakeBridge()
|
||||
bridge._locked = True
|
||||
r = _make_runner(bridge=bridge)
|
||||
|
||||
assert r._collect_inputs(1000).target_locked is True
|
||||
|
||||
|
||||
def test_collect_vision_second_judgement():
|
||||
r = _make_runner(vision_fn=lambda: True)
|
||||
assert r._collect_inputs(1000).vision_has_target is True
|
||||
|
||||
r2 = _make_runner(vision_fn=lambda: False)
|
||||
assert r2._collect_inputs(1000).vision_has_target is False
|
||||
|
||||
|
||||
# --- 居中累加 ---------------------------------------------------------------
|
||||
def test_centered_accumulates_duration():
|
||||
bridge = FakeBridge()
|
||||
bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
|
||||
sm_cfg = TaskStateConfig(center_threshold_deg=1.0)
|
||||
r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)
|
||||
|
||||
assert r._update_centered(1000) == (True, 0.0) # 开始居中
|
||||
assert r._update_centered(3000) == (True, 2.0) # 持续 2s
|
||||
assert r._update_centered(3500) == (True, 2.5) # 持续 2.5s
|
||||
|
||||
|
||||
def test_centered_resets_on_large_offset():
|
||||
bridge = FakeBridge()
|
||||
bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
|
||||
r = _make_runner(bridge=bridge)
|
||||
|
||||
r._update_centered(1000)
|
||||
bridge._telemetry = (Telemetry(off_target_x_deg=5.0, off_target_y_deg=0.1), 2000)
|
||||
assert r._update_centered(2000) == (False, 0.0) # 脱靶量大 → 清零
|
||||
|
||||
|
||||
def test_centered_no_telemetry_returns_false():
|
||||
r = _make_runner() # bridge.latest_telemetry is None
|
||||
assert r._update_centered(1000) == (False, 0.0)
|
||||
|
||||
|
||||
# --- 状态进入动作 -----------------------------------------------------------
|
||||
def test_on_enter_arm_takeoff_arms_and_takes_off():
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(drone=drone, sm_cfg=TaskStateConfig(takeoff_altitude_m=5.0))
|
||||
asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
|
||||
assert drone.armed is True
|
||||
assert drone.took_off is True
|
||||
assert drone.takeoff_alt == pytest.approx(5.0)
|
||||
|
||||
|
||||
def test_on_enter_goto_switches_to_l2():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
assert bridge.started is True
|
||||
assert bridge.law is False # L2 地理引导
|
||||
|
||||
|
||||
def test_on_enter_track_switches_to_l1():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
asyncio.run(r._on_enter(TaskState.TRACK, 1000))
|
||||
assert bridge.law is True # L1 航向伺服
|
||||
|
||||
|
||||
def test_on_enter_rtl_stops_bridge_and_returns():
|
||||
bridge = FakeBridge()
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(bridge=bridge, drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
assert bridge.stopped is True
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
# --- 持续动作 ---------------------------------------------------------------
|
||||
def test_tick_goto_steps_bridge():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
asyncio.run(r._tick(1000))
|
||||
# WAIT 状态不 step；切到 GOTO 才 step
|
||||
r._sm._state = TaskState.GOTO
|
||||
asyncio.run(r._tick(1000))
|
||||
assert bridge.steps == 1
|
||||
|
||||
|
||||
def test_tick_approach_brakes():
|
||||
px4 = FakePx4(_state(1000))
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm._state = TaskState.APPROACH
|
||||
asyncio.run(r._tick(1000))
|
||||
assert len(px4.brake_calls) == 1
|
||||
|
||||
|
||||
# --- 吊舱控制集成（真机）----------------------------------------------------
|
||||
class FakePodControl:
|
||||
def __init__(self):
|
||||
self.armed = False
|
||||
self.stopped = False
|
||||
|
||||
def arm_detection(self):
|
||||
self.armed = True
|
||||
|
||||
def stop_tracking(self):
|
||||
self.stopped = True
|
||||
|
||||
|
||||
def test_on_enter_goto_arms_detection():
|
||||
pod_control = FakePodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
assert pod_control.armed is True
|
||||
assert pod_control.stopped is False
|
||||
|
||||
|
||||
def test_on_enter_rtl_stops_tracking_before_teardown():
|
||||
pod_control = FakePodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
assert pod_control.stopped is True
|
||||
|
||||
|
||||
def test_no_pod_control_is_safe():
|
||||
"""未注入 pod_control（SITL）→ GOTO/RTL 进入动作不触发吊舱控制、不抛异常。"""
|
||||
r = _make_runner(pod_control=None)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
@@ -0,0 +1,233 @@
|
||||
"""state_machine_test.py —— 任务级状态机单元测试。
|
||||
|
||||
覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
|
||||
GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
|
||||
RTL 终态与 reset。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from task_runner.state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
||||
TaskStateMachine, TaskTransition)
|
||||
|
||||
|
||||
def _inp(**kw) -> TaskInputs:
|
||||
return TaskInputs(**kw)
|
||||
|
||||
|
||||
def _sm(cfg=None) -> TaskStateMachine:
|
||||
return TaskStateMachine(cfg or TaskStateConfig())
|
||||
|
||||
|
||||
# --- 初始态与 reset ---------------------------------------------------------
|
||||
def test_initial_state_is_wait():
|
||||
assert _sm().state is TaskState.WAIT
|
||||
|
||||
|
||||
def test_reset_returns_to_wait():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
assert sm.state is TaskState.ARM_TAKEOFF
|
||||
sm.reset()
|
||||
assert sm.state is TaskState.WAIT
|
||||
|
||||
|
||||
# --- WAIT -------------------------------------------------------------------
|
||||
def test_wait_without_coords_stays():
|
||||
tr = _sm().update(_inp(), 1000)
|
||||
assert not tr.occurred
|
||||
assert tr.to_state is TaskState.WAIT
|
||||
|
||||
|
||||
def test_wait_receives_coords_goes_arm_takeoff():
|
||||
sm = _sm()
|
||||
tr = sm.update(_inp(target_injected=True), 1000)
|
||||
assert tr.occurred
|
||||
assert tr.from_state is TaskState.WAIT
|
||||
assert tr.to_state is TaskState.ARM_TAKEOFF
|
||||
assert sm.state is TaskState.ARM_TAKEOFF
|
||||
|
||||
|
||||
# --- ARM_TAKEOFF ------------------------------------------------------------
|
||||
def test_arm_takeoff_reaches_altitude_goes_goto():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
tr = sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
assert tr.occurred and tr.to_state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_arm_takeoff_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(arm_takeoff_timeout_s=60.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
# 60s 未到高度 → 超时返航
|
||||
tr = sm.update(_inp(target_injected=True), 1000 + 61_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- GOTO -------------------------------------------------------------------
|
||||
def test_goto_far_away_stays():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0,
|
||||
target_locked=True), 3000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_goto_within_radius_and_locked_goes_track():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
assert tr.occurred and tr.to_state is TaskState.TRACK
|
||||
|
||||
|
||||
def test_goto_within_radius_but_unlocked_stays():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=False), 3000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_goto_requires_vision_when_enabled():
|
||||
cfg = TaskStateConfig(require_vision=True)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
# 有锁定但无视觉第二判据 → 不切换
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True, vision_has_target=False), 3000)
|
||||
assert not tr.occurred
|
||||
# 补上视觉 → 切换
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True, vision_has_target=True), 4000)
|
||||
assert tr.occurred and tr.to_state is TaskState.TRACK
|
||||
|
||||
|
||||
def test_goto_loses_coords_goes_rtl():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=False, distance_to_target_m=10.0), 3000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_goto_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(goto_timeout_s=120.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), 2000 + 121_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- TRACK ------------------------------------------------------------------
|
||||
def test_track_centered_stable_goes_approach():
|
||||
sm = _sm(TaskStateConfig(center_hold_s=3.0))
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
assert sm.state is TaskState.TRACK
|
||||
# 居中但未满时长 → 停留
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
||||
target_centered=True, centered_duration_s=1.0), 4000)
|
||||
assert not tr.occurred
|
||||
# 居中稳定满 3s → APPROACH
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
||||
target_centered=True, centered_duration_s=3.0), 5000)
|
||||
assert tr.occurred and tr.to_state is TaskState.APPROACH
|
||||
|
||||
|
||||
def test_track_lost_with_coords_goes_back_to_goto():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=False), 4000)
|
||||
assert tr.occurred and tr.to_state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_track_lost_without_coords_goes_rtl():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
tr = sm.update(_inp(target_injected=False, target_locked=False), 4000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_track_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(track_timeout_s=120.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 3000 + 121_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- APPROACH ---------------------------------------------------------------
|
||||
def test_approach_hover_done_goes_rtl():
|
||||
cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
sm.update(_inp(target_injected=True, target_locked=True, target_centered=True,
|
||||
centered_duration_s=0.0), 4000)
|
||||
assert sm.state is TaskState.APPROACH
|
||||
# 悬停未满 10s → 停留
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 5000)
|
||||
assert not tr.occurred
|
||||
# 满 10s → RTL
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- 全局终止（紧急 / 返航）-------------------------------------------------
|
||||
@pytest.mark.parametrize("state_kw", [
|
||||
{}, # WAIT
|
||||
{"target_injected": True}, # 会先转 ARM_TAKEOFF
|
||||
])
|
||||
def test_emergency_goes_rtl_from_any_state(state_kw):
|
||||
sm = _sm()
|
||||
sm.update(_inp(**state_kw), 1000)
|
||||
tr = sm.update(_inp(emergency=True), 2000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_rtl_requested_goes_rtl():
|
||||
sm = _sm()
|
||||
tr = sm.update(_inp(rtl_requested=True), 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- RTL 终态 ---------------------------------------------------------------
|
||||
def test_rtl_is_terminal():
|
||||
sm = _sm()
|
||||
sm.update(_inp(rtl_requested=True), 1000)
|
||||
assert sm.state is TaskState.RTL
|
||||
# 终态不再转移（连紧急/坐标也不动）
|
||||
tr = sm.update(_inp(target_injected=True, emergency=True), 2000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.RTL
|
||||
|
||||
|
||||
# --- 转移结果结构 -----------------------------------------------------------
|
||||
def test_transition_dataclass_defaults():
|
||||
tr = TaskTransition()
|
||||
assert tr.from_state is TaskState.WAIT
|
||||
assert tr.to_state is TaskState.WAIT
|
||||
assert tr.occurred is False
|
||||
assert tr.reason == ""
|
||||
Reference in New Issue
Block a user