精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
This commit is contained in:
keita
@@ -1,7 +1,7 @@
|
||||
"""log_test.py —— 任务日志模块单元测试。
|
||||
|
||||
覆盖：open 建目录/写表头、事件 JSONL、遥测 CSV、telemetry_every_n 节流、
|
||||
disabled 时零副作用、close 收尾。
|
||||
disabled 时零副作用、close 收尾、简洁任务简报（task_brief.log）。
|
||||
"""
|
||||
|
||||
import csv
|
||||
@@ -121,3 +121,84 @@ def test_append_keeps_existing_rows(tmp_path):
|
||||
assert len(rows) == 2
|
||||
assert rows[0]["state"] == "wait"
|
||||
assert rows[1]["state"] == "goto"
|
||||
|
||||
|
||||
# --- 简洁任务简报（task_brief.log）-------------------------------------------
|
||||
def test_brief_log_lines_and_summary(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="wait", to_state="wait",
|
||||
reason="任务启动")
|
||||
logger.log_event(2000, "state", from_state="wait", to_state="arm_takeoff",
|
||||
reason="收到地面坐标")
|
||||
logger.log_event(3000, "command", name="rtl")
|
||||
logger.log_event(4000, "fc_status", severity="WARNING", text="Low battery\t")
|
||||
logger.log_event(5000, "takeoff", altitude_m=5.0)
|
||||
logger.log_event(6000, "rtl")
|
||||
logger.log_telemetry(1000, TelemetryRecord(
|
||||
state="wait", relative_altitude_m=0.0, distance_to_target_m=33.5))
|
||||
logger.log_telemetry(1100, TelemetryRecord(
|
||||
state="arm_takeoff", relative_altitude_m=5.2, distance_to_target_m=20.0))
|
||||
logger.log_telemetry(1200, TelemetryRecord(
|
||||
state="arm_takeoff", relative_altitude_m=5.0,
|
||||
distance_to_target_m=float("inf")))
|
||||
logger.close()
|
||||
|
||||
text = (tmp_path / "task_brief.log").read_text(encoding="utf-8")
|
||||
assert "任务开始" in text
|
||||
assert "[00:00.0] 任务启动（当前: 待命）" in text
|
||||
assert "[00:01.0] 阶段: 待命 → 起飞（收到地面坐标）" in text
|
||||
assert "[00:02.0] 地面站命令: rtl" in text
|
||||
assert "[00:03.0] 飞控 WARNING: Low battery" in text # 尾随 tab 已 strip
|
||||
assert "[00:04.0] 起飞至 5.0 m" in text
|
||||
assert "[00:05.0] 开始返航" in text
|
||||
assert "本次运行汇总" in text
|
||||
assert "时长 0.2s" in text # 最后一行遥测的 t_s
|
||||
assert "最高 5.2m" in text
|
||||
assert "最近距目标 20.0m" in text # inf 不计入
|
||||
assert "事件 6 条" in text
|
||||
assert "状态链 待命→起飞" in text
|
||||
|
||||
|
||||
def test_brief_required_no_file_when_disabled_or_closed(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path, enabled=False))
|
||||
logger.open()
|
||||
logger.log_event(1000, "rtl")
|
||||
logger.close()
|
||||
assert not (tmp_path / "task_brief.log").exists()
|
||||
|
||||
|
||||
# --- 遥测落盘节流（2026-10-15：主循环 20Hz 不得每行同步 flush）----------------
|
||||
def test_telemetry_buffered_until_flush_interval(tmp_path):
|
||||
"""telemetry_flush_ms 内只写不 flush；close 兜底落盘，一行不丢。"""
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=1000))
|
||||
logger.open()
|
||||
rec = TelemetryRecord(state="track")
|
||||
logger.log_telemetry(1000, rec) # 首行：落盘一次并打戳
|
||||
for i in range(1, 19): # 其后 19 行都在 1s 内 → 不再 flush
|
||||
logger.log_telemetry(1000 + i * 50, rec)
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 19
|
||||
|
||||
|
||||
def test_telemetry_flush_zero_flushes_every_row(tmp_path):
|
||||
"""telemetry_flush_ms=0 保留旧的「每行立即 flush」行为（兼容/调试）。"""
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=0))
|
||||
logger.open()
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="goto"))
|
||||
assert logger._last_tel_flush_ms == 1000
|
||||
logger.close()
|
||||
|
||||
|
||||
def test_events_still_flushed_immediately(tmp_path):
|
||||
"""事件（阶段切换）仍即时落盘：地面站页面实时性不受遥测节流影响。"""
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=10_000))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="goto", to_state="search")
|
||||
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) == 1
|
||||
logger.close()
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
"""pod_control_test.py —— 吊舱目标识别/锁定控制器单元测试。
|
||||
|
||||
覆盖：arm_detection 发 0x07(识别,目标类型)+0x0F(自动锁定)、
|
||||
stop_tracking 发 0x0E(停止)+0x08(识别关)。
|
||||
stop_tracking 发 0x0E(停止)+0x08(识别关)、地面站手动命令
|
||||
stop_track(0x0E)/auto_lock(0x0F+0x10)/center(0x2B)。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode
|
||||
@@ -43,3 +44,31 @@ def test_stop_tracking_sends_stop_and_detect_off():
|
||||
assert len(bridge.sent) == 2
|
||||
assert bridge.sent[0].code == ControlCode.STOP_TRACK
|
||||
assert bridge.sent[1].code == ControlCode.TARGET_DETECT_OFF
|
||||
|
||||
|
||||
def test_stop_track_sends_only_stop():
|
||||
"""地面站「停跟踪」= 只发 0x0E（0x0F 常驻自动锁定仍生效，对齐按键 u）。"""
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.stop_track()
|
||||
|
||||
assert [c.code for c in bridge.sent] == [ControlCode.STOP_TRACK]
|
||||
|
||||
|
||||
def test_auto_lock_sends_autolock_and_detect_to_track():
|
||||
"""地面站「自动锁定」= 0x0F + 0x10（对齐按键 a）。"""
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.auto_lock()
|
||||
|
||||
assert [c.code for c in bridge.sent] == [ControlCode.AUTO_LOCK_ON_FOUND,
|
||||
ControlCode.DETECT_TO_TRACK]
|
||||
|
||||
|
||||
def test_center_sends_center():
|
||||
"""地面站「归中」= 0x2B（对齐按键 c）。"""
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.center()
|
||||
|
||||
assert [c.code for c in bridge.sent] == [ControlCode.CENTER]
|
||||
|
||||
@@ -0,0 +1,142 @@
|
||||
"""return_home_test.py —— 自定义返航（回起飞点 + 恢复初始航向）纯逻辑测试。
|
||||
|
||||
覆盖：转场速度/对准门控、到达判定、转场高度保持（含符号）、初始航向恢复、
|
||||
角度回绕、以及"不超过转场高度"的语义。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from task_runner.return_home import (HomingController, ReturnConfig,
|
||||
wrap_deg_180)
|
||||
|
||||
HOME_LAT, HOME_LON = 47.397971, 8.546164
|
||||
_DEG_LAT_M = 111320.0
|
||||
|
||||
|
||||
def _offset(lat0, lon0, north_m, east_m):
|
||||
return (lat0 + north_m / _DEG_LAT_M,
|
||||
lon0 + east_m / (_DEG_LAT_M * math.cos(math.radians(lat0))))
|
||||
|
||||
|
||||
def _ctrl(cfg=None, initial_yaw_deg=0.0, transit_alt_m=5.0):
|
||||
return HomingController(cfg or ReturnConfig(), HOME_LAT, HOME_LON,
|
||||
math.radians(initial_yaw_deg), transit_alt_m)
|
||||
|
||||
|
||||
# --- 角度回绕 ---------------------------------------------------------------
|
||||
@pytest.mark.parametrize("deg,expect", [
|
||||
(0.0, 0.0), (179.0, 179.0), (181.0, -179.0),
|
||||
(-181.0, 179.0), (360.0, 0.0), (-360.0, 0.0),
|
||||
])
|
||||
def test_wrap_deg_180(deg, expect):
|
||||
assert wrap_deg_180(deg) == pytest.approx(expect)
|
||||
|
||||
|
||||
# --- 转场：朝起飞点飞 -------------------------------------------------------
|
||||
def test_far_away_aligned_commands_full_speed():
|
||||
"""远在起飞点北侧、机头朝南（对准回家方向）→ 满速前进、几乎不转。"""
|
||||
cfg = ReturnConfig(speed_mps=2.0)
|
||||
c = _ctrl(cfg)
|
||||
# 在起飞点正北 50m，机头朝南（yaw=180°）→ 应朝南飞（北向速度为负）
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 50.0, 0.0)
|
||||
out = c.update(lat, lon, math.radians(180.0), 5.0)
|
||||
assert out.arrived is False
|
||||
assert out.distance_m == pytest.approx(50.0, rel=5e-2)
|
||||
assert out.vx_mps == pytest.approx(2.0, rel=1e-2) # 对准 → 全速
|
||||
assert abs(out.yaw_rate_dps) < 1e-6
|
||||
|
||||
|
||||
def test_misaligned_gates_forward_speed():
|
||||
"""机头与回家方向垂直（90°）→ 前向速度被 cos(err) 门控为 0，只转向。"""
|
||||
c = _ctrl(ReturnConfig(speed_mps=2.0, yaw_rate_limit_dps=20.0))
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 0.0, 50.0) # 起飞点正东 50m
|
||||
out = c.update(lat, lon, math.radians(0.0), 5.0) # 机头朝北
|
||||
assert out.vx_mps == pytest.approx(0.0, abs=1e-3) # cos(90°)=0（含地理近似误差）
|
||||
assert out.bearing_err_deg == pytest.approx(-90.0, abs=1e-2) # 回家方向在西
|
||||
assert out.yaw_rate_dps < 0.0 # 往左转（目标在西侧）
|
||||
|
||||
|
||||
def test_yaw_rate_limited():
|
||||
c = _ctrl(ReturnConfig(yaw_kp=5.0, yaw_rate_limit_dps=20.0))
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 0.0, 50.0)
|
||||
out = c.update(lat, lon, math.radians(0.0), 5.0)
|
||||
assert out.yaw_rate_dps == pytest.approx(-20.0) # 被限幅（左转）
|
||||
|
||||
|
||||
def test_arrival_stops_forward_motion():
|
||||
"""进入到达半径 → arrived，且不再有前向速度。"""
|
||||
c = _ctrl(ReturnConfig(speed_mps=2.0, arrival_radius_m=3.0))
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 1.0, 0.0) # 距起飞点 1m
|
||||
out = c.update(lat, lon, math.radians(180.0), 5.0)
|
||||
assert out.arrived is True
|
||||
assert out.vx_mps == 0.0
|
||||
|
||||
|
||||
def test_distance_measured_to_home():
|
||||
c = _ctrl()
|
||||
lat, lon = _offset(HOME_LAT, HOME_LON, 30.0, 40.0) # 3-4-5 三角形 → 50m
|
||||
out = c.update(lat, lon, 0.0, 5.0)
|
||||
assert out.distance_m == pytest.approx(50.0, rel=5e-2)
|
||||
|
||||
|
||||
# --- 转场高度保持（NED 下正）------------------------------------------------
|
||||
def test_alt_hold_climbs_when_below_transit():
|
||||
c = _ctrl(transit_alt_m=5.0)
|
||||
vz = c.alt_hold_vz(3.0) # 低于转场高度
|
||||
assert vz < 0.0 # NED 下正 → 负 = 爬升
|
||||
|
||||
|
||||
def test_alt_hold_descends_when_above_transit():
|
||||
c = _ctrl(transit_alt_m=5.0)
|
||||
vz = c.alt_hold_vz(7.0)
|
||||
assert vz > 0.0 # 正 = 下降
|
||||
|
||||
|
||||
def test_alt_hold_zero_at_target():
|
||||
assert _ctrl(transit_alt_m=5.0).alt_hold_vz(5.0) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_alt_hold_rate_limited():
|
||||
cfg = ReturnConfig(alt_kp=10.0, alt_limit_mps=0.5)
|
||||
c = _ctrl(cfg, transit_alt_m=5.0)
|
||||
assert c.alt_hold_vz(0.0) == pytest.approx(-0.5) # 限幅
|
||||
assert c.alt_hold_vz(100.0) == pytest.approx(0.5)
|
||||
|
||||
|
||||
# --- 恢复初始航向 -----------------------------------------------------------
|
||||
def test_yaw_align_rate_and_direction():
|
||||
c = _ctrl(ReturnConfig(align_kp=1.0, align_rate_limit_dps=15.0),
|
||||
initial_yaw_deg=0.0)
|
||||
err, rate = c.yaw_align(math.radians(10.0)) # 当前偏右 10°
|
||||
assert err == pytest.approx(-10.0) # 需要往左转
|
||||
assert rate == pytest.approx(-10.0) # 负 = 左转
|
||||
|
||||
|
||||
def test_yaw_align_limited():
|
||||
c = _ctrl(ReturnConfig(align_kp=10.0, align_rate_limit_dps=15.0),
|
||||
initial_yaw_deg=0.0)
|
||||
_, rate = c.yaw_align(math.radians(90.0))
|
||||
assert rate == pytest.approx(-15.0)
|
||||
|
||||
|
||||
def test_yaw_aligned_within_tolerance():
|
||||
c = _ctrl(ReturnConfig(yaw_align_tolerance_deg=5.0), initial_yaw_deg=30.0)
|
||||
assert c.yaw_aligned(math.radians(33.0)) is True # 差 3° < 5°
|
||||
assert c.yaw_aligned(math.radians(40.0)) is False # 差 10°
|
||||
|
||||
|
||||
def test_yaw_align_wraps_around_north():
|
||||
"""初始航向 179°、当前 -179° → 误差 2°（不是 358°）。"""
|
||||
c = _ctrl(initial_yaw_deg=179.0)
|
||||
err, _ = c.yaw_align(math.radians(-179.0))
|
||||
assert abs(err) == pytest.approx(2.0, abs=1e-6)
|
||||
|
||||
|
||||
# --- 配置默认值 -------------------------------------------------------------
|
||||
def test_default_config_is_non_climbing_and_enabled():
|
||||
cfg = ReturnConfig()
|
||||
assert cfg.enabled is True
|
||||
assert cfg.alt_limit_mps > 0.0
|
||||
assert cfg.speed_mps > 0.0
|
||||
@@ -25,10 +25,17 @@ class FakePx4:
|
||||
self.telemetry_started = 0
|
||||
self.offboard_started = False
|
||||
self.brake_calls = []
|
||||
self.yaw_calls = [] # SEARCH 扫视下发记录
|
||||
self.velocity_calls = [] # send_velocity_heading 记录（返航转场用）
|
||||
self.fc_msgs = []
|
||||
|
||||
def own_state(self):
|
||||
return replace(self._state)
|
||||
|
||||
def take_fc_messages(self):
|
||||
msgs, self.fc_msgs = self.fc_msgs, []
|
||||
return msgs
|
||||
|
||||
async def start_telemetry(self):
|
||||
self.telemetry_started += 1
|
||||
|
||||
@@ -43,19 +50,26 @@ class FakePx4:
|
||||
pass
|
||||
|
||||
async def send_velocity_heading(self, out, t=None):
|
||||
self.velocity_calls.append(out)
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, t=None):
|
||||
self.brake_calls.append(t)
|
||||
return True
|
||||
|
||||
async def send_yaw_rate(self, yaw_rate_dps):
|
||||
self.yaw_calls.append(yaw_rate_dps)
|
||||
return True
|
||||
|
||||
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.started = False
|
||||
self.stopped = False
|
||||
self.landing_stopped = False # landing_stop()：只退 offboard、保留遥测
|
||||
self.law = None # set_heading_servo_enabled 最后值
|
||||
self.steps = 0
|
||||
self.search_steps = [] # SEARCH 态收到的扫视角速度
|
||||
self._locked = False
|
||||
self._telemetry = None
|
||||
|
||||
@@ -66,6 +80,9 @@ class FakeBridge:
|
||||
async def stop(self):
|
||||
self.stopped = True
|
||||
|
||||
async def landing_stop(self):
|
||||
self.landing_stopped = True
|
||||
|
||||
def set_heading_servo_enabled(self, enabled):
|
||||
self.law = enabled
|
||||
|
||||
@@ -75,6 +92,9 @@ class FakeBridge:
|
||||
async def step(self, now_ms):
|
||||
self.steps += 1
|
||||
|
||||
async def search_step(self, now_ms, yaw_rate_dps):
|
||||
self.search_steps.append(yaw_rate_dps)
|
||||
|
||||
@property
|
||||
def latest_telemetry(self):
|
||||
return self._telemetry
|
||||
@@ -83,10 +103,16 @@ class FakeBridge:
|
||||
class FakeGround:
|
||||
def __init__(self, target=None):
|
||||
self._target = target
|
||||
self.commands = [] # 地面站命令队列（供 drain_commands 取走）
|
||||
|
||||
def latest(self):
|
||||
return self._target or GroundTarget()
|
||||
|
||||
def drain_commands(self):
|
||||
cmds = list(self.commands)
|
||||
self.commands.clear()
|
||||
return cmds
|
||||
|
||||
|
||||
class FakeDrone:
|
||||
def __init__(self):
|
||||
@@ -96,6 +122,25 @@ class FakeDrone:
|
||||
self.rtl = False
|
||||
self.landed = False
|
||||
self.disarmed = False
|
||||
self.telemetry = FakeDrone._Telemetry()
|
||||
|
||||
class _Telemetry:
|
||||
"""模仿 MAVSDK `in_air()`：**异步生成器**，finalize 时取消底层流。
|
||||
|
||||
真实实现在 finally 里 `in_air_stream.cancel()`；「取一帧就 break」只是
|
||||
把生成器挂起，复用同一生成器即可避免反复订阅。
|
||||
"""
|
||||
|
||||
def __init__(self, frames=(False,)):
|
||||
self._frames = frames
|
||||
self.cancelled = False
|
||||
|
||||
async def in_air(self):
|
||||
try:
|
||||
for value in self._frames:
|
||||
yield value
|
||||
finally:
|
||||
self.cancelled = True
|
||||
|
||||
class _Action:
|
||||
def __init__(self, d):
|
||||
@@ -132,7 +177,8 @@ def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
|
||||
|
||||
|
||||
def _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||||
sm_cfg=None, vision_fn=None, pod_control=None):
|
||||
sm_cfg=None, pod_control=None,
|
||||
land_instead_of_rtl=False, now_ms_fn=None):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = 5000
|
||||
@@ -143,9 +189,9 @@ def _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||||
TaskStateMachine(sm_cfg or TaskStateConfig()),
|
||||
ground or FakeGround(),
|
||||
drone or FakeDrone(),
|
||||
vision_fn=vision_fn,
|
||||
now_ms_fn=lambda: 1000,
|
||||
now_ms_fn=now_ms_fn or (lambda: 1000),
|
||||
pod_control=pod_control,
|
||||
land_instead_of_rtl=land_instead_of_rtl,
|
||||
)
|
||||
|
||||
|
||||
@@ -180,6 +226,15 @@ def test_collect_takeoff_reached():
|
||||
assert r._collect_inputs(1000).takeoff_reached is True
|
||||
|
||||
|
||||
def test_collect_takeoff_reached_tolerance():
|
||||
"""容差内（目标 5.0m，实际 4.6m ≥ 5.0-0.5）判到达；容差外（4.4m）判未到达。"""
|
||||
sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
|
||||
px4_low = FakePx4(_state(1000, rel_alt=4.6))
|
||||
assert _make_runner(px4=px4_low, sm_cfg=sm_cfg)._collect_inputs(1000).takeoff_reached is True
|
||||
px4_below = FakePx4(_state(1000, rel_alt=4.4))
|
||||
assert _make_runner(px4=px4_below, sm_cfg=sm_cfg)._collect_inputs(1000).takeoff_reached is False
|
||||
|
||||
|
||||
def test_collect_locked_from_bridge():
|
||||
bridge = FakeBridge()
|
||||
bridge._locked = True
|
||||
@@ -188,14 +243,6 @@ def test_collect_locked_from_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()
|
||||
@@ -257,6 +304,15 @@ def test_on_enter_rtl_stops_bridge_and_returns():
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
def test_land_instead_of_rtl_lands():
|
||||
"""--land 模式：收尾调 land 而不是 return_to_launch（真机就地回收）。"""
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(drone=drone, land_instead_of_rtl=True)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
assert drone.landed is True
|
||||
assert drone.rtl is False
|
||||
|
||||
|
||||
# --- 持续动作 ---------------------------------------------------------------
|
||||
def test_tick_goto_steps_bridge():
|
||||
bridge = FakeBridge()
|
||||
@@ -281,12 +337,24 @@ class FakePodControl:
|
||||
def __init__(self):
|
||||
self.armed = False
|
||||
self.stopped = False
|
||||
self.calls = []
|
||||
|
||||
def arm_detection(self):
|
||||
self.armed = True
|
||||
self.calls.append("arm_detection")
|
||||
|
||||
def stop_tracking(self):
|
||||
self.stopped = True
|
||||
self.calls.append("stop_tracking")
|
||||
|
||||
def stop_track(self):
|
||||
self.calls.append("stop_track")
|
||||
|
||||
def auto_lock(self):
|
||||
self.calls.append("auto_lock")
|
||||
|
||||
def center(self):
|
||||
self.calls.append("center")
|
||||
|
||||
|
||||
def test_on_enter_goto_arms_detection():
|
||||
@@ -309,3 +377,596 @@ def test_no_pod_control_is_safe():
|
||||
r = _make_runner(pod_control=None)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
|
||||
|
||||
# --- 地面站命令分派 ---------------------------------------------------------
|
||||
def test_ground_command_rtl_sets_requested():
|
||||
ground = FakeGround()
|
||||
ground.commands.append("rtl")
|
||||
r = _make_runner(ground=ground)
|
||||
|
||||
r._apply_ground_commands()
|
||||
assert r._collect_inputs(1000).rtl_requested is True
|
||||
|
||||
|
||||
def test_ground_command_emergency_sets_flag():
|
||||
ground = FakeGround()
|
||||
ground.commands.append("emergency")
|
||||
r = _make_runner(ground=ground)
|
||||
|
||||
r._apply_ground_commands()
|
||||
assert r._collect_inputs(1000).emergency is True
|
||||
|
||||
|
||||
def test_ground_command_pod_dispatch():
|
||||
pod_control = FakePodControl()
|
||||
ground = FakeGround()
|
||||
ground.commands.extend(["pod_detect_on", "pod_auto_lock", "pod_stop_track",
|
||||
"pod_detect_off", "pod_center"])
|
||||
r = _make_runner(ground=ground, pod_control=pod_control)
|
||||
|
||||
r._apply_ground_commands()
|
||||
assert pod_control.calls == ["arm_detection", "auto_lock", "stop_track",
|
||||
"stop_tracking", "center"]
|
||||
|
||||
|
||||
def test_ground_command_pod_without_control_is_ignored():
|
||||
"""SITL（无 pod_control）→ 吊舱命令记警告忽略，不抛异常。"""
|
||||
ground = FakeGround()
|
||||
ground.commands.append("pod_center")
|
||||
r = _make_runner(ground=ground, pod_control=None)
|
||||
|
||||
r._apply_ground_commands() # 不抛异常即通过
|
||||
|
||||
|
||||
def test_ground_command_unknown_is_ignored():
|
||||
ground = FakeGround()
|
||||
ground.commands.append("unknown_cmd")
|
||||
r = _make_runner(ground=ground)
|
||||
|
||||
r._apply_ground_commands() # 不抛异常即通过
|
||||
|
||||
|
||||
def test_run_drains_ground_commands():
|
||||
"""run() 主循环每拍排空命令队列：RTL 命令 → 状态机转 RTL → 调 return_to_launch。"""
|
||||
ground = FakeGround()
|
||||
ground.commands.append("rtl")
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(ground=ground, drone=drone)
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(r.run(stop))
|
||||
try:
|
||||
for _ in range(100):
|
||||
if drone.rtl:
|
||||
break
|
||||
await asyncio.sleep(0.01)
|
||||
finally:
|
||||
stop.set()
|
||||
await task
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert r.state is TaskState.RTL
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
# --- 飞控状态文本落日志 -----------------------------------------------------
|
||||
class FakeLogger:
|
||||
def __init__(self):
|
||||
self.events = []
|
||||
|
||||
def open(self):
|
||||
pass
|
||||
|
||||
def close(self):
|
||||
pass
|
||||
|
||||
def log_event(self, now_ms, event_type, **fields):
|
||||
self.events.append((event_type, fields))
|
||||
|
||||
def log_telemetry(self, now_ms, rec):
|
||||
pass
|
||||
|
||||
|
||||
def test_fc_messages_logged_as_events():
|
||||
px4 = FakePx4(_state(1000))
|
||||
px4.fc_msgs = [("WARNING", "Low battery"), ("INFO", "Takeoff detected")]
|
||||
logger = FakeLogger()
|
||||
r = _make_runner(px4=px4)
|
||||
r._logger = logger
|
||||
|
||||
r._log_fc_messages()
|
||||
assert [e[0] for e in logger.events] == ["fc_status", "fc_status"]
|
||||
assert logger.events[0][1] == {"severity": "WARNING", "text": "Low battery"}
|
||||
assert px4.fc_msgs == [] # 已排空，不重复记录
|
||||
|
||||
|
||||
# --- SEARCH 搜索扫视（2026-10-15 新增）--------------------------------------
|
||||
def test_on_enter_search_disables_servo_and_resets_sweep():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
r._sweep_dir = 9 # 脏值，应被重置
|
||||
asyncio.run(r._on_enter(TaskState.SEARCH, 1000))
|
||||
assert bridge.law is False # 搜索不用控制律（不由 L1/L2 出指令）
|
||||
assert r._sweep_dir == 0 # 每次进搜索从右向起扫
|
||||
assert r._sweep_since_ms == 1000
|
||||
|
||||
|
||||
def test_search_yaw_rate_sweeps_both_directions():
|
||||
"""扫视按 sweep_reverse_s 左右交替，幅值为配置角速度。"""
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=15.0, sweep_reverse_s=4.0)
|
||||
r = _make_runner(sm_cfg=sm_cfg)
|
||||
assert r._search_yaw_rate(1000) == pytest.approx(15.0) # 先向右
|
||||
assert r._search_yaw_rate(4999) == pytest.approx(15.0) # 未到换向点
|
||||
assert r._search_yaw_rate(5000) == pytest.approx(-15.0) # 换向左
|
||||
assert r._search_yaw_rate(9000) == pytest.approx(15.0) # 再换回右
|
||||
|
||||
|
||||
def test_search_yaw_rate_zero_disables_sweep():
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=0.0, sweep_reverse_s=4.0)
|
||||
r = _make_runner(sm_cfg=sm_cfg)
|
||||
assert r._search_yaw_rate(1000) == 0.0
|
||||
|
||||
|
||||
def test_search_yaw_rate_continuous_when_reverse_disabled():
|
||||
"""sweep_reverse_s=0 → 单向持续扫视（不换向）。"""
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=10.0, sweep_reverse_s=0.0)
|
||||
r = _make_runner(sm_cfg=sm_cfg)
|
||||
assert r._search_yaw_rate(1000) == pytest.approx(10.0)
|
||||
assert r._search_yaw_rate(600_000) == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_tick_search_sends_sweep_step():
|
||||
bridge = FakeBridge()
|
||||
sm_cfg = TaskStateConfig(sweep_rate_dps=12.0, sweep_reverse_s=4.0)
|
||||
r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)
|
||||
r._sm.force_state(TaskState.SEARCH, 1000)
|
||||
asyncio.run(r._tick(1000))
|
||||
assert bridge.search_steps == [pytest.approx(12.0)]
|
||||
assert bridge.steps == 0 # 搜索不走控制律 step
|
||||
|
||||
|
||||
def test_search_step_polls_pod_so_lock_can_be_fresh():
|
||||
"""SEARCH 必须 poll 吊舱：否则遥测超龄，搜到目标也切不进 TRACK。"""
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.protocol import Telemetry
|
||||
|
||||
class FakePod:
|
||||
def __init__(self):
|
||||
self.polls = 0
|
||||
|
||||
def poll(self):
|
||||
self.polls += 1
|
||||
return [Telemetry(status1=0b100, off_target_x_deg=1.0,
|
||||
off_target_y_deg=0.0)]
|
||||
|
||||
def send(self, data):
|
||||
pass
|
||||
|
||||
def close(self):
|
||||
pass
|
||||
|
||||
pod = FakePod()
|
||||
px4 = FakePx4(_state(1000))
|
||||
cfg = BridgeConfig()
|
||||
cfg.link.require_laser_valid = False # 本吊舱无激光（与部署配置一致）
|
||||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000,
|
||||
ground_target_fn=lambda: None)
|
||||
asyncio.run(bridge.search_step(1000, -15.0))
|
||||
|
||||
assert pod.polls == 1
|
||||
assert bridge.pod_locked(1000) is True # 本拍锁定位已刷新
|
||||
assert px4.yaw_calls == [pytest.approx(-15.0)] # 扫视角速度已下发
|
||||
|
||||
|
||||
# --- 进入动作只执行一次（防重入导致的自激）----------------------------------
|
||||
class _CountingPodControl:
|
||||
def __init__(self):
|
||||
self.arm_calls = 0
|
||||
self.stop_calls = 0
|
||||
|
||||
def arm_detection(self):
|
||||
self.arm_calls += 1
|
||||
|
||||
def stop_tracking(self):
|
||||
self.stop_calls += 1
|
||||
|
||||
def stop_track(self):
|
||||
pass
|
||||
|
||||
def auto_lock(self):
|
||||
pass
|
||||
|
||||
def center(self):
|
||||
pass
|
||||
|
||||
|
||||
def test_pod_detection_armed_only_once_across_reentry():
|
||||
"""丢目标回 SEARCH 再进 TRACK 时，不得重发 0x07/0x0F（会打断正在进行的跟踪）。"""
|
||||
pod_control = _CountingPodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.SEARCH, 2000))
|
||||
asyncio.run(r._on_enter(TaskState.TRACK, 3000))
|
||||
asyncio.run(r._on_enter(TaskState.SEARCH, 4000)) # 丢目标回搜索
|
||||
asyncio.run(r._on_enter(TaskState.TRACK, 5000)) # 再次锁定
|
||||
assert pod_control.arm_calls == 1
|
||||
|
||||
|
||||
def test_rtl_stops_pod_tracking_even_after_reentry():
|
||||
pod_control = _CountingPodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert pod_control.stop_calls == 1
|
||||
|
||||
|
||||
# --- 主循环异常兜底：必须返航，不能静默停摆 ---------------------------------
|
||||
def test_iteration_exception_triggers_rtl():
|
||||
"""主循环单周期异常 → 必须转紧急返航，不能只停机静默停摆。
|
||||
|
||||
异常注入在**每拍都跑**的输入采集路径（own_state），与当前状态无关。
|
||||
"""
|
||||
px4 = FakePx4(_state(1000))
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(px4=px4, drone=drone)
|
||||
calls = {"n": 0}
|
||||
|
||||
def boom():
|
||||
calls["n"] += 1
|
||||
raise RuntimeError("模拟瞬态异常（遥测/串口抖动）")
|
||||
|
||||
px4.own_state = boom
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(r.run(stop))
|
||||
for _ in range(300):
|
||||
if drone.rtl:
|
||||
break
|
||||
await asyncio.sleep(0.005)
|
||||
stop.set()
|
||||
task.cancel()
|
||||
try:
|
||||
await task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert calls["n"] >= 1
|
||||
assert drone.rtl is True # 异常兜底确实触发了返航
|
||||
assert r.state is TaskState.RTL
|
||||
|
||||
|
||||
def test_iteration_exception_records_event():
|
||||
"""异常兜底要在任务日志留痕，便于事后定位。"""
|
||||
px4 = FakePx4(_state(1000))
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(px4=px4, drone=drone)
|
||||
logger = FakeLogger()
|
||||
r._logger = logger
|
||||
|
||||
def boom():
|
||||
raise RuntimeError("串口拔线")
|
||||
|
||||
px4.own_state = boom
|
||||
|
||||
async def scenario():
|
||||
stop = asyncio.Event()
|
||||
task = asyncio.ensure_future(r.run(stop))
|
||||
for _ in range(300):
|
||||
if drone.rtl:
|
||||
break
|
||||
await asyncio.sleep(0.005)
|
||||
stop.set()
|
||||
task.cancel()
|
||||
try:
|
||||
await task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
|
||||
asyncio.run(scenario())
|
||||
types = [e[0] for e in logger.events]
|
||||
assert "iteration_error" in types
|
||||
assert "rtl" in types # 兜底返航事件也落盘
|
||||
|
||||
|
||||
def test_safe_abort_is_noop_when_already_rtl():
|
||||
"""已在 RTL 时兜底不重复触发返航（避免重复 RTL 指令）。"""
|
||||
bridge = FakeBridge()
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(bridge=bridge, drone=drone)
|
||||
r._sm.force_state(TaskState.RTL, 0)
|
||||
asyncio.run(r._safe_abort())
|
||||
assert drone.rtl is False
|
||||
assert bridge.stopped is False
|
||||
|
||||
|
||||
# --- in_air 遥测流复用（降低服务端订阅churn）--------------------------------
|
||||
def test_in_air_stream_reused_across_ticks():
|
||||
"""in_air 流只订阅一次：每拍新建会在服务端反复订阅/退订同一条流。
|
||||
|
||||
注意必须在**同一个事件循环**内连续调用（与生产一致：主循环是单 loop）；
|
||||
跨 loop 复用 async generator 会被 asyncio 拒绝，实现里也就丢弃重建。
|
||||
"""
|
||||
drone = FakeDrone()
|
||||
calls = {"n": 0}
|
||||
|
||||
class CountingTelemetry:
|
||||
def in_air(self):
|
||||
calls["n"] += 1
|
||||
return FakeDrone._Telemetry().in_air()
|
||||
|
||||
drone.telemetry = CountingTelemetry()
|
||||
r = _make_runner(drone=drone)
|
||||
|
||||
async def scenario():
|
||||
for _ in range(5):
|
||||
await r._check_landed()
|
||||
|
||||
asyncio.run(scenario())
|
||||
assert calls["n"] == 1 # 只订阅一次
|
||||
assert r.rtl_done is True # in_air=False → 判落地并 disarm
|
||||
|
||||
|
||||
def test_in_air_stream_rebuilt_when_broken():
|
||||
"""流建立/消费失败时丢弃缓存，下拍重建（不永久卡在坏流上）。"""
|
||||
drone = FakeDrone()
|
||||
calls = {"n": 0}
|
||||
|
||||
def broken():
|
||||
calls["n"] += 1
|
||||
raise RuntimeError("stream broken")
|
||||
|
||||
class FlakyTelemetry:
|
||||
def in_air(self):
|
||||
return broken()
|
||||
|
||||
drone.telemetry = FlakyTelemetry()
|
||||
r = _make_runner(drone=drone)
|
||||
|
||||
async def scenario():
|
||||
await r._check_landed()
|
||||
assert calls["n"] == 1
|
||||
assert r._in_air_stream is None # 已丢弃
|
||||
await r._check_landed()
|
||||
assert calls["n"] == 2 # 重建后再试
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
# --- RTL 落地等待上限（真机在环发现：判不出落地会无限等）--------------------
|
||||
def test_rtl_timeout_ends_task_when_landing_unobservable():
|
||||
"""in_air 始终不报落地 → 超过 rtl_timeout_s 后结束任务，不无限等待。"""
|
||||
now = {"ms": 1_000_000}
|
||||
drone = FakeDrone()
|
||||
|
||||
class AlwaysAirborne:
|
||||
async def in_air(self):
|
||||
while True:
|
||||
yield True
|
||||
|
||||
drone.telemetry = AlwaysAirborne()
|
||||
sm_cfg = TaskStateConfig(rtl_timeout_s=5.0)
|
||||
r = _make_runner(drone=drone, sm_cfg=sm_cfg,
|
||||
now_ms_fn=lambda: now["ms"])
|
||||
r._rtl_entered_ms = now["ms"]
|
||||
r._rtl_phase = "land"
|
||||
|
||||
async def scenario():
|
||||
now["ms"] += 4_000 # 未到上限：不应结束
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is False
|
||||
now["ms"] += 2_000 # 超过上限：结束任务
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is True
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
def test_rtl_timeout_not_triggered_after_landing():
|
||||
"""已判落地后不再触发超时逻辑（rtl_done 保持）。"""
|
||||
now = {"ms": 1_000_000}
|
||||
drone = FakeDrone() # 默认 in_air 首帧 False
|
||||
sm_cfg = TaskStateConfig(rtl_timeout_s=1.0)
|
||||
r = _make_runner(drone=drone, sm_cfg=sm_cfg,
|
||||
now_ms_fn=lambda: now["ms"])
|
||||
r._rtl_entered_ms = now["ms"]
|
||||
r._rtl_phase = "land"
|
||||
|
||||
async def scenario():
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is True
|
||||
now["ms"] += 60_000 # 大幅超过上限
|
||||
await r._tick_rtl(now["ms"])
|
||||
assert r.rtl_done is True
|
||||
|
||||
asyncio.run(scenario())
|
||||
|
||||
|
||||
# --- 自定义返航：回起飞点 → 恢复初始航向 → 降落 -----------------------------
|
||||
def _armed_runner(px4=None, drone=None, return_cfg=None):
|
||||
"""走完 解锁→GOTO（记录起飞点 + 进入 offboard）的 runner。"""
|
||||
px4 = px4 or FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r = _make_runner(px4=px4, drone=drone or FakeDrone())
|
||||
if return_cfg is not None:
|
||||
r._return_cfg = return_cfg
|
||||
asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
return r, px4
|
||||
|
||||
|
||||
def test_custom_return_selected_after_takeoff():
|
||||
"""已起飞 + 有起飞点 → 进入 RTL 走自定义返航，不调 PX4 RTL。"""
|
||||
drone = FakeDrone()
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert r._rtl_phase == "return"
|
||||
assert drone.rtl is False # 不再交给 PX4 原生 RTL
|
||||
assert r._bridge_started is True
|
||||
|
||||
|
||||
def test_custom_return_records_home_and_initial_yaw():
|
||||
r, px4 = _armed_runner()
|
||||
assert r._home_lat == pytest.approx(0.0)
|
||||
assert r._home_lon == pytest.approx(0.0)
|
||||
assert r._initial_yaw_rad == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_return_step_commands_velocity_toward_home():
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r, _ = _armed_runner(px4=px4)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
|
||||
# 飞到起飞点正北 ~111m，机头朝南（对准回家方向）
|
||||
px4._state = _state(3000, lat=0.001, lon=0.0, rel_alt=5.0)
|
||||
px4._state.yaw_rad = 3.141592653589793
|
||||
px4.velocity_calls.clear()
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
|
||||
assert r._rtl_phase == "return"
|
||||
assert len(px4.velocity_calls) == 1
|
||||
out = px4.velocity_calls[0]
|
||||
assert out.vx_body_mps > 0.0 # 朝起飞点前进
|
||||
assert out.vz_mps == pytest.approx(0.0) # 转场高度 5m = 当前高度
|
||||
|
||||
|
||||
def test_return_arrival_switches_to_yaw_phase():
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r, _ = _armed_runner(px4=px4)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
|
||||
px4._state = _state(3000, lat=0.000005, lon=0.0, rel_alt=5.0) # ~0.5m
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
assert r._rtl_phase == "yaw"
|
||||
|
||||
|
||||
def test_yaw_phase_restores_heading_then_lands():
|
||||
drone = FakeDrone()
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r, _ = _armed_runner(px4=px4, drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
r._rtl_phase = "yaw"
|
||||
r._yaw_started_ms = 2000
|
||||
|
||||
px4._state = _state(3000, lat=0.0, lon=0.0, rel_alt=5.0) # 航向已回 0
|
||||
px4.velocity_calls.clear()
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
|
||||
assert len(px4.velocity_calls) == 1 # 先发了一拍"原地保持"指令
|
||||
assert r._rtl_phase == "land"
|
||||
assert drone.landed is True # 航向对齐后降落
|
||||
assert r._land_cmd_sent is True
|
||||
|
||||
|
||||
def test_landing_keeps_telemetry_running():
|
||||
"""降落阶段只退 offboard、保留遥测（否则日志冻结在降落前高度）。"""
|
||||
drone = FakeDrone()
|
||||
bridge = FakeBridge()
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
r._bridge = bridge
|
||||
asyncio.run(r._start_land())
|
||||
assert bridge.landing_stopped is True
|
||||
assert bridge.stopped is False # 尚未全停：遥测/串口仍在
|
||||
assert drone.landed is True
|
||||
|
||||
|
||||
def test_land_phase_completes_on_landed():
|
||||
drone = FakeDrone() # in_air -> False
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
r._rtl_phase = "land"
|
||||
asyncio.run(r._tick_rtl(3000))
|
||||
assert r.rtl_done is True
|
||||
assert drone.disarmed is True
|
||||
|
||||
|
||||
def test_return_timeout_falls_back_to_px4_rtl():
|
||||
drone = FakeDrone()
|
||||
r, px4 = _armed_runner(drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
|
||||
px4._state = _state(3000, lat=0.001, lon=0.0, rel_alt=5.0)
|
||||
# 超过转场超时
|
||||
late = 2000 + int(r._return_cfg.timeout_s * 1000) + 1000
|
||||
asyncio.run(r._tick_rtl(late))
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True # 退回 PX4 原生 RTL
|
||||
|
||||
|
||||
def test_custom_return_disabled_uses_px4_rtl():
|
||||
from task_runner.return_home import ReturnConfig
|
||||
|
||||
drone = FakeDrone()
|
||||
r, _ = _armed_runner(drone=drone,
|
||||
return_cfg=ReturnConfig(enabled=False))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
def test_rtl_before_takeoff_uses_px4_rtl():
|
||||
"""未起飞（还在待命/地面）时不能自主返航 → 退回 PX4 RTL。"""
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(drone=drone) # 未记录起飞点、未进 offboard
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 2000))
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
def test_safe_abort_prefers_px4_fallback():
|
||||
"""异常兜底不再信任自身状态：强制走 PX4 RTL。"""
|
||||
drone = FakeDrone()
|
||||
r, _ = _armed_runner(drone=drone)
|
||||
asyncio.run(r._safe_abort())
|
||||
assert r._rtl_phase == "fallback"
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
# --- 起飞点记录时机（SITL 实测：解锁时 gps_info 可能还没到）------------------
|
||||
def test_home_recorded_when_position_becomes_valid_during_takeoff():
|
||||
"""解锁瞬间定位无效 → 起飞阶段每拍补记，不丢起飞点。"""
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
px4._state.position_valid = False
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm.force_state(TaskState.ARM_TAKEOFF, 0)
|
||||
|
||||
r._collect_inputs(1000)
|
||||
assert r._home_lat is None # 定位无效 → 暂不记录
|
||||
|
||||
px4._state = _state(1000, lat=0.0012, lon=0.0034)
|
||||
r._collect_inputs(1100)
|
||||
assert r._home_lat == pytest.approx(0.0012) # 补记成功
|
||||
assert r._home_lon == pytest.approx(0.0034)
|
||||
|
||||
|
||||
def test_home_not_recorded_once_airborne():
|
||||
"""进入 GOTO（已在空中）后不再把当前位置当作起飞点。"""
|
||||
px4 = FakePx4(_state(1000, lat=0.005, lon=0.0))
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm.force_state(TaskState.GOTO, 0)
|
||||
r._collect_inputs(1000)
|
||||
assert r._home_lat is None
|
||||
|
||||
|
||||
def test_home_recorded_only_once():
|
||||
px4 = FakePx4(_state(1000, lat=0.0, lon=0.0))
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm.force_state(TaskState.ARM_TAKEOFF, 0)
|
||||
r._collect_inputs(1000)
|
||||
first = (r._home_lat, r._home_lon, r._initial_yaw_rad)
|
||||
|
||||
px4._state = _state(1000, lat=0.009, lon=0.009) # 位置已变（飞机在爬升/移动）
|
||||
r._collect_inputs(1100)
|
||||
assert (r._home_lat, r._home_lon, r._initial_yaw_rad) == first
|
||||
|
||||
|
||||
def test_record_home_returns_false_without_fix():
|
||||
px4 = FakePx4(_state(1000))
|
||||
px4._state.position_valid = False
|
||||
r = _make_runner(px4=px4)
|
||||
assert r._record_home() is False
|
||||
assert r._home_lat is None
|
||||
|
||||
|
||||
|
||||
@@ -1,8 +1,12 @@
|
||||
"""state_machine_test.py —— 任务级状态机单元测试。
|
||||
|
||||
覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
|
||||
GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
|
||||
RTL 终态与 reset。
|
||||
覆盖（2026-10-15 按「到参考点 → 视觉搜索 → 缓慢靠近」重构后的契约）：
|
||||
* 完整状态链 WAIT→ARM_TAKEOFF→GOTO→SEARCH→TRACK→(超时)RTL
|
||||
* GOTO 只按「到参考点半径」退出（不看锁定）；SEARCH 才看锁定/视觉守卫
|
||||
* 搜索超时 → RTL；追踪（靠近）超时 → RTL
|
||||
* TRACK 丢目标 → 回 SEARCH（不回 GOTO）；坐标同时失效 → RTL
|
||||
* TRACK **不**因「目标居中」退出（居中即应继续靠近）
|
||||
* 紧急/返航全局终止、RTL 终态与 reset、force_state 兜底
|
||||
"""
|
||||
|
||||
import pytest
|
||||
@@ -19,6 +23,25 @@ def _sm(cfg=None) -> TaskStateMachine:
|
||||
return TaskStateMachine(cfg or TaskStateConfig())
|
||||
|
||||
|
||||
def _to_search(cfg=None, t0: int = 1000):
|
||||
"""推进到 SEARCH 态：WAIT→ARM_TAKEOFF→GOTO→SEARCH。返回 (sm, now_ms)。"""
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), t0)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), t0 + 1000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0), t0 + 2000)
|
||||
assert tr.to_state is TaskState.SEARCH
|
||||
return sm, t0 + 2000
|
||||
|
||||
|
||||
def _to_track(cfg=None, t0: int = 1000):
|
||||
"""推进到 TRACK 态：WAIT→…→SEARCH→TRACK。返回 (sm, now_ms)。"""
|
||||
sm, t = _to_search(cfg, t0)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), t + 1000)
|
||||
assert tr.to_state is TaskState.TRACK
|
||||
return sm, t + 1000
|
||||
|
||||
|
||||
# --- 初始态与 reset ---------------------------------------------------------
|
||||
def test_initial_state_is_wait():
|
||||
assert _sm().state is TaskState.WAIT
|
||||
@@ -32,6 +55,13 @@ def test_reset_returns_to_wait():
|
||||
assert sm.state is TaskState.WAIT
|
||||
|
||||
|
||||
def test_force_state_jumps_without_transition():
|
||||
"""force_state 供异常兜底直接落状态（不产生 TaskTransition）。"""
|
||||
sm = _sm()
|
||||
sm.force_state(TaskState.RTL, 5000)
|
||||
assert sm.state is TaskState.RTL
|
||||
|
||||
|
||||
# --- WAIT -------------------------------------------------------------------
|
||||
def test_wait_without_coords_stays():
|
||||
tr = _sm().update(_inp(), 1000)
|
||||
@@ -60,12 +90,11 @@ 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 -------------------------------------------------------------------
|
||||
# --- GOTO：只按「到参考点半径」退出（不看锁定）-------------------------------
|
||||
def test_goto_far_away_stays():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
@@ -76,45 +105,31 @@ def test_goto_far_away_stays():
|
||||
assert sm.state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_goto_within_radius_and_locked_goes_track():
|
||||
def test_goto_within_radius_goes_search_even_if_locked():
|
||||
"""到参考点即转搜索——参考点是搜索起点，锁定与否不影响该转移。"""
|
||||
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
|
||||
assert tr.occurred and tr.to_state is TaskState.SEARCH
|
||||
assert "参考点" in tr.reason
|
||||
|
||||
|
||||
def test_goto_within_radius_but_unlocked_stays():
|
||||
def test_goto_within_radius_unlocked_still_goes_search():
|
||||
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
|
||||
assert tr.occurred and tr.to_state is TaskState.SEARCH
|
||||
|
||||
|
||||
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)
|
||||
tr = sm.update(_inp(target_injected=False, distance_to_target_m=100.0), 3000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
@@ -123,87 +138,113 @@ def test_goto_timeout_goes_rtl():
|
||||
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)
|
||||
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)
|
||||
# --- SEARCH：搜索发现目标才进 TRACK -----------------------------------------
|
||||
def test_search_sees_target_goes_track():
|
||||
sm, t = _to_search()
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), t + 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.TRACK
|
||||
assert "发现目标" in tr.reason
|
||||
|
||||
|
||||
def test_search_without_target_stays():
|
||||
sm, t = _to_search()
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=False), t + 1000)
|
||||
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
|
||||
assert sm.state is TaskState.SEARCH
|
||||
|
||||
|
||||
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)
|
||||
def test_search_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(search_timeout_s=20.0)
|
||||
sm, t = _to_search(cfg)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0),
|
||||
t + 21_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
assert "搜索超时" in tr.reason
|
||||
|
||||
|
||||
def test_search_loses_coords_goes_rtl():
|
||||
sm, t = _to_search()
|
||||
tr = sm.update(_inp(target_injected=False), t + 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- TRACK：缓慢靠近，只受超时/返航/紧急控制 ---------------------------------
|
||||
def test_track_centered_does_not_exit():
|
||||
"""目标居中**不再**触发退出：居中正是应当继续缓慢靠近的时刻。"""
|
||||
sm, t = _to_track()
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
||||
target_centered=True, centered_duration_s=99.0), t + 1000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.TRACK
|
||||
|
||||
|
||||
def test_track_holds_while_locked():
|
||||
sm, t = _to_track()
|
||||
for dt in (1000, 2000, 3000):
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), t + dt)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.TRACK
|
||||
|
||||
|
||||
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)
|
||||
cfg = TaskStateConfig(track_timeout_s=20.0)
|
||||
sm, t = _to_track(cfg)
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), t + 21_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
assert "超时" in tr.reason
|
||||
|
||||
|
||||
def test_track_lost_with_coords_goes_back_to_search():
|
||||
"""丢目标回 SEARCH 重新扫视（不回 GOTO：飞机本就在参考点附近）。"""
|
||||
sm, t = _to_track()
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=False), t + 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.SEARCH
|
||||
assert "重新搜索" in tr.reason
|
||||
|
||||
|
||||
def test_track_lost_without_coords_goes_rtl():
|
||||
sm, t = _to_track()
|
||||
tr = sm.update(_inp(target_injected=False, target_locked=False), t + 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- APPROACH ---------------------------------------------------------------
|
||||
def test_reacquire_after_loss_returns_to_track():
|
||||
"""丢目标→SEARCH→重新锁定→TRACK 全链路可循环。"""
|
||||
sm, t = _to_track()
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=False), t + 1000)
|
||||
assert tr.to_state is TaskState.SEARCH
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), t + 2000)
|
||||
assert tr.to_state is TaskState.TRACK
|
||||
|
||||
|
||||
# --- APPROACH（保留态：真机可选收尾）----------------------------------------
|
||||
def test_approach_hover_done_goes_rtl():
|
||||
cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0)
|
||||
cfg = TaskStateConfig(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)
|
||||
sm.force_state(TaskState.APPROACH, 1000)
|
||||
tr = sm.update(_inp(), 1000 + 5000)
|
||||
assert not tr.occurred
|
||||
# 满 10s → RTL
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
|
||||
tr = sm.update(_inp(), 1000 + 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)
|
||||
def test_emergency_from_search_goes_rtl():
|
||||
sm, t = _to_search()
|
||||
tr = sm.update(_inp(emergency=True), t + 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_emergency_from_track_goes_rtl():
|
||||
sm, t = _to_track()
|
||||
tr = sm.update(_inp(target_locked=True, emergency=True), t + 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
@@ -218,7 +259,6 @@ 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
|
||||
@@ -231,3 +271,11 @@ def test_transition_dataclass_defaults():
|
||||
assert tr.to_state is TaskState.WAIT
|
||||
assert tr.occurred is False
|
||||
assert tr.reason == ""
|
||||
|
||||
|
||||
# --- 配置默认值（搜索/靠近超时口径）------------------------------------------
|
||||
def test_default_config_has_search_and_pursuit_timeouts():
|
||||
cfg = TaskStateConfig()
|
||||
assert cfg.search_timeout_s == pytest.approx(20.0)
|
||||
assert cfg.track_timeout_s == pytest.approx(20.0)
|
||||
assert cfg.sweep_rate_dps > 0.0
|
||||
|
||||
Reference in New Issue
Block a user