107 lines
4.1 KiB
Python
107 lines
4.1 KiB
Python
from __future__ import annotations
|
|
|
|
import bisect
|
|
|
|
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("#UNIHEADINGA"):
|
|
row.update(parse_heading(line))
|
|
except ValueError as ex:
|
|
row["parse_error"] = str(ex)
|
|
rows.append(row)
|
|
return rows
|
|
|
|
|
|
def parse_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
|