keita
e1dbf081ab
- 伪造载机姿态/位置/时戳持续喂表3,单进程收发一体观测表2-2/表4 - 回答Q1(无激光是否仍解算表4)/Q2(表4实际频率)/Q3(丢失后行为) - 自动判读「目标贴地」假设: 表4相对高度是否约等于喂入的载机高度 - 退出码0/1/2/3便于脚本串测; --json落逐帧采样供阶段2聚类分析 - 默认参数已敲定: lat=39.9042 lon=116.4074 rel-alt=50 rate=20Hz - 纯软单测21条(注入fake serial), 全量回归372 passed
270 lines
9.5 KiB
Python
270 lines
9.5 KiB
Python
"""sim_fc_test.py —— 模拟飞控喂表3 工具的纯软单测(注入 fake serial,无硬件)。
|
|
|
|
覆盖:
|
|
* Feeder:静态姿态 / 时戳冻结 / yaw 匀速推进 / 地速位移;
|
|
* offset_m: East/North 换算方向;
|
|
* Collector:遥测与表4 计数、锁定跳变、贴地假设、回环帧识别;
|
|
* stddev / ptpm 统计;
|
|
* verdict 四种判读分支;
|
|
* main() 端到端(假串口回回放表4)→ 退出码 0。
|
|
"""
|
|
|
|
import sys
|
|
import types
|
|
|
|
import pytest
|
|
|
|
from pod_bridge.protocol import (CarrierAttitude, TargetGeo, Telemetry,
|
|
encode_attitude, encode_target)
|
|
from pod_bridge.sim_fc import (EXIT_LINK_ERROR, EXIT_NO_TARGET_FRAME, EXIT_OK,
|
|
EXIT_ZERO_GEO, Collector, Feeder, main, offset_m,
|
|
ptpm, stddev, verdict)
|
|
|
|
_LAT, _LON = 39.9042, 116.4074
|
|
|
|
|
|
class _FakeSerialInstance:
|
|
"""pyserial.Serial 的最小替身:可预设 RX 字节,记录 TX 写入。"""
|
|
|
|
def __init__(self, **kwargs):
|
|
self.kwargs = kwargs
|
|
self._buf = b""
|
|
self.written = []
|
|
self.closed = False
|
|
|
|
def seed_rx(self, data: bytes) -> None:
|
|
self._buf += data
|
|
|
|
@property
|
|
def in_waiting(self):
|
|
return len(self._buf)
|
|
|
|
def read(self, n):
|
|
data, self._buf = self._buf[:n], self._buf[n:]
|
|
return data
|
|
|
|
def write(self, data):
|
|
self.written.append(bytes(data))
|
|
return len(data)
|
|
|
|
def close(self):
|
|
self.closed = True
|
|
|
|
|
|
@pytest.fixture
|
|
def fake_serial(monkeypatch):
|
|
"""注入 serial 模块并返回**共享实例**——PodSerial 内部 Serial(...) 必须拿到
|
|
同一个对象,否则外部 seed_rx 预置的字节 main() 读不到。"""
|
|
mod = types.ModuleType("serial")
|
|
mod.EIGHTBITS = 8
|
|
mod.PARITY_NONE = "N"
|
|
mod.STOPBITS_ONE = 1
|
|
shared = _FakeSerialInstance(port="FAKE")
|
|
mod.Serial = lambda **kwargs: shared
|
|
monkeypatch.setitem(sys.modules, "serial", mod)
|
|
return shared
|
|
|
|
|
|
def _locked_telemetry(locked: bool) -> Telemetry:
|
|
status1 = (1 << 2) if locked else 0
|
|
return Telemetry(status1=status1)
|
|
|
|
|
|
# --- Feeder -----------------------------------------------------------------
|
|
def test_feeder_static_defaults():
|
|
f = Feeder(_LAT, _LON, abs_alt_m=50.0, rel_alt_m=50.0)
|
|
frame = f.build(0.0)
|
|
assert isinstance(frame, CarrierAttitude)
|
|
assert (frame.latitude_deg, frame.longitude_deg) == (_LAT, _LON)
|
|
assert (frame.roll_deg, frame.pitch_deg, frame.yaw_deg) == (0.0, 0.0, 0.0)
|
|
assert (frame.abs_alt_m, frame.rel_alt_m) == (50.0, 50.0)
|
|
assert 0 <= frame.centisecond <= 99
|
|
|
|
|
|
def test_feeder_freeze_time_keeps_stamp():
|
|
f = Feeder(_LAT, _LON, 50.0, 50.0, freeze_time=True)
|
|
assert f.build(0.0).second == f.build(30.0).second
|
|
|
|
|
|
def test_feeder_yaw_sweep_wraps():
|
|
f = Feeder(_LAT, _LON, 50.0, 50.0, yaw_deg=350.0, yaw_rate_dps=20.0)
|
|
assert abs(f.build(0.0).yaw_deg - 350.0) < 1e-9
|
|
assert abs(f.build(1.0).yaw_deg - 10.0) < 1e-9 # 360 → 回绕到 10°
|
|
|
|
|
|
def test_feeder_ground_speed_moves_position_north():
|
|
f = Feeder(_LAT, _LON, 50.0, 50.0, yaw_deg=0.0, ground_speed_mps=10.0)
|
|
later = f.build(10.0) # 向北 100m
|
|
north_m = (later.latitude_deg - _LAT) * 111320.0
|
|
assert 95.0 < north_m < 105.0
|
|
assert abs((later.longitude_deg - _LON) * 111320.0) < 1.0
|
|
|
|
|
|
# --- 几何 / 统计 ------------------------------------------------------------
|
|
def test_offset_m_directions():
|
|
east, north = offset_m(0.0, 0.0, 0.0, 0.0001)
|
|
assert north == pytest.approx(0.0, abs=1e-6)
|
|
assert east > 10.0
|
|
east2, north2 = offset_m(0.0, 0.0, 0.0001, 0.0)
|
|
assert east2 == pytest.approx(0.0, abs=1e-6)
|
|
assert north2 > 10.0
|
|
|
|
|
|
def test_stddev_and_ptpm():
|
|
assert stddev([1.0]) == 0.0
|
|
assert stddev([]) == 0.0
|
|
assert stddev([2.0, 2.0, 2.0]) == 0.0
|
|
assert stddev([1.0, 3.0]) == pytest.approx(1.0)
|
|
assert ptpm([]) == 0.0
|
|
assert ptpm([1.0, 5.0, 3.0]) == pytest.approx(4.0)
|
|
|
|
|
|
# --- Collector --------------------------------------------------------------
|
|
def test_collector_counts_and_lock_events():
|
|
c = Collector(_LAT, _LON)
|
|
for t, lk in ((0.1, False), (0.2, True), (0.3, True), (0.4, False)):
|
|
c.update(_locked_telemetry(lk), t)
|
|
assert c.n_tele == 4
|
|
assert c.n_lock_events == 2 # False→True, True→False
|
|
assert c.first_lock_s == pytest.approx(0.2)
|
|
assert c.n_tgt == 0
|
|
|
|
|
|
def test_collector_target_stats_and_first_frame():
|
|
c = Collector(_LAT, _LON)
|
|
assert c.has_valid_geo is False
|
|
c.update(TargetGeo(latitude_deg=_LAT, longitude_deg=_LON), 1.0)
|
|
c.update(TargetGeo(latitude_deg=_LAT + 0.0001, longitude_deg=_LON), 2.0)
|
|
assert c.has_valid_geo is True
|
|
assert c.first_tgt_s == pytest.approx(1.0)
|
|
assert c.last_tgt_s == pytest.approx(2.0)
|
|
north = c.records[-1]["north_m"]
|
|
assert north == pytest.approx(11.132, abs=0.5)
|
|
|
|
|
|
def test_collector_flags_loopback_and_unknown():
|
|
c = Collector(_LAT, _LON)
|
|
c.update(CarrierAttitude(), 0.1)
|
|
c.update(object(), 0.2)
|
|
assert c.n_loopback == 1
|
|
assert c.n_other == 1
|
|
|
|
|
|
def test_collector_summary_includes_geo_validity():
|
|
c = Collector(_LAT, _LON)
|
|
assert c.summary(1.0, 20, 50.0, 50.0)["has_valid_geo"] is False
|
|
c.update(TargetGeo(latitude_deg=_LAT, longitude_deg=_LON, rel_alt_m=50.0), 0.5)
|
|
summary = c.summary(1.0, 20, 50.0, 50.0)
|
|
assert summary["has_valid_geo"] is True
|
|
assert summary["target_hz"] == pytest.approx(1.0)
|
|
assert summary["target_stale_s"] == pytest.approx(0.5)
|
|
|
|
|
|
# --- verdict ----------------------------------------------------------------
|
|
def _summary(**over):
|
|
base = {
|
|
"elapsed_s": 10.0, "target_frames": 0, "target_hz": 0.0,
|
|
"first_target_s": None, "first_lock_s": None, "has_valid_geo": False,
|
|
"loopback_frames": 0, "east_mean_m": 0.0, "north_mean_m": 0.0,
|
|
"spread_east_m": 0.0, "spread_north_m": 0.0,
|
|
"rel_alt_mean_m": 0.0, "ref_rel_alt_m": 50.0,
|
|
}
|
|
base.update(over)
|
|
return base
|
|
|
|
|
|
def test_verdict_no_target_frame():
|
|
code, lines = verdict(_summary(), None)
|
|
assert code == EXIT_NO_TARGET_FRAME
|
|
assert any("Q1 失败" in ln for ln in lines)
|
|
|
|
|
|
def test_verdict_zero_geo():
|
|
code, lines = verdict(_summary(target_frames=100), None)
|
|
assert code == EXIT_ZERO_GEO
|
|
assert any("有帧无解算" in ln for ln in lines)
|
|
|
|
|
|
def test_verdict_ok_and_ground_plane_warning():
|
|
code, lines = verdict(_summary(target_frames=100, target_hz=5.5,
|
|
has_valid_geo=True, first_target_s=3.4,
|
|
first_lock_s=3.0, rel_alt_mean_m=50.0), None)
|
|
assert code == EXIT_OK
|
|
assert any("Q1 通过" in ln for ln in lines)
|
|
assert any("目标在地面" in ln for ln in lines) # 表4 相对高度 ≈ 喂入值
|
|
|
|
|
|
def test_verdict_link_error_takes_priority():
|
|
code, lines = verdict(_summary(target_frames=100, has_valid_geo=True),
|
|
OSError(5, "Input/output error"))
|
|
assert code == EXIT_LINK_ERROR
|
|
assert any("链路中断" in ln for ln in lines)
|
|
|
|
|
|
def test_verdict_flags_loopback():
|
|
code, lines = verdict(_summary(target_frames=1, has_valid_geo=True,
|
|
loopback_frames=7), None)
|
|
assert code == EXIT_OK
|
|
assert any("回环" in ln for ln in lines)
|
|
|
|
|
|
# --- main() 端到端(假串口回环) ---------------------------------------------
|
|
def test_main_end_to_end_reports_target(fake_serial, tmp_path, capsys):
|
|
inst = fake_serial
|
|
target = TargetGeo(latitude_deg=_LAT + 0.0002, longitude_deg=_LON,
|
|
abs_alt_m=50.0, rel_alt_m=50.0)
|
|
inst.seed_rx(encode_target(target))
|
|
|
|
code = main(["FAKE", "--seconds", "0.05", "--rate", "60",
|
|
"--rel-alt", "50", "--quiet", "--json", str(tmp_path / "r.json")])
|
|
out = capsys.readouterr().out
|
|
|
|
assert code == EXIT_OK
|
|
assert inst.closed is True
|
|
assert len(inst.written) >= 1 # 至少发过一帧 表3
|
|
assert inst.written[0].startswith(b"\xeb\x91") # 同步字 EB 91
|
|
assert "Q1 通过" in out
|
|
assert (tmp_path / "r.json").exists()
|
|
|
|
|
|
def test_main_no_target_returns_error(fake_serial, capsys):
|
|
fake_serial # RX 为空
|
|
code = main(["FAKE", "--seconds", "0.05", "--rate", "60", "--quiet"])
|
|
assert code == EXIT_NO_TARGET_FRAME
|
|
|
|
|
|
def test_main_rejects_bad_rate(fake_serial, capsys):
|
|
code = main(["FAKE", "--rate", "100", "--quiet"])
|
|
out = capsys.readouterr().out
|
|
assert code == EXIT_LINK_ERROR
|
|
assert "1~60Hz" in out
|
|
|
|
|
|
def test_main_auto_lock_requires_detect(fake_serial, capsys):
|
|
code = main(["FAKE", "--auto-lock", "--quiet"])
|
|
out = capsys.readouterr().out
|
|
assert code == EXIT_LINK_ERROR
|
|
assert "--auto-lock 需配合" in out
|
|
|
|
|
|
def test_main_sends_detect_sequence(fake_serial, capsys):
|
|
inst = fake_serial
|
|
main(["FAKE", "--seconds", "0.02", "--rate", "60", "--quiet",
|
|
"--detect", "drone", "--auto-lock", "--motor-on"])
|
|
codes = [w[2] for w in inst.written]
|
|
assert 0x27 in codes # 开电机
|
|
assert 0x07 in codes # 目标识别开
|
|
assert 0x0F in codes # 自动锁定
|
|
assert 0x10 in codes # 识别转跟踪
|
|
|
|
|
|
def test_feeder_frame_is_encodable():
|
|
"""Feeder 产物必须能通过 protocol 编码/解码往返(防止字段对不上规格)。"""
|
|
from pod_bridge.protocol import decode_attitude
|
|
f = Feeder(_LAT, _LON, 50.0, 50.0, roll_deg=1.5, pitch_deg=-2.0, yaw_deg=30.0)
|
|
frame = f.build(0.0)
|
|
back = decode_attitude(encode_attitude(frame))
|
|
assert back.latitude_deg == pytest.approx(frame.latitude_deg, abs=1e-7)
|
|
assert back.rel_alt_m == pytest.approx(50.0, abs=0.2) # ALT_SCALE=0.2m
|
|
assert back.yaw_deg == pytest.approx(30.0, abs=0.01)
|