重构RTK-IMU标定链路并完成机械先验工程验证

tests/test_hi13_imu.py

@@ -5,7 +5,12 @@ from __future__ import annotations
 import struct
 
 from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
-from tools.rscap_v2.hi13_imu import crc16_hi13, iter_hi13_imu_samples, parse_hi91_frame
+from tools.rscap_v2.hi13_imu import (
+    crc16_hi13,
+    iter_hi13_imu_samples,
+    parse_hi91_frame,
+    parse_hi91_sample,
+)
 
 
 def _hi91_frame(
@@ -13,6 +18,8 @@ def _hi91_frame(
     device_ms: int = 123456,
     accel_g=(0.0, 0.0, 1.0),
     gyro_dps=(1.0, -2.0, 3.0),
+    rpy_deg=(4.0, 5.0, 6.0),
+    quaternion_wxyz=(1.0, 0.0, 0.0, 0.0),
 ) -> bytes:
     payload = bytearray(76)
     payload[0] = 0x91
@@ -22,7 +29,8 @@ def _hi91_frame(
     struct.pack_into("<I", payload, 8, device_ms)
     struct.pack_into("<fff", payload, 12, *accel_g)
     struct.pack_into("<fff", payload, 24, *gyro_dps)
-    # remaining mag/rpy/quat left zero
+    struct.pack_into("<fff", payload, 48, *rpy_deg)
+    struct.pack_into("<ffff", payload, 60, *quaternion_wxyz)
     payload_length = len(payload)
     header = bytearray(6)
     header[0] = 0x5A
@@ -48,6 +56,12 @@ def test_parse_hi91_units():
     assert device_ms == 5000
     assert abs(accel[2] - 9.80665) < 1e-4
     assert abs(gyro[0] - 1.0) < 1e-5
+    full = parse_hi91_sample(frame, host_receive_utc_ticks=123)
+    assert full is not None
+    assert full.system_time_ms == 5000
+    assert full.host_receive_utc_ticks == 123
+    assert full.rpy_deg == (4.0, 5.0, 6.0)
+    assert full.quaternion_wxyz == (1.0, 0.0, 0.0, 0.0)
 
 
 def test_iter_hi13_from_capture():