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

tools/rscap_v2/hi13_imu.py

@@ -12,6 +12,7 @@ HI91 (preferred for calibration):
 from __future__ import annotations
 
 import struct
+from dataclasses import dataclass
 
 import numpy as np
 
@@ -22,6 +23,29 @@ G0 = 9.80665
 DEG2RAD = np.pi / 180.0
 
 
+@dataclass(frozen=True)
+class Hi13Sample:
+    """One CRC-valid native HI91 record.
+
+    t_s/system_time_ms are sensor-owned. Host receive ticks are retained only
+    for clock diagnostics and cross-clock fitting.
+    """
+
+    t_s: float
+    system_time_ms: int
+    device_timestamp_us: int
+    host_receive_utc_ticks: int
+    gyro_rad_s: tuple[float, float, float]
+    accel_m_s2: tuple[float, float, float]
+    rpy_deg: tuple[float, float, float]
+    quaternion_wxyz: tuple[float, float, float, float]
+    mag_ut: tuple[float, float, float]
+    pps_sync_stamp_ms: int
+    temperature_c: int
+    air_pressure_pa: float
+    frame_tag: int = 0x91
+
+
 def crc16_hi13(frame: bytes, payload_length: int) -> int:
     crc = 0
     for value in frame[:4]:
@@ -41,8 +65,8 @@ def _update_crc16(crc: int, value: int) -> int:
     return crc
 
 
-def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
-    """Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
+def parse_hi91_sample(raw: bytes, host_receive_utc_ticks: int = 0) -> Hi13Sample | None:
+    """Decode every calibration-relevant field from a CRC-valid HI91 frame."""
 
     if len(raw) < 6 + 76:
         return None
@@ -54,12 +78,34 @@ def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[floa
     expected = raw[4] | (raw[5] << 8)
     if crc16_hi13(raw, payload_length) != expected:
         return None
-    device_ms = struct.unpack_from("<I", raw, 14)[0]
+    device_ms = int(struct.unpack_from("<I", raw, 14)[0])
     ax, ay, az = struct.unpack_from("<fff", raw, 18)
     gx, gy, gz = struct.unpack_from("<fff", raw, 30)
     gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
     accel = (ax * G0, ay * G0, az * G0)
-    return gyro, accel, int(device_ms)
+    return Hi13Sample(
+        t_s=float(device_ms) * 1e-3,
+        system_time_ms=device_ms,
+        device_timestamp_us=device_ms * 1000,
+        host_receive_utc_ticks=int(host_receive_utc_ticks),
+        gyro_rad_s=gyro,
+        accel_m_s2=accel,
+        rpy_deg=struct.unpack_from("<fff", raw, 54),
+        quaternion_wxyz=struct.unpack_from("<ffff", raw, 66),
+        mag_ut=struct.unpack_from("<fff", raw, 42),
+        pps_sync_stamp_ms=int(struct.unpack_from("<H", raw, 7)[0]),
+        temperature_c=int(struct.unpack_from("<b", raw, 9)[0]),
+        air_pressure_pa=float(struct.unpack_from("<f", raw, 10)[0]),
+    )
+
+
+def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
+    """Backward-compatible compact HI91 decoder."""
+
+    sample = parse_hi91_sample(raw)
+    if sample is None:
+        return None
+    return sample.gyro_rad_s, sample.accel_m_s2, sample.system_time_ms
 
 
 def iter_hi13_imu_samples(
@@ -67,14 +113,14 @@ def iter_hi13_imu_samples(
     *,
     host_utc_ticks_min: int | None = None,
     host_utc_ticks_max: int | None = None,
-) -> list[ImuSample]:
+) -> list[Hi13Sample]:
     """Return CRC-valid HI91 samples sorted by device timestamp.
 
     Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
     host UTC receive window before parsing.
     """
 
-    samples: list[ImuSample] = []
+    samples: list[Hi13Sample] = []
     carry = b""
     for chunk in capture.chunks:
         if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
@@ -102,25 +148,16 @@ def iter_hi13_imu_samples(
             if end > len(stream):
                 carry = stream[sync:]
                 break
-            parsed = parse_hi91_frame(stream[sync:end])
+            host_ticks = chunk.receive_utc_ticks
+            parsed = parse_hi91_sample(stream[sync:end], host_ticks)
             cursor = end
             if parsed is None:
                 continue
-            gyro, accel, device_ms = parsed
-            host_ticks = chunk.receive_utc_ticks
             if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
                 continue
             if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
                 continue
-            samples.append(
-                ImuSample(
-                    t_s=float(device_ms) * 1e-3,
-                    gyro_rad_s=gyro,
-                    accel_m_s2=accel,
-                    host_receive_utc_ticks=host_ticks,
-                    device_timestamp_us=int(device_ms) * 1000,
-                )
-            )
+            samples.append(parsed)
         else:
             carry = b""
     samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
@@ -129,8 +166,10 @@ def iter_hi13_imu_samples(
 
 __all__ = [
     "ImuSample",
+    "Hi13Sample",
     "crc16_hi13",
     "iter_hi13_imu_samples",
     "parse_hi91_frame",
+    "parse_hi91_sample",
     "samples_to_arrays",
 ]