import struct
|
from datetime import datetime, timezone
|
|
import pytest
|
|
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 test_build_gprmi_sentence_format():
|
ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
|
sentence = protocols.build_gprmi_sentence(
|
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,
|
).decode("ascii")
|
|
assert sentence.startswith("$GPFMI,")
|
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 个字段
|
|
|
def test_build_gpimu_sentence_format():
|
ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
|
sentence = protocols.build_gpimu_sentence(
|
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 字段
|
|
|
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)
|
