新增原始数据一步导出到 combined：对齐 Lidar-IMU 导出入口，适配 H32/G90/N300
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,371 @@
|
||||
"""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
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def ticks_to_unix_ns(ticks: int) -> int:
|
||||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 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
|
||||
|
||||
|
||||
@dataclass
|
||||
class LidarFramePolarExport:
|
||||
"""One H32 frame in the calibration ``points_raw`` polar contract.
|
||||
|
||||
Columns: ``d_mm, azimuth_deg, altitude_deg, intensity, progression``.
|
||||
Azimuth already includes the H32 channel horizontal offset and sign flip so
|
||||
``rigorous_calibration.load_npz_xyz`` reproduces the same Cartesian points.
|
||||
"""
|
||||
|
||||
t_start_s: float
|
||||
t_end_s: float
|
||||
points_raw: np.ndarray # (N, 5) float32
|
||||
host_receive_utc_ns: int
|
||||
|
||||
|
||||
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 _block_points_raw(
|
||||
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:
|
||||
"""Return polar ``points_raw`` rows compatible with ``load_npz_xyz``."""
|
||||
|
||||
rows: list[list[float]] = []
|
||||
idx = block_offset + 4
|
||||
for ch in range(CHANNELS):
|
||||
raw = read_u16_be(packet, idx)
|
||||
intensity = float(packet[idx + 2])
|
||||
idx += 3
|
||||
if raw == 0:
|
||||
continue
|
||||
d_mm = float(raw) * unit_mm
|
||||
d_m = d_mm * 0.001
|
||||
if d_m < min_range_m or d_m > max_range_m:
|
||||
continue
|
||||
az_ch = normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch])))
|
||||
rows.append([d_mm, az_ch, float(vertical_deg[ch]), intensity, float(ch)])
|
||||
if not rows:
|
||||
return np.zeros((0, 5), dtype=np.float32)
|
||||
return np.asarray(rows, dtype=np.float32)
|
||||
|
||||
|
||||
def iter_h32_frames_polar(
|
||||
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[LidarFramePolarExport]:
|
||||
"""Assemble MSOP packets into polar frames for the RTK–LiDAR combined contract."""
|
||||
|
||||
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[LidarFramePolarExport] = []
|
||||
point_chunks: list[np.ndarray] = []
|
||||
t_start: float | None = None
|
||||
t_end: float | None = None
|
||||
host_ns = 0
|
||||
prev_az: float | None = None
|
||||
kept = 0
|
||||
stride = max(1, int(frame_stride))
|
||||
|
||||
def emit() -> None:
|
||||
nonlocal point_chunks, t_start, t_end, host_ns, 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
|
||||
frame_host = host_ns
|
||||
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(
|
||||
LidarFramePolarExport(
|
||||
t_start_s=start_s,
|
||||
t_end_s=end_s,
|
||||
points_raw=points.astype(np.float32, copy=False),
|
||||
host_receive_utc_ns=int(frame_host),
|
||||
)
|
||||
)
|
||||
|
||||
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)
|
||||
chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks)
|
||||
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_raw(
|
||||
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
|
||||
host_ns = chunk_host
|
||||
point_chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
|
||||
emit()
|
||||
return frames
|
||||
|
||||
|
||||
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
|
||||
@@ -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
|
||||
@@ -131,6 +131,39 @@ def parse_heading(line: str) -> dict:
|
||||
}
|
||||
|
||||
|
||||
def parse_pvtslna(line: str) -> dict:
|
||||
"""Parse Unicore/G90 ``#PVTSLNA`` into GGA-compatible position fields.
|
||||
|
||||
``fix_quality`` is synthesized as 4 when checksum-valid coordinates exist so
|
||||
the existing prepare gate (accepted fixes {4,5}) keeps working. Position
|
||||
stddevs are retained for audits.
|
||||
"""
|
||||
star = line.rfind("*")
|
||||
fields = line[1:star if star >= 0 else None].split(",")
|
||||
if len(fields) < 16:
|
||||
raise ValueError("PVTSLNA has too few fields")
|
||||
tow = safe_float(fields[5])
|
||||
return {
|
||||
"type": "PVTSLNA",
|
||||
"gnss_week": safe_int(fields[4]),
|
||||
"gnss_tow_ms": int(tow) if tow is not None else None,
|
||||
"altitude_m": safe_float(fields[10]),
|
||||
"lat_deg": safe_float(fields[11]),
|
||||
"lon_deg": safe_float(fields[12]),
|
||||
"height_std_m": safe_float(fields[13]),
|
||||
"latitude_std_m": safe_float(fields[14]),
|
||||
"longitude_std_m": safe_float(fields[15]),
|
||||
# Downstream prepare still filters on NMEA-style fix quality.
|
||||
"fix_quality": 4,
|
||||
"satellites": -1,
|
||||
"hdop": None,
|
||||
"differential_age_s": None,
|
||||
"position_time_utc": "",
|
||||
"geoid_separation_m": None,
|
||||
"station_id": "",
|
||||
}
|
||||
|
||||
|
||||
def chunk_source(chunks: list[RawChunk], offset: int, end: int) -> dict:
|
||||
first = chunks[0]
|
||||
last = chunks[-1]
|
||||
@@ -184,6 +217,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:
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user