新增原始数据一步导出到 combined：对齐 Lidar-IMU 导出入口，适配 H32/G90/N300

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

tools/rscap_v2/pipeline_common_corrected.py

@@ -1,6 +1,7 @@
 from __future__ import annotations
 
 import bisect
+import struct
 
 from pipeline_common import *
 from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
@@ -39,6 +40,7 @@ def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: in
         "host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks,
     }
 
+
 def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
     rows = []
     for segment_id, chunks in iter_contiguous_segments(capture.chunks):
@@ -60,6 +62,8 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
             try:
                 if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
                     row.update(parse_gga(line))
+                elif line.startswith("#PVTSLNA"):
+                    row.update(parse_pvtslna(line))
                 elif line.startswith("#UNIHEADINGA"):
                     row.update(parse_heading(line))
             except ValueError as ex:
@@ -68,7 +72,103 @@ def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
     return rows
 
 
-def parse_imu_capture(capture: CaptureFile) -> list[dict]:
+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 parse_n300_imu_capture(capture: CaptureFile) -> list[dict]:
+    """Parse Wheeltec N300 FDILink IMU frames; normalize to HI13-like keys."""
+
+    rows = []
+    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
+            row = {
+                "type": "N300",
+                "tag": int(packet_id),
+                "frame_length": len(frame),
+                "crc_valid": bool(header_ok and payload_ok and length_ok),
+                "raw_frame_hex": frame.hex(),
+            }
+            row.update(source_for_span(chunks, start, end, segment_id))
+            if row["crc_valid"] and packet_id == 0x40:
+                try:
+                    gyro = struct.unpack_from("<3f", payload, 0)
+                    accel = struct.unpack_from("<3f", payload, 12)
+                    device_us = struct.unpack_from("<q", payload, 48)[0]
+                    row.update(
+                        {
+                            "device_timestamp_us": int(device_us),
+                            # build_multisensor_npz.estimate_imu_times uses ms.
+                            "device_timestamp_ms": int(device_us) // 1000,
+                            "gyro_x_radps": gyro[0],
+                            "gyro_y_radps": gyro[1],
+                            "gyro_z_radps": gyro[2],
+                            "accel_x_mps2": accel[0],
+                            "accel_y_mps2": accel[1],
+                            "accel_z_mps2": accel[2],
+                            "pps_sync_stamp_ms": -1,
+                        }
+                    )
+                except (IndexError, struct.error, ValueError) as ex:
+                    row["parse_error"] = str(ex)
+                    row["crc_valid"] = False
+            elif row["crc_valid"] and packet_id == 0x41:
+                try:
+                    device_us = struct.unpack_from("<q", payload, 40)[0]
+                    row.update(
+                        {
+                            "device_timestamp_us": int(device_us),
+                            "device_timestamp_ms": int(device_us) // 1000,
+                            "pps_sync_stamp_ms": -1,
+                        }
+                    )
+                except (IndexError, struct.error, ValueError) as ex:
+                    row["parse_error"] = str(ex)
+            rows.append(row)
+            cursor = end if row["crc_valid"] else start + 1
+    return rows
+
+
+def parse_hi13_imu_capture(capture: CaptureFile) -> list[dict]:
     rows = []
     for segment_id, chunks in iter_contiguous_segments(capture.chunks):
         stream = b"".join(chunk.raw for chunk in chunks)
@@ -104,3 +204,12 @@ def parse_imu_capture(capture: CaptureFile) -> list[dict]:
             rows.append(row)
             cursor = end
     return rows
+
+
+def parse_imu_capture(capture: CaptureFile) -> list[dict]:
+    """Prefer N300 FDILink when present; fall back to legacy HI13."""
+
+    n300 = parse_n300_imu_capture(capture)
+    if any(row.get("crc_valid") and row.get("type") == "N300" for row in n300):
+        return n300
+    return parse_hi13_imu_capture(capture)