songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航

Browse Source 
归档 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
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
Download Patch File Download Diff File Expand all files Collapse all files
utils/tests/task_runner/runner_test.py
+672 -11
View File
@@ -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