| | |
| | | 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, |
| | |
| | | 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: |