"""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 = [] 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 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): 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 async def start(self): self.started = True return True 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 def pod_locked(self, now_ms): return self._locked 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 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): self.armed = False self.took_off = False self.takeoff_alt = 0.0 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): 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, 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 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(), now_ms_fn=now_ms_fn or (lambda: 1000), pod_control=pod_control, land_instead_of_rtl=land_instead_of_rtl, ) # --- 输入采集 --------------------------------------------------------------- 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_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 r = _make_runner(bridge=bridge) assert r._collect_inputs(1000).target_locked is True # --- 居中累加 --------------------------------------------------------------- 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_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() 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 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(): 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)) # --- 地面站命令分派 --------------------------------------------------------- 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