新增 N300/H32 rscap 到 V1 中间格式的导出工具与单元测试

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

tests/test_export_rscap_helpers.py

@@ -0,0 +1,104 @@
+"""Unit tests for rscap → V1 export helpers (no large real captures)."""
+
+from __future__ import annotations
+
+import struct
+
+import numpy as np
+
+from tools.rscap_v2.h32_msop import (
+    CHANNELS,
+    PACKET_LENGTH,
+    decode_packet_points,
+    default_vertical_deg,
+    default_horizontal_deg,
+    device_timestamp_ms,
+    normalize_azimuth_deg,
+)
+from tools.rscap_v2.n300_imu import crc8_fdilink, crc16_fdilink, iter_n300_imu_samples
+from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk
+
+
+def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
+    packet = bytearray(PACKET_LENGTH)
+    packet[17] = 1  # 2.5 mm unit
+    sec = seconds.to_bytes(6, "big")
+    packet[20:26] = sec
+    packet[26:30] = int(microseconds).to_bytes(4, "big")
+    az_raw = int(round(az_deg * 100))
+    for block in range(12):
+        offset = 42 + block * 100
+        packet[offset] = 255
+        packet[offset + 1] = 238
+        packet[offset + 2] = (az_raw >> 8) & 0xFF
+        packet[offset + 3] = az_raw & 0xFF
+        idx = offset + 4
+        for _ch in range(CHANNELS):
+            # 4.0 m at 2.5 mm/unit => raw = 1600
+            packet[idx] = (1600 >> 8) & 0xFF
+            packet[idx + 1] = 1600 & 0xFF
+            packet[idx + 2] = 10
+            idx += 3
+    return bytes(packet)
+
+
+def test_h32_device_timestamp_and_points():
+    packet = _make_msop_packet(seconds=1700000000, microseconds=123000)
+    assert device_timestamp_ms(packet) == 1700000000 * 1000 + 123
+    az_list, pts = decode_packet_points(
+        packet,
+        default_vertical_deg(),
+        default_horizontal_deg(),
+        min_range_m=0.1,
+        max_range_m=50.0,
+    )
+    assert len(az_list) == 12
+    assert pts.shape[0] == 12 * CHANNELS
+    assert np.allclose(np.linalg.norm(pts, axis=1), 4.0, atol=1e-3)
+
+
+def test_normalize_azimuth():
+    assert abs(normalize_azimuth_deg(190.0) + 170.0) < 1e-9
+
+
+def _n300_imu_frame(device_us: int = 123456) -> bytes:
+    payload = bytearray(56)
+    struct.pack_into("<3f", payload, 0, 0.1, -0.2, 0.3)
+    struct.pack_into("<3f", payload, 12, 0.0, 0.0, 9.81)
+    struct.pack_into("<q", payload, 48, device_us)
+    header = bytearray([0xFC, 0x40, 56, 7])
+    header.append(crc8_fdilink(header))
+    crc = crc16_fdilink(payload)
+    frame = bytes(header) + crc.to_bytes(2, "big") + bytes(payload) + b"\xFD"
+    return frame
+
+
+def test_n300_imu_sample_from_capture_chunks():
+    frame = _n300_imu_frame(654321)
+    header = CaptureHeader(
+        sensor_kind="wheeltec-n300",
+        session_id="test",
+        session_start_utc_ticks=0,
+        session_start_monotonic_ticks=0,
+        monotonic_frequency=10_000_000,
+        port="COM1",
+        baud=921600,
+        file_start_utc_ticks=0,
+    )
+    chunk = RawChunk(
+        sequence=1,
+        receive_utc_ticks=100,
+        receive_monotonic_ticks=1,
+        raw=frame,
+        record_file_offset=0,
+        raw_file_offset=0,
+        record_crc32=0,
+        crc_valid=True,
+    )
+    capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
+    samples = iter_n300_imu_samples(capture)
+    assert len(samples) == 1
+    assert samples[0].device_timestamp_us == 654321
+    assert abs(samples[0].t_s - 654321e-6) < 1e-12
+    assert abs(samples[0].gyro_rad_s[0] - 0.1) < 1e-6
+    assert abs(samples[0].accel_m_s2[2] - 9.81) < 1e-5

tools/export_rscap_to_v1.py

@@ -0,0 +1,191 @@
+#!/usr/bin/env python3
+"""Export N300 IMU + H32 MSOP V2 .rscap files to Lidar-IMU V1 intermediate format.
+
+Output layout under --out:
+
+    imu.csv
+    lidar/
+      frames_index.csv
+      frames/frame_XXXXX.npz
+    export_summary.json
+
+Timestamps written into the intermediate format are **device times**
+(N300 device_timestamp_us, H32 MSOP device timestamp), not host receive time.
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import sys
+from pathlib import Path
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+    sys.path.insert(0, str(ROOT))
+
+from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
+from tools.rscap_v2.h32_msop import iter_h32_frames
+from tools.rscap_v2.n300_imu import iter_n300_imu_samples, samples_to_arrays
+
+
+def write_imu_csv(path: Path, t: np.ndarray, gyro: np.ndarray, accel: np.ndarray) -> None:
+    path.parent.mkdir(parents=True, exist_ok=True)
+    with path.open("w", newline="", encoding="utf-8") as handle:
+        writer = csv.writer(handle)
+        writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"])
+        for index in range(t.shape[0]):
+            writer.writerow(
+                [
+                    f"{t[index]:.9f}",
+                    f"{gyro[index, 0]:.12g}",
+                    f"{gyro[index, 1]:.12g}",
+                    f"{gyro[index, 2]:.12g}",
+                    f"{accel[index, 0]:.12g}",
+                    f"{accel[index, 1]:.12g}",
+                    f"{accel[index, 2]:.12g}",
+                ]
+            )
+
+
+def write_lidar_session(root: Path, frames) -> dict:
+    frames_dir = root / "frames"
+    frames_dir.mkdir(parents=True, exist_ok=True)
+    index_path = root / "frames_index.csv"
+    with index_path.open("w", newline="", encoding="utf-8") as handle:
+        writer = csv.writer(handle)
+        writer.writerow(["frame_id", "filename", "t_start", "t_end"])
+        point_counts = []
+        for index, frame in enumerate(frames):
+            rel = f"frames/frame_{index:05d}.npz"
+            np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32))
+            writer.writerow(
+                [
+                    index,
+                    rel,
+                    f"{frame.t_start_s:.9f}",
+                    f"{frame.t_end_s:.9f}",
+                ]
+            )
+            point_counts.append(int(frame.points_xyz.shape[0]))
+    return {
+        "frames": len(frames),
+        "points_min": int(min(point_counts)) if point_counts else 0,
+        "points_max": int(max(point_counts)) if point_counts else 0,
+        "points_mean": float(np.mean(point_counts)) if point_counts else 0.0,
+        "t_start": float(frames[0].t_start_s) if frames else None,
+        "t_end": float(frames[-1].t_end_s) if frames else None,
+    }
+
+
+def export_session(
+    *,
+    imu_rscap: Path,
+    lidar_rscap: Path,
+    out: Path,
+    frame_stride: int = 1,
+    max_points_per_frame: int | None = 80000,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+    min_frame_points: int = 100,
+) -> dict:
+    out.mkdir(parents=True, exist_ok=True)
+    imu_capture = read_capture(imu_rscap)
+    lidar_capture = read_capture(lidar_rscap)
+
+    samples = iter_n300_imu_samples(imu_capture)
+    t, gyro, accel = samples_to_arrays(samples)
+    imu_csv = out / "imu.csv"
+    write_imu_csv(imu_csv, t, gyro, accel)
+
+    frames = iter_h32_frames(
+        lidar_capture,
+        min_frame_points=min_frame_points,
+        frame_stride=frame_stride,
+        min_range_m=min_range_m,
+        max_range_m=max_range_m,
+        max_points_per_frame=max_points_per_frame,
+    )
+    lidar_dir = out / "lidar"
+    lidar_stats = write_lidar_session(lidar_dir, frames)
+
+    summary = {
+        "imu_rscap": str(imu_rscap),
+        "lidar_rscap": str(lidar_rscap),
+        "out": str(out),
+        "timestamp_policy": {
+            "imu": "n300_device_timestamp_us -> seconds",
+            "lidar": "h32_msop_device_timestamp_ms -> seconds (t_start/t_end per frame)",
+            "host_utc": "not used as calibration timeline",
+        },
+        "imu": {
+            "samples": int(t.shape[0]),
+            "t_start": float(t[0]) if t.size else None,
+            "t_end": float(t[-1]) if t.size else None,
+            "capture": file_summary(imu_capture),
+        },
+        "lidar": {
+            **lidar_stats,
+            "frame_stride": int(frame_stride),
+            "max_points_per_frame": max_points_per_frame,
+            "capture": file_summary(lidar_capture),
+            "angle_source": "default_msop_only_vertical_-16_to_16_deg",
+        },
+        "outputs": {
+            "imu_csv": str(imu_csv),
+            "lidar_session": str(lidar_dir),
+        },
+    }
+    (out / "export_summary.json").write_text(
+        json.dumps(summary, indent=2, ensure_ascii=False) + "\n",
+        encoding="utf-8",
+    )
+    return summary
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap")
+    parser.add_argument("--lidar-rscap", type=Path, required=True, help="H32 MSOP V2 .rscap")
+    parser.add_argument("--out", type=Path, required=True, help="Output session directory")
+    parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame")
+    parser.add_argument(
+        "--max-points-per-frame",
+        type=int,
+        default=80000,
+        help="Uniform downsample cap per frame; 0 disables",
+    )
+    parser.add_argument("--min-range-m", type=float, default=0.3)
+    parser.add_argument("--max-range-m", type=float, default=120.0)
+    parser.add_argument("--min-frame-points", type=int, default=100)
+    args = parser.parse_args()
+    max_points = None if args.max_points_per_frame <= 0 else args.max_points_per_frame
+    summary = export_session(
+        imu_rscap=args.imu_rscap,
+        lidar_rscap=args.lidar_rscap,
+        out=args.out,
+        frame_stride=args.frame_stride,
+        max_points_per_frame=max_points,
+        min_range_m=args.min_range_m,
+        max_range_m=args.max_range_m,
+        min_frame_points=args.min_frame_points,
+    )
+    print(json.dumps({
+        "imu_samples": summary["imu"]["samples"],
+        "lidar_frames": summary["lidar"]["frames"],
+        "imu_csv": summary["outputs"]["imu_csv"],
+        "lidar_session": summary["outputs"]["lidar_session"],
+        "export_summary": str(Path(args.out) / "export_summary.json"),
+    }, ensure_ascii=False, indent=2))
+    if summary["imu"]["samples"] == 0:
+        raise SystemExit("no valid N300 IMU samples decoded")
+    if summary["lidar"]["frames"] == 0:
+        raise SystemExit("no valid H32 frames decoded")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/rscap_v2/__init__.py

@@ -0,0 +1 @@
+"""V2 .rscap readers and sensor decoders for export to V1 intermediate format."""

tools/rscap_v2/capture_format_v2.py

@@ -0,0 +1,254 @@
+from __future__ import annotations
+
+import binascii
+import io
+import struct
+from dataclasses import dataclass, asdict
+from pathlib import Path
+from typing import BinaryIO, Iterator
+
+
+FILE_MAGIC = "RAW_SERIAL_CAPTURE_FILE_V2"
+RECORD_MAGIC = "RAW_SERIAL_RECORD_V2"
+FOOTER_MAGIC = "RAW_SERIAL_CAPTURE_FOOTER_V2"
+
+
+def read_7bit_int(stream: BinaryIO) -> int:
+    value = 0
+    shift = 0
+    while True:
+        raw = stream.read(1)
+        if not raw:
+            raise EOFError("truncated .NET string length")
+        value |= (raw[0] & 0x7F) << shift
+        if not raw[0] & 0x80:
+            return value
+        shift += 7
+        if shift > 35:
+            raise ValueError("invalid .NET string length")
+
+
+def read_dotnet_string(stream: BinaryIO) -> str:
+    length = read_7bit_int(stream)
+    raw = stream.read(length)
+    if len(raw) != length:
+        raise EOFError("truncated .NET string")
+    return raw.decode("utf-8")
+
+
+def read_i32(stream: BinaryIO) -> int:
+    raw = stream.read(4)
+    if len(raw) != 4:
+        raise EOFError("truncated int32")
+    return struct.unpack("<i", raw)[0]
+
+
+def read_i64(stream: BinaryIO) -> int:
+    raw = stream.read(8)
+    if len(raw) != 8:
+        raise EOFError("truncated int64")
+    return struct.unpack("<q", raw)[0]
+
+
+def read_u32(stream: BinaryIO) -> int:
+    raw = stream.read(4)
+    if len(raw) != 4:
+        raise EOFError("truncated uint32")
+    return struct.unpack("<I", raw)[0]
+
+
+@dataclass(frozen=True)
+class CaptureHeader:
+    sensor_kind: str
+    session_id: str
+    session_start_utc_ticks: int
+    session_start_monotonic_ticks: int
+    monotonic_frequency: int
+    port: str
+    baud: int
+    file_start_utc_ticks: int
+
+
+@dataclass(frozen=True)
+class RawChunk:
+    sequence: int
+    receive_utc_ticks: int
+    receive_monotonic_ticks: int
+    raw: bytes
+    record_file_offset: int
+    raw_file_offset: int
+    record_crc32: int
+    crc_valid: bool
+
+
+@dataclass(frozen=True)
+class CaptureFooter:
+    clean_close: bool
+    records: int
+    bytes: int
+    first_sequence: int
+    last_sequence: int
+    dropped_chunks: int
+    dropped_bytes: int
+    crc_valid: bool
+
+
+@dataclass
+class CaptureFile:
+    path: str
+    header: CaptureHeader
+    chunks: list[RawChunk]
+    footer: CaptureFooter | None
+    truncated_tail: bool = False
+
+
+def read_header(stream: BinaryIO) -> CaptureHeader:
+    if read_dotnet_string(stream) != FILE_MAGIC:
+        raise ValueError("not a V2 raw capture file")
+    version = read_i32(stream)
+    if version != 2:
+        raise ValueError(f"unsupported capture version: {version}")
+    return CaptureHeader(
+        sensor_kind=read_dotnet_string(stream),
+        session_id=read_dotnet_string(stream),
+        session_start_utc_ticks=read_i64(stream),
+        session_start_monotonic_ticks=read_i64(stream),
+        monotonic_frequency=read_i64(stream),
+        port=read_dotnet_string(stream),
+        baud=read_i32(stream),
+        file_start_utc_ticks=read_i64(stream),
+    )
+
+
+def parse_record_body(body: bytes, record_file_offset: int, record_crc: int) -> RawChunk:
+    stream = io.BytesIO(body)
+    if read_dotnet_string(stream) != RECORD_MAGIC:
+        raise ValueError("invalid record magic")
+    sequence = read_i64(stream)
+    receive_utc_ticks = read_i64(stream)
+    receive_monotonic_ticks = read_i64(stream)
+    raw_length = read_i32(stream)
+    if raw_length < 0 or raw_length > 64 * 1024 * 1024:
+        raise ValueError(f"invalid raw length: {raw_length}")
+    raw_offset = record_file_offset + 4 + stream.tell()
+    raw = stream.read(raw_length)
+    if len(raw) != raw_length:
+        raise EOFError("truncated raw bytes")
+    crc_valid = (binascii.crc32(body) & 0xFFFFFFFF) == record_crc
+    return RawChunk(
+        sequence=sequence,
+        receive_utc_ticks=receive_utc_ticks,
+        receive_monotonic_ticks=receive_monotonic_ticks,
+        raw=raw,
+        record_file_offset=record_file_offset,
+        raw_file_offset=raw_offset,
+        record_crc32=record_crc,
+        crc_valid=crc_valid,
+    )
+
+
+def parse_footer(body: bytes, expected_crc: int) -> CaptureFooter:
+    stream = io.BytesIO(body)
+    if read_dotnet_string(stream) != FOOTER_MAGIC:
+        raise ValueError("invalid footer magic")
+    clean_close = stream.read(1) == b"\x01"
+    records = read_i64(stream)
+    raw_bytes = read_i64(stream)
+    first_sequence = read_i64(stream)
+    last_sequence = read_i64(stream)
+    dropped_chunks = read_i64(stream)
+    dropped_bytes = read_i64(stream)
+    return CaptureFooter(
+        clean_close=clean_close,
+        records=records,
+        bytes=raw_bytes,
+        first_sequence=first_sequence,
+        last_sequence=last_sequence,
+        dropped_chunks=dropped_chunks,
+        dropped_bytes=dropped_bytes,
+        crc_valid=(binascii.crc32(body) & 0xFFFFFFFF) == expected_crc,
+    )
+
+
+def read_capture(path: Path) -> CaptureFile:
+    chunks: list[RawChunk] = []
+    footer = None
+    truncated = False
+    with path.open("rb") as stream:
+        header = read_header(stream)
+        while True:
+            record_offset = stream.tell()
+            length_raw = stream.read(4)
+            if not length_raw:
+                break
+            if len(length_raw) != 4:
+                truncated = True
+                break
+            length = struct.unpack("<i", length_raw)[0]
+            try:
+                if length == -1:
+                    footer_length = read_i32(stream)
+                    if footer_length < 0 or footer_length > 1024 * 1024:
+                        raise ValueError("invalid footer length")
+                    footer_body = stream.read(footer_length)
+                    if len(footer_body) != footer_length:
+                        raise EOFError("truncated footer")
+                    footer = parse_footer(footer_body, read_u32(stream))
+                    break
+                if length <= 0 or length > 64 * 1024 * 1024:
+                    raise ValueError("invalid record length")
+                body = stream.read(length)
+                if len(body) != length:
+                    raise EOFError("truncated record body")
+                record_crc = read_u32(stream)
+                chunks.append(parse_record_body(body, record_offset, record_crc))
+            except (EOFError, ValueError):
+                truncated = True
+                break
+    return CaptureFile(str(path), header, chunks, footer, truncated)
+
+
+def sequence_gaps(chunks: list[RawChunk]) -> list[tuple[int, int, int]]:
+    result = []
+    for previous, current in zip(chunks, chunks[1:]):
+        if current.sequence > previous.sequence + 1:
+            result.append((previous.sequence, current.sequence, current.sequence - previous.sequence - 1))
+    return result
+
+
+def file_summary(capture: CaptureFile) -> dict:
+    gaps = sequence_gaps(capture.chunks)
+    sequences = [chunk.sequence for chunk in capture.chunks]
+    return {
+        "path": capture.path,
+        "sensor": capture.header.sensor_kind,
+        "session_id": capture.header.session_id,
+        "port": capture.header.port,
+        "baud": capture.header.baud,
+        "chunks_read": len(capture.chunks),
+        "bytes_read": sum(len(chunk.raw) for chunk in capture.chunks),
+        "first_sequence": sequences[0] if sequences else None,
+        "last_sequence": sequences[-1] if sequences else None,
+        "missing_chunks": sum(gap[2] for gap in gaps),
+        "gap_count": len(gaps),
+        "bad_record_crc": sum(not chunk.crc_valid for chunk in capture.chunks),
+        "truncated_tail": capture.truncated_tail,
+        "footer": None if capture.footer is None else asdict(capture.footer),
+        "gaps": gaps[:100],
+    }
+
+
+def iter_contiguous_segments(chunks: list[RawChunk]) -> Iterator[tuple[int, list[RawChunk]]]:
+    if not chunks:
+        return
+    segment_id = 0
+    current = [chunks[0]]
+    for previous, chunk in zip(chunks, chunks[1:]):
+        if chunk.sequence != previous.sequence + 1:
+            yield segment_id, current
+            segment_id += 1
+            current = [chunk]
+        else:
+            current.append(chunk)
+    yield segment_id, current
+

tools/rscap_v2/h32_msop.py

@@ -0,0 +1,224 @@
+"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
+
+Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``:
+azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
+distance_mm = raw * distance_unit_mm, then:
+
+    x = d_m * cos(alt) * cos(az)
+    y = d_m * cos(alt) * sin(az)
+    z = d_m * sin(alt)
+
+MSOP-only captures do not include DIFOP; vertical angles default to a uniform
+-16°…+16° fan, horizontal channel offsets default to 0.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from .capture_format_v2 import CaptureFile
+
+PACKET_LENGTH = 1248
+DATA_START = 42
+BLOCKS = 12
+BLOCK_LENGTH = 100
+CHANNELS = 32
+MIN_FRAME_POINTS_DEFAULT = 100
+
+
+def default_vertical_deg() -> np.ndarray:
+    return -16.0 + np.arange(CHANNELS, dtype=np.float64) * (32.0 / (CHANNELS - 1))
+
+
+def default_horizontal_deg() -> np.ndarray:
+    return np.zeros(CHANNELS, dtype=np.float64)
+
+
+def read_u16_be(packet: bytes, index: int) -> int:
+    return (packet[index] << 8) | packet[index + 1]
+
+
+def device_timestamp_ms(packet: bytes) -> int:
+    seconds = int.from_bytes(packet[20:26], "big")
+    microseconds = int.from_bytes(packet[26:30], "big")
+    return seconds * 1000 + microseconds // 1000
+
+
+def distance_unit_mm(packet: bytes, *, auto: bool = True, fallback: float = 2.5) -> float:
+    if not auto:
+        return float(fallback)
+    return 2.5 if packet[17] == 1 else 0.5
+
+
+def normalize_azimuth_deg(angle: float) -> float:
+    while angle > 180.0:
+        angle -= 360.0
+    while angle < -180.0:
+        angle += 360.0
+    return angle
+
+
+@dataclass
+class LidarFrameExport:
+    t_start_s: float
+    t_end_s: float
+    points_xyz: np.ndarray  # (N, 3) metres
+
+
+def decode_packet_points(
+    packet: bytes,
+    vertical_deg: np.ndarray,
+    horizontal_deg: np.ndarray,
+    *,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+) -> tuple[list[float], np.ndarray]:
+    """Decode one MSOP packet into block azimuths and concatenated XYZ points."""
+
+    if len(packet) != PACKET_LENGTH:
+        return [], np.zeros((0, 3), dtype=np.float64)
+    unit = distance_unit_mm(packet)
+    az_list: list[float] = []
+    chunks: list[np.ndarray] = []
+    idx = DATA_START
+    for _block in range(BLOCKS):
+        if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
+            break
+        az = read_u16_be(packet, idx + 2) * 0.01
+        az_list.append(az)
+        pts = _block_points(
+            packet,
+            idx,
+            az,
+            unit,
+            vertical_deg,
+            horizontal_deg,
+            min_range_m=min_range_m,
+            max_range_m=max_range_m,
+        )
+        if pts.shape[0]:
+            chunks.append(pts)
+        idx += BLOCK_LENGTH
+    if not chunks:
+        return az_list, np.zeros((0, 3), dtype=np.float64)
+    return az_list, np.vstack(chunks)
+
+
+def _block_points(
+    packet: bytes,
+    block_offset: int,
+    az_deg: float,
+    unit_mm: float,
+    vertical_deg: np.ndarray,
+    horizontal_deg: np.ndarray,
+    *,
+    min_range_m: float,
+    max_range_m: float,
+) -> np.ndarray:
+    xs: list[float] = []
+    ys: list[float] = []
+    zs: list[float] = []
+    idx = block_offset + 4  # after FF EE + azimuth
+    for ch in range(CHANNELS):
+        raw = read_u16_be(packet, idx)
+        idx += 3
+        if raw == 0:
+            continue
+        d_m = (raw * unit_mm) * 0.001
+        if d_m < min_range_m or d_m > max_range_m:
+            continue
+        az_ch = np.deg2rad(normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch]))))
+        alt = np.deg2rad(float(vertical_deg[ch]))
+        cos_alt = np.cos(alt)
+        xs.append(d_m * cos_alt * np.cos(az_ch))
+        ys.append(d_m * cos_alt * np.sin(az_ch))
+        zs.append(d_m * np.sin(alt))
+    if not xs:
+        return np.zeros((0, 3), dtype=np.float64)
+    return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False)
+
+
+def iter_h32_frames(
+    capture: CaptureFile,
+    *,
+    min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
+    frame_stride: int = 1,
+    min_range_m: float = 0.3,
+    max_range_m: float = 120.0,
+    max_points_per_frame: int | None = None,
+    vertical_deg: np.ndarray | None = None,
+    horizontal_deg: np.ndarray | None = None,
+) -> list[LidarFrameExport]:
+    """Assemble MSOP packets into frames using the 270°→90° azimuth wrap."""
+
+    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},)")
+
+    frames: list[LidarFrameExport] = []
+    point_chunks: list[np.ndarray] = []
+    t_start: float | None = None
+    t_end: float | 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
+        if not point_chunks or t_start is None or t_end is None:
+            point_chunks = []
+            t_start = t_end = None
+            return
+        points = np.vstack(point_chunks)
+        point_chunks = []
+        start_s, end_s = t_start, t_end
+        t_start = t_end = None
+        if points.shape[0] < min_frame_points:
+            return
+        if kept % stride != 0:
+            kept += 1
+            return
+        kept += 1
+        if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
+            select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
+            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))
+
+    for chunk in capture.chunks:
+        packet = chunk.raw
+        if len(packet) != PACKET_LENGTH:
+            continue
+        packet_t = device_timestamp_ms(packet) * 1e-3
+        unit = distance_unit_mm(packet)
+        idx = DATA_START
+        for _block in range(BLOCKS):
+            if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
+                break
+            az = read_u16_be(packet, idx + 2) * 0.01
+            if prev_az is not None and prev_az > 270.0 and az < 90.0:
+                emit()
+            prev_az = az
+            pts = _block_points(
+                packet,
+                idx,
+                az,
+                unit,
+                vertical,
+                horizontal,
+                min_range_m=min_range_m,
+                max_range_m=max_range_m,
+            )
+            if pts.shape[0]:
+                if t_start is None:
+                    t_start = packet_t
+                t_end = packet_t
+                point_chunks.append(pts)
+            idx += BLOCK_LENGTH
+
+    emit()
+    return frames

tools/rscap_v2/n300_imu.py

@@ -0,0 +1,113 @@
+"""Decode Wheeltec N300 FDILink IMU frames from a V2 .rscap capture."""
+
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+
+import numpy as np
+
+from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
+
+
+@dataclass(frozen=True)
+class ImuSample:
+    t_s: float
+    gyro_rad_s: tuple[float, float, float]
+    accel_m_s2: tuple[float, float, float]
+    host_receive_utc_ticks: int
+    device_timestamp_us: int
+
+
+def crc8_fdilink(data: bytes) -> int:
+    crc = 0
+    for value in data:
+        crc ^= value
+        for _ in range(8):
+            crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF
+    return crc
+
+
+def crc16_fdilink(data: bytes) -> int:
+    crc = 0
+    for value in data:
+        crc ^= value << 8
+        for _ in range(8):
+            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
+    return crc
+
+
+def _host_ticks_for_span(chunks: list[RawChunk], start: int, end: int) -> int:
+    stream_offset = 0
+    last = chunks[0]
+    for chunk in chunks:
+        next_offset = stream_offset + len(chunk.raw)
+        if start < next_offset and end > stream_offset:
+            last = chunk
+        stream_offset = next_offset
+    return last.receive_utc_ticks
+
+
+def iter_n300_imu_samples(capture: CaptureFile) -> list[ImuSample]:
+    """Return CRC-valid MSG_IMU (0x40) samples sorted by device timestamp."""
+
+    samples: list[ImuSample] = []
+    expected_lengths = {0x40: 56, 0x41: 48}
+    for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
+        stream = b"".join(chunk.raw for chunk in chunks)
+        cursor = 0
+        while cursor < len(stream):
+            start = stream.find(b"\xFC", cursor)
+            if start < 0:
+                break
+            if start + 8 > len(stream):
+                break
+            payload_length = stream[start + 2]
+            end = start + payload_length + 8
+            if end > len(stream):
+                if stream.find(b"\xFC", start + 1) < 0:
+                    break
+                cursor = start + 1
+                continue
+            frame = stream[start:end]
+            if frame[-1] != 0xFD:
+                cursor = start + 1
+                continue
+            packet_id = frame[1]
+            payload = frame[7:-1]
+            header_ok = crc8_fdilink(frame[:4]) == frame[4]
+            payload_ok = crc16_fdilink(payload) == int.from_bytes(frame[5:7], "big")
+            expected = expected_lengths.get(packet_id)
+            length_ok = expected is None or len(payload) == expected
+            if not (header_ok and payload_ok and length_ok):
+                cursor = start + 1
+                continue
+            if packet_id == 0x40:
+                gyro = struct.unpack_from("<3f", payload, 0)
+                accel = struct.unpack_from("<3f", payload, 12)
+                device_us = struct.unpack_from("<q", payload, 48)[0]
+                samples.append(
+                    ImuSample(
+                        t_s=float(device_us) * 1e-6,
+                        gyro_rad_s=gyro,
+                        accel_m_s2=accel,
+                        host_receive_utc_ticks=_host_ticks_for_span(chunks, start, end),
+                        device_timestamp_us=int(device_us),
+                    )
+                )
+            cursor = end
+    samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
+    return samples
+
+
+def samples_to_arrays(samples: list[ImuSample]) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+    if not samples:
+        return (
+            np.zeros(0, dtype=np.float64),
+            np.zeros((0, 3), dtype=np.float64),
+            np.zeros((0, 3), dtype=np.float64),
+        )
+    t = np.asarray([sample.t_s for sample in samples], dtype=np.float64)
+    gyro = np.asarray([sample.gyro_rad_s for sample in samples], dtype=np.float64)
+    accel = np.asarray([sample.accel_m_s2 for sample in samples], dtype=np.float64)
+    return t, gyro, accel