实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口

- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
2026-10-13 15:50:32 +08:00
parent 5a944af4f5
commit ded17d7995
30 changed files with 3552 additions and 27 deletions
@@ -0,0 +1,104 @@
+"""virtual_pod_bridge_test.py —— VirtualPod × PodBridge × TrackingControlLaw 集成测试。
+
+用部署配置(pod_bridge.json)把"SITL 闭环段"在无飞控下跑通，端到端复核 S1 标定：
+  目标偏右 → 控制律应输出正 yaw_rate（右转）；
+  目标偏下 → 应输出正 vz（下降）；
+  目标消失 → 应判丢零速刹车。
+"""
+
+import asyncio
+from dataclasses import replace
+from pathlib import Path
+
+import pytest
+
+from pod_bridge.bridge import PodBridge
+from pod_bridge.config import load_from_json
+from pod_bridge.px4_gateway import OwnState
+from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
+                                    VirtualTargetConfig)
+
+_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"
+
+
+class FakePx4:
+    def __init__(self, yaw_rad=0.0):
+        self._yaw = yaw_rad
+        self.clock = 1000          # 测试循环每拍推进，与 step(now_ms) 同步
+        self.velocity_calls = []
+        self.brake_calls = []
+
+    def own_state(self):
+        return OwnState(has_attitude=True, has_position=True, position_valid=True,
+                        fix_type=3, yaw_rad=self._yaw, time_ms=self.clock,
+                        position_time_ms=self.clock)
+
+    async def start_telemetry(self): pass
+    async def stop_telemetry(self): pass
+    async def start_offboard(self): return True
+    async def stop_offboard(self): pass
+    async def send_velocity_heading(self, out, time_boot_ms=None):
+        self.velocity_calls.append(out); return True
+    async def send_brake_hover(self, time_boot_ms=None):
+        self.brake_calls.append(time_boot_ms); return True
+
+
+def _bridge(target, own):
+    cfg = load_from_json(str(_DEFAULT_CONFIG))  # 部署配置：L1 enabled、S1 符号
+    pod = VirtualPod(target, lambda: own)
+    px4 = FakePx4(yaw_rad=own.yaw_rad)
+    return PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0), px4
+
+
+def _static_target(ahead=10.0, right=0.0, down=0.0):
+    cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
+                              arc_speed_mps=0.0)
+    return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
+
+
+def _run_ticks(bridge, px4, n, t0=1000, dt=50):
+    """跑 n 拍（每拍 50ms，节拍与 20Hz 一致），返回末拍输出。"""
+    out = None
+    for i in range(n):
+        t = t0 + i * dt
+        px4.clock = t
+        out = asyncio.run(bridge.step(t))
+    return out
+
+
+def test_deployed_config_turns_right_for_right_target():
+    # 目标在右前方：S1 标定 beta_sign=+1 → yaw_rate 应为正（右转）
+    tgt = _static_target(ahead=10.0, right=5.0)
+    bridge, px4 = _bridge(tgt, CarrierView())
+    out = _run_ticks(bridge, px4, 5)
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert out.yaw_rate_dps > 0.0
+    assert px4.velocity_calls  # 正常速度航向指令
+    assert not px4.brake_calls
+
+
+def test_deployed_config_descends_for_below_target():
+    # 目标偏下：S1 标定 alpha_sign=-1（吊舱上正→控制律下正）→ vz 应为正（下降）
+    tgt = _static_target(ahead=10.0, down=2.0)
+    bridge, px4 = _bridge(tgt, CarrierView())
+    out = _run_ticks(bridge, px4, 10)
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert out.vz_mps > 0.0
+
+
+def test_deployed_config_slow_approach_forward():
+    # L1 无距离：slow_approach 0.2 m/s 起步（决策3 低限速）
+    tgt = _static_target()
+    bridge, px4 = _bridge(tgt, CarrierView())
+    out = _run_ticks(bridge, px4, 10)
+    assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
+
+
+def test_target_lost_brakes_through_pipeline():
+    tgt = _static_target()
+    bridge, px4 = _bridge(tgt, CarrierView())
+    _run_ticks(bridge, px4, 3)
+    tgt.set_visible(False)  # 目标消失 → 吊舱转搜索态 → 判丢刹车
+    out = _run_ticks(bridge, px4, 2, t0=1000 + 3 * 50)
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.brake_calls