216 lines
8.3 KiB
Python
216 lines
8.3 KiB
Python
from __future__ import annotations
|
|
|
|
import bisect
|
|
import struct
|
|
|
|
from pipeline_common import *
|
|
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
|
|
|
|
|
_SPAN_CACHE: dict[int, tuple[list[RawChunk], list[int]]] = {}
|
|
|
|
|
|
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
|
key = id(chunks)
|
|
cached = _SPAN_CACHE.get(key)
|
|
if cached is not None and cached[0] is chunks:
|
|
return cached[1]
|
|
starts = []
|
|
cursor = 0
|
|
for chunk in chunks:
|
|
starts.append(cursor)
|
|
cursor += len(chunk.raw)
|
|
_SPAN_CACHE[key] = (chunks, starts)
|
|
return starts
|
|
|
|
|
|
def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: int) -> dict:
|
|
starts = _chunk_starts(chunks)
|
|
start_index = max(0, min(len(chunks) - 1, bisect.bisect_right(starts, start) - 1))
|
|
end_index = max(start_index, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
|
start_chunk = chunks[start_index]
|
|
end_chunk = chunks[end_index]
|
|
return {
|
|
"source_segment_id": segment_id,
|
|
"source_chunk_sequence_first": start_chunk.sequence,
|
|
"source_chunk_sequence_last": end_chunk.sequence,
|
|
"source_raw_file_offset": start_chunk.raw_file_offset + (start - starts[start_index]),
|
|
"source_raw_byte_length": end - start,
|
|
"host_receive_utc_ns": ticks_to_unix_ns(end_chunk.receive_utc_ticks),
|
|
"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):
|
|
stream = b"".join(chunk.raw for chunk in chunks)
|
|
cursor = 0
|
|
while cursor < len(stream):
|
|
newline = stream.find(b"\n", cursor)
|
|
if newline < 0:
|
|
break
|
|
end = newline + 1
|
|
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
|
start = cursor
|
|
cursor = end
|
|
if not raw_line:
|
|
continue
|
|
line = raw_line.decode("ascii", "replace")
|
|
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
|
row.update(source_for_span(chunks, start, end, segment_id))
|
|
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:
|
|
row["parse_error"] = str(ex)
|
|
rows.append(row)
|
|
return rows
|
|
|
|
|
|
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)
|
|
cursor = 0
|
|
while True:
|
|
start = stream.find(b"\x5a\xa5", cursor)
|
|
if start < 0 or start + 6 > len(stream):
|
|
break
|
|
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
|
frame_length = 6 + payload_length
|
|
if payload_length <= 0 or payload_length > 512:
|
|
cursor = start + 1
|
|
continue
|
|
if start + frame_length > len(stream):
|
|
break
|
|
frame = stream[start:start + frame_length]
|
|
expected = int.from_bytes(frame[4:6], "little")
|
|
actual = crc16_hi13(frame[:4] + frame[6:])
|
|
end = start + frame_length
|
|
row = {
|
|
"type": "HI13",
|
|
"tag": frame[6],
|
|
"frame_length": frame_length,
|
|
"crc_valid": expected == actual,
|
|
"raw_frame_hex": frame.hex(),
|
|
}
|
|
row.update(source_for_span(chunks, start, end, segment_id))
|
|
if row["crc_valid"]:
|
|
try:
|
|
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
|
except (IndexError, struct.error, ValueError) as ex:
|
|
row["parse_error"] = str(ex)
|
|
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)
|