支持 HI13/H32 主机 UTC 桥接对齐、多会话联合标定与 CAD 平移先验。

Co-authored-by: Cursor <cursoragent@cursor.com>

tools/rscap_v2/hi13_imu.py

@@ -0,0 +1,136 @@
+"""Decode Hipnuc / HI13 (HI91/HI92) IMU frames from a V2 .rscap capture.
+
+Matches ``EcarSensorMinimal/RawSerialImu/Hi13Protocol.cs``:
+sync ``5A A5``, CRC16 over header[0:4]+payload, tag ``0x91`` / ``0x92``.
+
+HI91 (preferred for calibration):
+- accel: float32 in g → m/s² (* 9.80665)
+- gyro: float32 in deg/s → rad/s
+- device time: uint32 ms at frame offset 14 → ``t_s = ms * 1e-3``
+"""
+
+from __future__ import annotations
+
+import struct
+
+import numpy as np
+
+from .capture_format_v2 import CaptureFile
+from .n300_imu import ImuSample, samples_to_arrays
+
+G0 = 9.80665
+DEG2RAD = np.pi / 180.0
+
+
+def crc16_hi13(frame: bytes, payload_length: int) -> int:
+    crc = 0
+    for value in frame[:4]:
+        crc = _update_crc16(crc, value)
+    for value in frame[6 : 6 + payload_length]:
+        crc = _update_crc16(crc, value)
+    return crc & 0xFFFF
+
+
+def _update_crc16(crc: int, value: int) -> int:
+    crc ^= (value & 0xFF) << 8
+    for _ in range(8):
+        if crc & 0x8000:
+            crc = ((crc << 1) ^ 0x1021) & 0xFFFF
+        else:
+            crc = (crc << 1) & 0xFFFF
+    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."""
+
+    if len(raw) < 6 + 76:
+        return None
+    payload_length = raw[2] | (raw[3] << 8)
+    if payload_length < 76 or len(raw) < 6 + payload_length:
+        return None
+    if raw[6] != 0x91:
+        return None
+    expected = raw[4] | (raw[5] << 8)
+    if crc16_hi13(raw, payload_length) != expected:
+        return None
+    device_ms = 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)
+
+
+def iter_hi13_imu_samples(
+    capture: CaptureFile,
+    *,
+    host_utc_ticks_min: int | None = None,
+    host_utc_ticks_max: int | None = None,
+) -> list[ImuSample]:
+    """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] = []
+    carry = b""
+    for chunk in capture.chunks:
+        if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
+            carry = b""
+            continue
+        if host_utc_ticks_max is not None and chunk.receive_utc_ticks > host_utc_ticks_max:
+            # chunks are time-ordered; remaining ones are later
+            if chunk.receive_utc_ticks > host_utc_ticks_max:
+                break
+        stream = carry + chunk.raw
+        cursor = 0
+        while cursor + 6 < len(stream):
+            sync = stream.find(b"\x5A\xA5", cursor)
+            if sync < 0:
+                carry = b""
+                break
+            if sync + 6 > len(stream):
+                carry = stream[sync:]
+                break
+            payload_length = stream[sync + 2] | (stream[sync + 3] << 8)
+            if payload_length < 1 or payload_length > 512:
+                cursor = sync + 1
+                continue
+            end = sync + 6 + payload_length
+            if end > len(stream):
+                carry = stream[sync:]
+                break
+            parsed = parse_hi91_frame(stream[sync:end])
+            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,
+                )
+            )
+        else:
+            carry = b""
+    samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
+    return samples
+
+
+__all__ = [
+    "ImuSample",
+    "crc16_hi13",
+    "iter_hi13_imu_samples",
+    "parse_hi91_frame",
+    "samples_to_arrays",
+]