yincheng.zhong
2 天以前 567085ead3f6adaabd884f16ab4b17c62e8f0403
python/tests_hitl/test_protocols.py
@@ -6,16 +6,15 @@
from hitl import protocols
def _calc_checksum(sentence: str) -> str:
    cs = 0
    for ch in sentence[1:]:
        cs ^= ord(ch)
    return f"{cs:02X}"
def _checksum_matches(frame: bytes) -> bool:
    calc = sum(frame[:-4]) & 0xFFFF
    stored = struct.unpack("<H", frame[-4:-2])[0]
    return calc == stored
def test_build_gprmi_sentence_format():
def test_build_im23a_nav_frame_format():
    ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
    sentence = protocols.build_gprmi_sentence(
    frame = protocols.build_im23a_nav_frame(
        timestamp=ts,
        lat_deg=39.8301751,
        lon_deg=116.2781268,
@@ -26,30 +25,43 @@
        heading_deg=123.4,
        pitch_deg=1.2,
        roll_deg=-0.6,
    ).decode("ascii")
        accel_bias=(0.0, 0.0, 0.0),
        gyro_bias=(0.0, 0.0, 0.0),
        temperature_c=29.5,
        status_flags=4,
    )
    assert sentence.startswith("$GPRMI,")
    assert sentence.endswith("\r\n")
    body, checksum = sentence.strip()[0:sentence.index("*")], sentence[sentence.index("*") + 1 : sentence.index("*") + 3]
    assert checksum == _calc_checksum(body)
    fields = body.split(",")
    assert len(fields) == 24  # $GPRMI + 23 个字段
    assert frame.startswith(b"fmin")
    assert frame.endswith(b"ed")
    assert len(frame) == protocols.IM23A_NAV_LEN
    assert _checksum_matches(frame)
    # 解码部分字段验证顺序
    ts_val = struct.unpack("<d", frame[4:12])[0]
    assert ts_val == pytest.approx(8 * 3600 + 1 * 60 + 12, rel=1e-6)
    lat = struct.unpack("<d", frame[12:20])[0]
    lon = struct.unpack("<d", frame[20:28])[0]
    assert lat == pytest.approx(39.8301751)
    assert lon == pytest.approx(116.2781268)
def test_build_gpimu_sentence_format():
def test_build_im23a_imu_frame_format():
    ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
    sentence = protocols.build_gpimu_sentence(
    frame = protocols.build_im23a_imu_frame(
        timestamp=ts,
        accel_g=(0.01, -0.02, -0.99),
        gyro_deg_s=(0.1, 0.2, -0.3),
        temperature_c=29.5,
    ).decode("ascii")
    )
    assert sentence.startswith("$GPIMU,")
    body = sentence.strip()[0:sentence.index("*")]
    checksum = sentence[sentence.index("*") + 1 : sentence.index("*") + 3]
    assert checksum == _calc_checksum(body)
    assert len(body.split(",")) == 9  # $GPIMU + 8 字段
    assert frame.startswith(b"fmim")
    assert frame.endswith(b"ed")
    assert len(frame) == protocols.IM23A_IMU_LEN
    assert _checksum_matches(frame)
    accel_x = struct.unpack("<f", frame[12:16])[0]
    gyro_z = struct.unpack("<f", frame[28:32])[0]
    assert accel_x == pytest.approx(0.01)
    assert gyro_z == pytest.approx(-0.3)
def _build_control_frame(payload: bytes) -> bytes: