实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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结论与实现状态
This commit is contained in:
@@ -13,6 +13,7 @@ import pytest
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import BridgeConfig, load_from_json
|
||||
from pod_bridge.ground_link import GroundTarget
|
||||
from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
|
||||
Telemetry, decode_attitude, decode_control)
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
@@ -523,3 +524,75 @@ def test_l1_require_vision_passes_with_source():
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
# --- 地面坐标注入(A2)-------------------------------------------------------
|
||||
def _inject_config(timeout_ms=5000):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = timeout_ms
|
||||
return cfg
|
||||
|
||||
|
||||
def test_ground_inject_priority_over_table4():
|
||||
"""注入坐标优先于表4:吊舱无表4/无锁定(本设备常态),注入新鲜坐标照常引导。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||||
b = PodBridge(_inject_config(), px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert len(px4.velocity_calls) == 1
|
||||
|
||||
|
||||
def test_ground_inject_stale_falls_back_to_brake():
|
||||
"""注入坐标超龄 → 视为无目标 → 刹车(不得朝过期坐标飞)。"""
|
||||
px4 = FakePx4(_valid_state(2000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000) # 1000ms 前注入
|
||||
cfg = _inject_config(timeout_ms=500)
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(2000)) # 差值 1000ms > 500ms → 超龄
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
assert px4.velocity_calls == []
|
||||
|
||||
|
||||
def test_ground_inject_disabled_ignores_injection():
|
||||
"""未启用地面注入 → 忽略注入坐标,回退表4 原路径(无表4 → 刹车)。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||||
cfg = BridgeConfig() # ground_inject.enabled=False
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
|
||||
|
||||
# --- 运行时切换控制律(A3:GOTO(L2) ↔ TRACK(L1))-----------------------------
|
||||
def test_set_heading_servo_enabled_runtime_switch():
|
||||
"""运行时切换控制律:初始 L1 → 切 L2(刹车)→ 切回 L1(速度)。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
b = _bridge(px4, pod, _l1_config()) # enabled=True → 初始 L1
|
||||
|
||||
# 初始 L1:锁定 + 脱靶量 → 速度
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
# 切 L2:无表4/无锁定 → 刹车
|
||||
b.set_heading_servo_enabled(False)
|
||||
out = asyncio.run(b.step(1100))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
|
||||
# 切回 L1:重新喂锁定脱靶量 → 速度
|
||||
b.set_heading_servo_enabled(True)
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = asyncio.run(b.step(1200))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
@@ -133,6 +133,17 @@ def test_heading_servo_loads():
|
||||
assert hs.vertical_enabled is True # 决策3:无人机 L1 开垂直
|
||||
|
||||
|
||||
def test_deployed_config_has_calibrated_signs():
|
||||
"""S1 台架标定回归(2026-10-09):目标在左下时 off_target_x/y 均为负 →
|
||||
吊舱 X 轴右正(与控制律 β 同向) beta_sign=+1;Y 轴上正(与 α 下正相反) alpha_sign=-1。
|
||||
厂商确认本版电机无用 → 框架角路径必须关。改动这些值前必须重新台架标定。"""
|
||||
cfg = load_from_json(str(_DEFAULT_CONFIG))
|
||||
assert cfg.heading_servo.beta_sign == 1
|
||||
assert cfg.heading_servo.alpha_sign == -1
|
||||
assert cfg.heading_servo.beta_from_frame is False
|
||||
assert cfg.heading_servo.alpha_from_frame is False
|
||||
|
||||
|
||||
def test_heading_servo_partial_override(tmp_path):
|
||||
cfg = load_from_json(_write(tmp_path, {
|
||||
"heading_servo": {"beta_sign": -1, "beta_from_frame": True},
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
"""ground_link_test.py —— 地面坐标注入通道单元测试。
|
||||
|
||||
覆盖:parse_payload 解析(合法/缺字段/非法 JSON/非对象/类型错误)、
|
||||
is_valid 新鲜度判定(未注入/新鲜/超龄边界)。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget
|
||||
|
||||
|
||||
# --- parse_payload ----------------------------------------------------------
|
||||
def test_parse_valid_payload():
|
||||
t = GroundLink.parse_payload(
|
||||
b'{"lat": 39.9042, "lon": 116.4074, "alt": 50.0}')
|
||||
assert t is not None
|
||||
assert t.latitude_deg == pytest.approx(39.9042)
|
||||
assert t.longitude_deg == pytest.approx(116.4074)
|
||||
assert t.abs_alt_m == pytest.approx(50.0)
|
||||
assert t.time_ms == 0 # 打戳由接收方(run)负责
|
||||
|
||||
|
||||
def test_parse_default_alt_is_zero():
|
||||
t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0}')
|
||||
assert t is not None
|
||||
assert t.abs_alt_m == 0.0
|
||||
|
||||
|
||||
def test_parse_missing_lat_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_missing_lon_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lat": 1.0}') is None
|
||||
|
||||
|
||||
def test_parse_invalid_json_returns_none():
|
||||
assert GroundLink.parse_payload(b'not json') is None
|
||||
|
||||
|
||||
def test_parse_non_object_returns_none():
|
||||
assert GroundLink.parse_payload(b'[1, 2, 3]') is None
|
||||
|
||||
|
||||
def test_parse_string_coords_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lat": "x", "lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_bool_coords_returns_none():
|
||||
# True 是 bool 不是数值 → 拒绝
|
||||
assert GroundLink.parse_payload(b'{"lat": true, "lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_ignores_ts_field():
|
||||
"""ts 字段仅诊断,不参与解析(新鲜度用本地接收时刻)。"""
|
||||
t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0, "ts": 999}')
|
||||
assert t is not None
|
||||
assert t.latitude_deg == pytest.approx(1.0)
|
||||
|
||||
|
||||
# --- is_valid ---------------------------------------------------------------
|
||||
def _link(timeout_ms=5000):
|
||||
return GroundLink(GroundInjectConfig(timeout_ms=timeout_ms),
|
||||
now_ms_fn=lambda: 0)
|
||||
|
||||
|
||||
def _inject(link, time_ms):
|
||||
link._target = GroundTarget(latitude_deg=1.0, longitude_deg=2.0,
|
||||
time_ms=time_ms)
|
||||
|
||||
|
||||
def test_is_valid_false_when_never_injected():
|
||||
assert _link().is_valid(1000) is False
|
||||
|
||||
|
||||
def test_is_valid_true_when_fresh():
|
||||
link = _link(timeout_ms=5000)
|
||||
_inject(link, 1000)
|
||||
assert link.is_valid(1000) is True
|
||||
assert link.is_valid(4000) is True
|
||||
|
||||
|
||||
def test_is_valid_timeout_boundary():
|
||||
link = _link(timeout_ms=5000)
|
||||
_inject(link, 1000)
|
||||
assert link.is_valid(6000) is True # 恰好 5000ms,未超龄
|
||||
assert link.is_valid(6001) is False # 5001ms,超龄
|
||||
|
||||
|
||||
def test_latest_returns_injected_target():
|
||||
link = _link()
|
||||
_inject(link, 1000)
|
||||
assert link.latest().latitude_deg == pytest.approx(1.0)
|
||||
assert link.latest().time_ms == 1000
|
||||
@@ -5,6 +5,7 @@
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import math
|
||||
from enum import Enum
|
||||
from types import SimpleNamespace
|
||||
|
||||
@@ -62,7 +63,7 @@ class FakeTelemetry:
|
||||
def gps_info(self):
|
||||
return self._stream(self._gps)
|
||||
|
||||
def velocity_body(self):
|
||||
def velocity_ned(self):
|
||||
return self._stream(self._velocity)
|
||||
|
||||
|
||||
@@ -77,8 +78,9 @@ def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0):
|
||||
absolute_altitude_m=aalt, relative_altitude_m=ralt)
|
||||
|
||||
|
||||
def _att(roll=0.1, pitch=0.2, yaw=1.57):
|
||||
return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw)
|
||||
def _att(roll_deg=5.0, pitch_deg=10.0, yaw_deg=90.0):
|
||||
"""MAVSDK EulerAngle 字段是度(roll_deg/pitch_deg/yaw_deg),网关转弧度。"""
|
||||
return SimpleNamespace(roll_deg=roll_deg, pitch_deg=pitch_deg, yaw_deg=yaw_deg)
|
||||
|
||||
|
||||
# --- 枚举转换 ---------------------------------------------------------------
|
||||
@@ -120,12 +122,14 @@ def test_update_attitude_sets_radians():
|
||||
gw.update_attitude(_att())
|
||||
s = gw.own_state()
|
||||
assert s.has_attitude is True
|
||||
assert s.yaw_rad == pytest.approx(1.57)
|
||||
assert s.roll_rad == pytest.approx(math.radians(5.0))
|
||||
assert s.pitch_rad == pytest.approx(math.radians(10.0))
|
||||
assert s.yaw_rad == pytest.approx(math.radians(90.0))
|
||||
|
||||
|
||||
def test_update_velocity_body_ground_speed_is_horizontal_magnitude():
|
||||
def test_update_velocity_ned_ground_speed_is_horizontal_magnitude():
|
||||
gw = Px4Gateway(FakeDrone())
|
||||
gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0,
|
||||
gw.update_velocity_ned(SimpleNamespace(north_m_s=3.0, east_m_s=4.0,
|
||||
down_m_s=-9.9))
|
||||
assert gw.own_state().ground_speed_mps == pytest.approx(5.0) # 不含 down
|
||||
|
||||
@@ -173,9 +177,9 @@ def test_command_delegation_reaches_offboard():
|
||||
def test_start_telemetry_populates_cache_then_stop():
|
||||
telemetry = FakeTelemetry(
|
||||
position=[_pos(lat=22.0, lon=114.0)],
|
||||
attitude=[_att(yaw=0.5)],
|
||||
attitude=[_att(yaw_deg=30.0)],
|
||||
gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
|
||||
velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)],
|
||||
velocity=[SimpleNamespace(north_m_s=2.0, east_m_s=0.0, down_m_s=0.0)],
|
||||
)
|
||||
gw = Px4Gateway(FakeDrone(telemetry))
|
||||
|
||||
@@ -184,7 +188,7 @@ def test_start_telemetry_populates_cache_then_stop():
|
||||
await asyncio.sleep(0.05) # 让后台任务消费预设消息
|
||||
s = gw.own_state()
|
||||
assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0
|
||||
assert s.yaw_rad == pytest.approx(0.5)
|
||||
assert s.yaw_rad == pytest.approx(math.radians(30.0))
|
||||
assert s.position_valid is True
|
||||
assert s.ground_speed_mps == pytest.approx(2.0)
|
||||
await gw.stop_telemetry()
|
||||
@@ -210,7 +214,7 @@ def test_position_timestamp_not_refreshed_by_other_streams():
|
||||
clock[0] = 2000
|
||||
gw.update_attitude(_att())
|
||||
gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9))
|
||||
gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0,
|
||||
gw.update_velocity_ned(SimpleNamespace(north_m_s=1.0, east_m_s=0.0,
|
||||
down_m_s=0.0))
|
||||
s = gw.own_state()
|
||||
assert s.time_ms == 2000 # 诊断时间戳照常刷新
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user