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结论与实现状态
105 lines
3.8 KiB
Python
105 lines
3.8 KiB
Python
"""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
|