精简仓库结构并增强任务链路：归档双目可选后端、新增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。
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
@@ -6,6 +6,7 @@ start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢
 """

 import asyncio
+import math
 import time
 from dataclasses import replace

@@ -29,6 +30,7 @@ class FakePx4:
        self.offboard_start_ok = True
        self.velocity_calls = []
        self.brake_calls = []
+        self.yaw_calls = []

    def own_state(self):
        return replace(self._state)
@@ -54,6 +56,10 @@ class FakePx4:
        self.brake_calls.append(time_boot_ms)
        return True

+    async def send_yaw_rate(self, yaw_rate_dps):
+        self.yaw_calls.append(yaw_rate_dps)
+        return True
+

 class FakePod:
    def __init__(self):
@@ -159,6 +165,116 @@ def test_attitude_downlink_throttled():
    assert len(pod.sent) == 2


+def test_control_law_throttled_to_own_frequency():
+    """生效律按自身频率推进：L2 配置 10Hz，外部 20Hz 调用时控制律只跑 10 次/秒。
+
+    链路维持（表3 姿态下发）不受此节流影响——否则吊舱解算会断流。
+    """
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    cfg = BridgeConfig()
+    cfg.heading_servo.enabled = False          # L2 地理引导（10Hz）
+    cfg.guidance.frequency_hz = 10.0
+    cfg.link.attitude_downlink_hz = 20.0
+    cfg.ground_inject.enabled = True
+    ground = GroundTarget(latitude_deg=0.01, longitude_deg=0.0, time_ms=1000)
+    b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000,
+                  ground_target_fn=lambda: ground)
+
+    # 统计控制律实际推进次数（包装 update，不改变行为）
+    law_updates = {"n": 0}
+    real_update = b._guidance.update
+
+    def counting_update(inp, now):
+        law_updates["n"] += 1
+        return real_update(inp, now)
+
+    b._guidance.update = counting_update
+
+    ticks = 10                                 # 20Hz 调用 10 拍 = 500ms
+
+    async def scenario():
+        for i in range(ticks):
+            await b.step(1000 + i * 50)
+    asyncio.run(scenario())
+
+    # 10Hz 下 500ms 只应推进 5 拍控制律；其余 5 拍重发上一拍输出保心跳
+    assert law_updates["n"] == 5
+    assert len(px4.velocity_calls) == ticks    # offboard 设定点每拍都在下发
+    assert len(pod.sent) == ticks              # 表3 姿态下发不受控制律节流影响
+
+
+def test_control_law_throttle_resets_on_law_switch():
+    """切律后立即出第一拍（不被上一拍的节流压住）。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    cfg = BridgeConfig()
+    cfg.guidance.frequency_hz = 10.0
+    b = _bridge(px4, pod, cfg)
+
+    async def scenario():
+        await b.step(1000)
+        b.set_heading_servo_enabled(True)
+        await b.step(1010)                     # 切律后 10ms 即应推进
+    asyncio.run(scenario())
+    assert b._last_law_ms == 1010
+
+
+def test_stop_is_idempotent():
+    """重复 stop() 只真正停机一次（真机在环实测：RTL 与收尾各调一次）。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        await b.start()
+        await b.stop()
+        await b.stop()                         # 第二次应为 no-op
+    asyncio.run(scenario())
+
+    assert pod.closed is True
+    assert px4.offboard_stopped is True
+    assert len(px4.brake_calls) == 1           # 刹车只发一次
+
+
+def test_start_after_stop_reenables_stop():
+    """停机后重新 start() 可再次生效（标志位复位）。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        await b.start()
+        await b.stop()
+        assert b._stopped is True
+        await b.start()
+        assert b._stopped is False
+        await b.stop()
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) == 2           # 两次停机各发一次刹车
+
+
+def test_landing_stop_keeps_telemetry_and_pod_open():
+    """降落前只退 offboard：遥测与串口保留，供落地判定与高度曲线。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        await b.start()
+        await b.landing_stop()
+        assert b._landing_stopped is True
+        assert b._stopped is False             # 未全停
+        assert px4.offboard_stopped is True
+        assert px4.telemetry_stopped is False  # 遥测仍在
+        assert pod.closed is False             # 串口仍在
+        await b.stop()                         # 收尾才全停
+    asyncio.run(scenario())
+    assert px4.telemetry_stopped is True
+    assert pod.closed is True
+    assert len(px4.brake_calls) == 2           # 两次停机各发一次刹车
+
+
 # --- 吊舱帧入站缓存 --------------------------------------------------------
 def test_target_frame_updates_latest_and_flips_to_velocity():
    px4 = FakePx4(_valid_state(1000))
@@ -423,6 +539,12 @@ def _locked_off_target(x=0.0, y=0.0, frame_yaw=0.0, frame_pitch=0.0, laser=50.0)
                     frame_yaw_deg=frame_yaw, frame_pitch_deg=frame_pitch)


+def _set_pose(px4, now_ms: int, yaw_deg: float):
+    """更新假飞控姿态与时间戳（保证控制律的姿态超龄判定通过）。"""
+    px4._state = replace(px4._state, yaw_rad=math.radians(yaw_deg),
+                         time_ms=now_ms, position_time_ms=now_ms)
+
+
 def test_l1_off_target_beta_drives_yaw_rate():
    px4 = FakePx4(_valid_state(1000))
    pod = FakePod()
@@ -503,26 +625,73 @@ def test_l1_no_telemetry_degrades_and_keeps_heartbeat():
    assert len(px4.brake_calls) == 1


-def test_l1_require_vision_blocks_without_source():
-    # §2.3 第二判据：require_vision=True 但未接视觉源 → fail-safe 视为无目标 → 刹车
-    px4 = FakePx4(_valid_state(1000))
+def test_l1_los_consistent_keeps_tracking():
+    """机头右转多少、脱靶量就左移多少（目标在世界系静止）→ 判一致，正常跟踪。"""
+    clock = {"ms": 1_000_000}
+    px4 = FakePx4(_valid_state(clock["ms"]))
    pod = FakePod()
-    pod.queue(_locked_off_target(x=10.0))
-    b = _bridge(px4, pod, _l1_config(require_vision=True))
-    out = asyncio.run(b.step(1000))
-    assert out.mode.name == "BRAKE_HOVER"
-    assert px4.velocity_calls == []
+    b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"])

+    async def scenario():
+        for i in range(80):                      # 4s，20°/s 右转、脱靶量 −20°/s
+            clock["ms"] += 50
+            yaw = 20.0 * (i + 1) * 0.05
+            beta = 10.0 - 20.0 * (i + 1) * 0.05
+            _set_pose(px4, clock["ms"], yaw)
+            pod.queue(_locked_off_target(x=beta))
+            out = await b.step(clock["ms"])
+        return out

-def test_l1_require_vision_passes_with_source():
-    px4 = FakePx4(_valid_state(1000))
+    out = asyncio.run(scenario())
+    assert b._los_consistent is True
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert b.pod_locked(clock["ms"]) is True
+
+
+def test_l1_los_frozen_angle_blocks_tracking():
+    """机头转了 60° 而脱靶量几乎不变 → 角度冻结/幽灵目标 → 判丢（刹车）。"""
+    clock = {"ms": 1_000_000}
+    px4 = FakePx4(_valid_state(clock["ms"]))
    pod = FakePod()
-    pod.queue(_locked_off_target(x=10.0))
-    b = PodBridge(_l1_config(require_vision=True), px4, pod,
-                  now_ms_fn=lambda: 0, vision_target_fn=lambda: True)
-    out = asyncio.run(b.step(1000))
+    b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"])
+
+    async def scenario():
+        out = None
+        for i in range(80):                      # 4s，右转 20°/s，脱靶量恒定 10°
+            clock["ms"] += 50
+            yaw = 20.0 * (i + 1) * 0.05
+            _set_pose(px4, clock["ms"], yaw)
+            pod.queue(_locked_off_target(x=10.0))
+            out = await b.step(clock["ms"])
+        return out
+
+    out = asyncio.run(scenario())
+    assert b._los_consistent is False
+    assert out.mode.name == "BRAKE_HOVER"        # 控制律被喂 tracked=False
+    assert b.pod_locked(clock["ms"]) is False    # 状态机侧也判为丢目标
+
+
+def test_l1_los_disabled_falls_back_to_bit2():
+    """关掉一致性判据 → 退回"只信 bit2"（角度冻结也不再判丢）。"""
+    clock = {"ms": 1_000_000}
+    px4 = FakePx4(_valid_state(clock["ms"]))
+    pod = FakePod()
+    b = PodBridge(_l1_config(los_consistency_enabled=False), px4, pod,
+                  now_ms_fn=lambda: clock["ms"])
+
+    async def scenario():
+        out = None
+        for i in range(80):
+            clock["ms"] += 50
+            yaw = 20.0 * (i + 1) * 0.05
+            _set_pose(px4, clock["ms"], yaw)
+            pod.queue(_locked_off_target(x=10.0))
+            out = await b.step(clock["ms"])
+        return out
+
+    out = asyncio.run(scenario())
+    assert b._los_consistent is True
    assert out.mode.name == "VELOCITY_HEADING"
-    assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)


 # --- 地面坐标注入（A2）-------------------------------------------------------