import struct
|
from datetime import datetime, timezone
|
|
import pytest
|
|
from hitl import protocols
|
|
|
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_im23a_nav_frame_format():
|
ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
|
frame = protocols.build_im23a_nav_frame(
|
timestamp=ts,
|
lat_deg=39.8301751,
|
lon_deg=116.2781268,
|
alt_m=48.5,
|
east_vel=0.2,
|
north_vel=0.1,
|
up_vel=0.0,
|
heading_deg=123.4,
|
pitch_deg=1.2,
|
roll_deg=-0.6,
|
accel_bias=(0.0, 0.0, 0.0),
|
gyro_bias=(0.0, 0.0, 0.0),
|
temperature_c=29.5,
|
status_flags=4,
|
)
|
|
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_im23a_imu_frame_format():
|
ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
|
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),
|
)
|
|
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:
|
header = b"\xAA\x55"
|
frame_type = b"\x10"
|
length = struct.pack("<H", len(payload))
|
checksum = (sum(frame_type + length + payload)) & 0xFFFF
|
footer = b"\x0D\x0A"
|
return header + frame_type + length + payload + struct.pack("<H", checksum) + footer
|
|
|
def test_pythonlink_decoder_float_payload():
|
received = []
|
decoder = protocols.PythonLinkDecoder(lambda frame: received.append(frame))
|
frame = _build_control_frame(struct.pack("<ff", 1.2, -0.4))
|
decoder.feed(frame[:5])
|
decoder.feed(frame[5:])
|
|
assert len(received) == 1
|
assert received[0].forward == pytest.approx(1.2)
|
assert received[0].turn == pytest.approx(-0.4)
|
|
|
def test_pythonlink_decoder_pwm_payload():
|
received = []
|
decoder = protocols.PythonLinkDecoder(lambda frame: received.append(frame))
|
frame = _build_control_frame(struct.pack("<HH", 2000, 1600))
|
decoder.feed(frame)
|
|
assert len(received) == 1
|
assert received[0].forward == pytest.approx(0.3, rel=0.05) # (1600-1500)/500 * 1.5
|
assert received[0].turn == pytest.approx(1.5708, rel=0.05)
|
