keita
477ad5f5de
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
223 lines
8.6 KiB
Python
223 lines
8.6 KiB
Python
"""protocol_test.py —— 光电吊舱协议编解码单元测试（规格：docs/光电吊舱控制协议G版-PB.docx）。
|
||
|
||
覆盖：文档样例校验和、四帧 encode/decode 回环、量纲换算、遥测位域属性、
|
||
分帧器（粘包/半帧/噪声前缀/坏校验恢复）、异常帧拒绝。
|
||
"""
|
||
|
||
import pytest
|
||
|
||
from pod_bridge.protocol import (
|
||
CarrierAttitude,
|
||
ControlCode,
|
||
ControlCommand,
|
||
FrameParser,
|
||
Telemetry,
|
||
TargetGeo,
|
||
checksum,
|
||
decode_attitude,
|
||
decode_control,
|
||
decode_target,
|
||
decode_telemetry,
|
||
encode_attitude,
|
||
encode_control,
|
||
encode_target,
|
||
encode_telemetry,
|
||
)
|
||
|
||
|
||
# --- 校验和：文档第 589 行样例 ---------------------------------------------
|
||
def test_checksum_matches_doc_example():
|
||
# 文档样例 eb 90 2b 00..00（15 字节 body）累加取低 8 位应为 0xa6
|
||
body = bytes.fromhex("eb902b000000000000000000000000")
|
||
assert len(body) == 15
|
||
assert checksum(body) == 0xA6
|
||
|
||
|
||
def test_encode_control_center_matches_doc_bytes():
|
||
frame = encode_control(ControlCommand(code=ControlCode.CENTER))
|
||
assert frame == bytes.fromhex("eb902b000000000000000000000000a6")
|
||
assert len(frame) == 16
|
||
|
||
|
||
# --- 表2-1 控制指令回环 -----------------------------------------------------
|
||
def test_control_roundtrip_box_track():
|
||
cmd = ControlCommand(code=ControlCode.BOX_TRACK, param_x=-120, param_y=340,
|
||
param3=2, file_save_mode=1, param_x2=500, param_y2=-600)
|
||
decoded = decode_control(encode_control(cmd))
|
||
assert decoded == cmd
|
||
|
||
|
||
def test_control_signed_params_preserved():
|
||
cmd = ControlCommand(code=ControlCode.POINT_TRACK, param_x=-32768, param_y=32767)
|
||
decoded = decode_control(encode_control(cmd))
|
||
assert decoded.param_x == -32768
|
||
assert decoded.param_y == 32767
|
||
|
||
|
||
# --- 表3 载机姿态回环（含量纲换算）----------------------------------------
|
||
def test_attitude_roundtrip():
|
||
att = CarrierAttitude(
|
||
roll_deg=1.23, pitch_deg=-4.56, yaw_deg=179.99,
|
||
latitude_deg=31.2345678, longitude_deg=121.4737012,
|
||
abs_alt_m=123.4, rel_alt_m=56.8,
|
||
year=2026, month=9, day=22, hour=14, minute=30, second=15, centisecond=42,
|
||
airspeed_mps=15.5, ground_speed_mps=12.0,
|
||
)
|
||
d = decode_attitude(encode_attitude(att))
|
||
assert d.roll_deg == pytest.approx(1.23, abs=0.01)
|
||
assert d.pitch_deg == pytest.approx(-4.56, abs=0.01)
|
||
assert d.yaw_deg == pytest.approx(179.99, abs=0.01)
|
||
assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7)
|
||
assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7)
|
||
assert d.abs_alt_m == pytest.approx(123.4, abs=0.2)
|
||
assert d.rel_alt_m == pytest.approx(56.8, abs=0.2)
|
||
assert (d.year, d.month, d.day) == (2026, 9, 22)
|
||
assert (d.hour, d.minute, d.second, d.centisecond) == (14, 30, 15, 42)
|
||
assert d.airspeed_mps == pytest.approx(15.5, abs=0.5)
|
||
assert d.ground_speed_mps == pytest.approx(12.0, abs=0.5)
|
||
|
||
|
||
def test_attitude_sync_and_length():
|
||
frame = encode_attitude(CarrierAttitude())
|
||
assert len(frame) == 32
|
||
assert frame[0] == 0xEB and frame[1] == 0x91
|
||
|
||
|
||
# --- 表4 目标经纬度回环 -----------------------------------------------------
|
||
def test_target_roundtrip():
|
||
tgt = TargetGeo(latitude_deg=31.2345678, longitude_deg=121.4737012,
|
||
abs_alt_m=100.2, rel_alt_m=33.4,
|
||
year=2026, month=9, day=22, hour=8, minute=5, second=30,
|
||
centisecond=99)
|
||
d = decode_target(encode_target(tgt))
|
||
assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7)
|
||
assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7)
|
||
assert d.abs_alt_m == pytest.approx(100.2, abs=0.2)
|
||
assert d.rel_alt_m == pytest.approx(33.4, abs=0.2)
|
||
assert d.year == 2026 and d.centisecond == 99
|
||
|
||
|
||
def test_target_negative_longitude():
|
||
tgt = TargetGeo(latitude_deg=-33.8688, longitude_deg=-70.6693)
|
||
d = decode_target(encode_target(tgt))
|
||
assert d.longitude_deg == pytest.approx(-70.6693, abs=1e-7)
|
||
assert d.latitude_deg == pytest.approx(-33.8688, abs=1e-7)
|
||
|
||
|
||
# --- 表2-2 遥测：物理量 + 位域 ---------------------------------------------
|
||
def test_telemetry_roundtrip_physical_values():
|
||
t = Telemetry(status1=0b10000100, status2=0b00101010, status3=0b01000000,
|
||
off_target_x_deg=1.23, off_target_y_deg=-4.56,
|
||
frame_roll_deg=10.0, frame_pitch_deg=-20.0, frame_yaw_deg=90.5,
|
||
att_roll_deg=2.0, att_pitch_deg=-3.0,
|
||
roll_rate_dps=1.5, pitch_rate_dps=-2.5, yaw_rate_dps=30.0,
|
||
laser_range_m=123.4, self_check=0)
|
||
d = decode_telemetry(encode_telemetry(t))
|
||
assert d.off_target_x_deg == pytest.approx(1.23, abs=0.01)
|
||
assert d.off_target_y_deg == pytest.approx(-4.56, abs=0.01)
|
||
assert d.frame_yaw_deg == pytest.approx(90.5, abs=0.01)
|
||
assert d.yaw_rate_dps == pytest.approx(30.0, abs=0.01)
|
||
assert d.laser_range_m == pytest.approx(123.4, abs=0.1)
|
||
|
||
|
||
def test_telemetry_laser_zero_is_none():
|
||
d = decode_telemetry(encode_telemetry(Telemetry(laser_range_m=None)))
|
||
assert d.laser_range_m is None
|
||
|
||
|
||
def test_telemetry_bitfield_properties():
|
||
t = Telemetry(status1=0b11000100, status2=0b00101110, status3=0b01100000)
|
||
d = decode_telemetry(encode_telemetry(t))
|
||
assert d.track_source == 0b11 # 红外2
|
||
assert d.is_target_locked is True # status1 Bit2 = 1
|
||
assert d.motor_on is True # status2 Bit3 = 1
|
||
assert d.follow_on is True # status2 Bit2 = 1
|
||
assert d.big_picture == 0b01
|
||
assert d.small_picture == 0b10
|
||
|
||
|
||
# --- 异常帧拒绝 ------------------------------------------------------------
|
||
def test_decode_rejects_bad_sync():
|
||
frame = bytearray(encode_control(ControlCommand(code=ControlCode.CENTER)))
|
||
frame[1] = 0x91 # 破坏同步字
|
||
with pytest.raises(ValueError, match="同步字"):
|
||
decode_control(bytes(frame))
|
||
|
||
|
||
def test_decode_rejects_bad_checksum():
|
||
frame = bytearray(encode_target(TargetGeo(latitude_deg=10.0)))
|
||
frame[31] ^= 0xFF
|
||
with pytest.raises(ValueError, match="校验和"):
|
||
decode_target(bytes(frame))
|
||
|
||
|
||
def test_decode_rejects_wrong_length():
|
||
with pytest.raises(ValueError, match="帧长"):
|
||
decode_telemetry(b"\xee\x16" + b"\x00" * 10)
|
||
|
||
|
||
# --- 分帧器 ----------------------------------------------------------------
|
||
def test_parser_single_frame():
|
||
parser = FrameParser()
|
||
frames = parser.feed(encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0)))
|
||
assert len(frames) == 1
|
||
assert isinstance(frames[0], TargetGeo)
|
||
assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7)
|
||
|
||
|
||
def test_parser_byte_by_byte_half_frame():
|
||
parser = FrameParser()
|
||
raw = encode_telemetry(Telemetry(laser_range_m=50.0))
|
||
collected = []
|
||
for i in range(len(raw)):
|
||
collected.extend(parser.feed(raw[i:i + 1]))
|
||
assert len(collected) == 1
|
||
assert collected[0].laser_range_m == pytest.approx(50.0, abs=0.1)
|
||
|
||
|
||
def test_parser_concatenated_and_mixed_frames():
|
||
parser = FrameParser()
|
||
blob = (encode_telemetry(Telemetry(laser_range_m=10.0))
|
||
+ encode_target(TargetGeo(latitude_deg=1.0))
|
||
+ encode_telemetry(Telemetry(laser_range_m=20.0)))
|
||
frames = parser.feed(blob)
|
||
assert [type(f) for f in frames] == [Telemetry, TargetGeo, Telemetry]
|
||
assert frames[0].laser_range_m == pytest.approx(10.0, abs=0.1)
|
||
assert frames[2].laser_range_m == pytest.approx(20.0, abs=0.1)
|
||
|
||
|
||
def test_parser_skips_noise_prefix():
|
||
parser = FrameParser()
|
||
blob = b"\x00\xff\x12\x34" + encode_target(TargetGeo(latitude_deg=5.0))
|
||
frames = parser.feed(blob)
|
||
assert len(frames) == 1
|
||
assert frames[0].latitude_deg == pytest.approx(5.0, abs=1e-7)
|
||
|
||
|
||
def test_parser_recovers_from_bad_checksum():
|
||
parser = FrameParser()
|
||
bad = bytearray(encode_telemetry(Telemetry(laser_range_m=1.0)))
|
||
bad[31] ^= 0xFF # 坏校验
|
||
good = encode_target(TargetGeo(latitude_deg=7.0))
|
||
frames = parser.feed(bytes(bad) + good)
|
||
# 坏帧被丢弃，紧随的好帧仍能解析
|
||
assert len(frames) == 1
|
||
assert isinstance(frames[0], TargetGeo)
|
||
|
||
|
||
def test_parser_split_across_feeds():
|
||
parser = FrameParser()
|
||
raw = encode_target(TargetGeo(latitude_deg=42.0))
|
||
assert parser.feed(raw[:5]) == [] # 半帧，未输出
|
||
frames = parser.feed(raw[5:])
|
||
assert len(frames) == 1
|
||
assert frames[0].latitude_deg == pytest.approx(42.0, abs=1e-7)
|
||
|
||
|
||
def test_parser_accepts_configured_kinds():
|
||
parser = FrameParser(kinds=((0xEB, 0x90),)) # 只识别控制帧（模拟器侧）
|
||
frames = parser.feed(encode_control(ControlCommand(code=ControlCode.CENTER)))
|
||
assert len(frames) == 1
|
||
assert isinstance(frames[0], ControlCommand)
|
||
assert frames[0].code == ControlCode.CENTER
|