keita
ded17d7995
- 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结论与实现状态
125 lines
4.3 KiB
Python
125 lines
4.3 KiB
Python
"""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
|