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

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-03 17:27:00 +08:00
parent e50a79b114
commit ea06a3a523
6 changed files with 887 additions and 0 deletions
@@ -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