实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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,124 @@
+"""virtual_pod_test.py —— 模拟吊舱几何与 S1 符号约定回归测试。
+
+钉死 S1 台架标定结论（2026-10-09）：目标在左下时 off_target_x/y 均为负，
+即 X 右正、Y 上正；并验证不可见/超视场 → 搜索态 + 脱靶量归 0（对齐真实
+吊舱"丢失时脱靶量恒 0"的行为）。
+"""
+
+import math
+import time
+
+import pytest
+
+from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
+                                    VirtualTargetConfig)
+
+
+def _pod(target, own):
+    return VirtualPod(target, lambda: own)
+
+
+def _static_target(ahead=30.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 _carrier(yaw_deg=0.0):
+    return CarrierView(north_m=0.0, east_m=0.0, down_m=0.0,
+                       yaw_rad=math.radians(yaw_deg))
+
+
+def _telemetry(pod):
+    frames = pod.poll()
+    assert len(frames) == 1
+    return frames[0]
+
+
+# ---- S1 符号约定回归：目标位置 → 脱靶量符号 --------------------------------
+def test_target_right_gives_positive_off_x():
+    # 目标在右前方：rel_e>0、机头朝北 → 脱靶量 X 应为正（右正）
+    tgt = _static_target(ahead=10.0, right=5.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_x_deg > 0.0
+    assert tel.off_target_x_deg == pytest.approx(math.degrees(math.atan2(5.0, 10.0)),
+                                                 abs=1e-9)
+
+
+def test_target_left_gives_negative_off_x():
+    tgt = _static_target(ahead=10.0, right=-5.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_x_deg < 0.0
+
+
+def test_target_below_gives_negative_off_y():
+    # S1 实测：目标偏下 → off_target_y 为负（上正）。11.3° 在垂直视场(35°)内
+    tgt = _static_target(ahead=10.0, down=2.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_y_deg < 0.0
+    assert tel.off_target_y_deg == pytest.approx(-math.degrees(math.atan2(2.0, 10.0)),
+                                                 abs=1e-9)
+
+
+def test_target_above_gives_positive_off_y():
+    tgt = _static_target(ahead=10.0, down=-2.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_y_deg > 0.0
+
+
+def test_yaw_rotation_follows_body_frame():
+    # 机头偏东 20°：目标在正北 → 目标在机体左侧 20° → off_x 为负
+    tgt = _static_target(ahead=10.0)
+    tel = _telemetry(_pod(tgt, _carrier(yaw_deg=20.0)))
+    assert tel.off_target_x_deg == pytest.approx(-20.0, abs=1e-9)
+
+
+# ---- 无电机 / 无激光语义 ---------------------------------------------------
+def test_frame_angles_always_zero_and_laser_none():
+    tgt = _static_target()
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.frame_yaw_deg == 0.0
+    assert tel.frame_pitch_deg == 0.0
+    assert tel.frame_roll_deg == 0.0
+    assert tel.laser_range_m is None
+
+
+# ---- 锁定门控 --------------------------------------------------------------
+def test_visible_target_locks():
+    tel = _telemetry(_pod(_static_target(), _carrier()))
+    assert tel.is_target_locked
+
+
+def test_hidden_target_searches_with_zero_off():
+    tgt = _static_target()
+    tgt.set_visible(False)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert not tel.is_target_locked
+    assert tel.off_target_x_deg == 0.0 and tel.off_target_y_deg == 0.0
+
+
+def test_target_behind_searches():
+    # 机头朝北、目标在正南 10m → fwd<0 → 搜索态
+    cfg = VirtualTargetConfig(ahead_m=-10.0, right_m=0.0, down_m=0.0,
+                              arc_speed_mps=0.0)
+    tgt = VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert not tel.is_target_locked
+
+
+def test_target_out_of_fov_searches():
+    # 目标几乎在正右方（β→90°）超出默认 FOV 60° → 搜索态
+    tgt = _static_target(ahead=1.0, right=100.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert not tel.is_target_locked
+
+
+# ---- PodLink 接口 ----------------------------------------------------------
+def test_send_counts_and_close_marks():
+    pod = _pod(_static_target(), _carrier())
+    pod.send(b"\x00" * 4)
+    pod.send(b"\x00" * 4)
+    assert pod.attitude_frames_rx == 2
+    pod.close()
+    assert pod.closed