lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 13:26:32 +08:00
co-authored by Cursor
parent 30f7e66db3
commit 2237be77a4
20 changed files with 1830 additions and 347 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/rscap_v2/h32_msop.py
+41 -6
View File
@@ -15,7 +15,7 @@ and horizontal channel offsets default to 0.
from __future__ import annotations
from dataclasses import dataclass
from typing import Iterable
from typing import Iterable, Sequence
import numpy as np
@@ -66,6 +66,8 @@ class LidarFrameExport:
t_start_s: float
t_end_s: float
points_xyz: np.ndarray # (N, 3) metres
host_receive_utc_ticks_start: int = 0
host_receive_utc_ticks_end: int = 0
def decode_packet_points(
@@ -144,6 +146,7 @@ def _block_points(
def iter_h32_frames_from_packets(
packets: Iterable[bytes],
*,
host_utc_ticks: Sequence[int] | None = None,
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
frame_stride: int = 1,
min_range_m: float = 0.3,
@@ -152,31 +155,49 @@ def iter_h32_frames_from_packets(
vertical_deg: np.ndarray | None = None,
horizontal_deg: np.ndarray | None = None,
) -> list[LidarFrameExport]:
"""Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap."""
"""Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap.
``host_utc_ticks`` is optional per-packet ``HostReceiveUtcTicks`` from the
MSOP DLog payload (UTC DateTime ticks). When provided, each emitted frame
carries host receive start/end ticks from the first/last contributing packet.
"""
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
packet_list = list(packets)
host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
if host_list is not None and len(host_list) != len(packet_list):
raise ValueError(
f"host_utc_ticks length {len(host_list)} != packets length {len(packet_list)}"
)
frames: list[LidarFrameExport] = []
point_chunks: list[np.ndarray] = []
t_start: float | None = None
t_end: float | None = None
host_start: int | None = None
host_end: int | None = None
prev_az: float | None = None
kept = 0
stride = max(1, int(frame_stride))
def emit() -> None:
nonlocal point_chunks, t_start, t_end, kept
nonlocal point_chunks, t_start, t_end, host_start, host_end, kept
if not point_chunks or t_start is None or t_end is None:
point_chunks = []
t_start = t_end = None
host_start = host_end = None
return
points = np.vstack(point_chunks)
point_chunks = []
start_s, end_s = t_start, t_end
h0 = int(host_start or 0)
h1 = int(host_end or 0)
t_start = t_end = None
host_start = host_end = None
if points.shape[0] < min_frame_points:
return
if kept % stride != 0:
@@ -188,12 +209,21 @@ def iter_h32_frames_from_packets(
points = points[select]
if end_s <= start_s:
end_s = start_s + 0.1
frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points))
frames.append(
LidarFrameExport(
t_start_s=start_s,
t_end_s=end_s,
points_xyz=points,
host_receive_utc_ticks_start=h0,
host_receive_utc_ticks_end=h1,
)
)
for packet in packets:
for index, packet in enumerate(packet_list):
if len(packet) != PACKET_LENGTH:
continue
packet_t = device_timestamp_ms(packet) * 1e-3
packet_host = int(host_list[index]) if host_list is not None else 0
unit = distance_unit_mm(packet)
idx = DATA_START
for _block in range(BLOCKS):
@@ -216,7 +246,9 @@ def iter_h32_frames_from_packets(
if pts.shape[0]:
if t_start is None:
t_start = packet_t
host_start = packet_host
t_end = packet_t
host_end = packet_host
point_chunks.append(pts)
idx += BLOCK_LENGTH
@@ -237,8 +269,11 @@ def iter_h32_frames(
) -> list[LidarFrameExport]:
"""Assemble MSOP packets from a V2 .rscap capture into frames."""
packets = [chunk.raw for chunk in capture.chunks]
host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks]
return iter_h32_frames_from_packets(
(chunk.raw for chunk in capture.chunks),
packets,
host_utc_ticks=host_ticks,
min_frame_points=min_frame_points,
frame_stride=frame_stride,
min_range_m=min_range_m,
tools/rscap_v2/hi13_imu.py
+136
View File
@@ -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",
]