253 lines
7.6 KiB
Python
253 lines
7.6 KiB
Python
"""Parser and CSV renderer for the Trikke TRK1 binary telemetry stream."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import struct
|
|
import zlib
|
|
from dataclasses import dataclass
|
|
|
|
MAGIC = b"TRK1"
|
|
VERSION = 1
|
|
HEADER_SIZE = 36
|
|
SAMPLE_RECORD_SIZE = 20
|
|
METADATA_SIZE = 48
|
|
MAX_RECORDS = 8
|
|
|
|
PACKET_TYPE_METADATA = 1
|
|
PACKET_TYPE_SAMPLES = 2
|
|
|
|
HEADER = struct.Struct("<4sBBBBBBHIQIII")
|
|
METADATA = struct.Struct("<HHHH10f")
|
|
SAMPLE = struct.Struct("<IHhhhhhhBB")
|
|
|
|
CSV_COLUMNS = [
|
|
"sequence",
|
|
"poll_timestamp_us",
|
|
"accel_x_raw",
|
|
"accel_y_raw",
|
|
"accel_z_raw",
|
|
"gyro_x_raw",
|
|
"gyro_y_raw",
|
|
"gyro_z_raw",
|
|
"accel_x_mg",
|
|
"accel_y_mg",
|
|
"accel_z_mg",
|
|
"gyro_x_mdps",
|
|
"gyro_y_mdps",
|
|
"gyro_z_mdps",
|
|
"accel_native_x_raw",
|
|
"accel_native_y_raw",
|
|
"accel_native_z_raw",
|
|
"gyro_native_x_raw",
|
|
"gyro_native_y_raw",
|
|
"gyro_native_z_raw",
|
|
"accel_int_source",
|
|
"gyro_status",
|
|
"loop_overrun_count",
|
|
]
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Metadata:
|
|
sample_rate_hz: int
|
|
accel_range_g: int
|
|
gyro_range_dps: int
|
|
flags: int
|
|
accel_offset_counts: tuple[float, float, float]
|
|
accel_counts_per_g: tuple[float, float, float]
|
|
gyro_bias_counts: tuple[float, float, float]
|
|
gyro_mdps_per_lsb: float
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Sample:
|
|
sequence: int
|
|
timestamp_us: int
|
|
accel: tuple[int, int, int]
|
|
gyro: tuple[int, int, int]
|
|
accel_status: int
|
|
gyro_status: int
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Frame:
|
|
packet_type: int
|
|
flags: int
|
|
packet_sequence: int
|
|
base_timestamp_us: int
|
|
dropped_sample_count: int
|
|
loop_overrun_count: int
|
|
metadata: Metadata | None
|
|
samples: tuple[Sample, ...]
|
|
raw: bytes
|
|
|
|
|
|
def _lround(value: float) -> int:
|
|
"""Match C lroundf: nearest integer, halfway cases away from zero."""
|
|
return math.floor(value + 0.5) if value >= 0 else math.ceil(value - 0.5)
|
|
|
|
|
|
def sample_to_csv_row(sample: Sample, metadata: Metadata, loop_overruns: int) -> list[int]:
|
|
accel_mg = [
|
|
_lround((sample.accel[i] - metadata.accel_offset_counts[i]) * 1000.0 /
|
|
metadata.accel_counts_per_g[i])
|
|
for i in range(3)
|
|
]
|
|
gyro_mdps = [
|
|
_lround((sample.gyro[i] - metadata.gyro_bias_counts[i]) *
|
|
metadata.gyro_mdps_per_lsb)
|
|
for i in range(3)
|
|
]
|
|
|
|
# Inverse of enclosure accel(x,y,z)=(native_y,-native_x,native_z).
|
|
accel_native = (-sample.accel[1], sample.accel[0], sample.accel[2])
|
|
gyro_native = sample.gyro
|
|
return [
|
|
sample.sequence,
|
|
sample.timestamp_us,
|
|
*sample.accel,
|
|
*sample.gyro,
|
|
*accel_mg,
|
|
*gyro_mdps,
|
|
*accel_native,
|
|
*gyro_native,
|
|
sample.accel_status,
|
|
sample.gyro_status,
|
|
loop_overruns,
|
|
]
|
|
|
|
|
|
class StreamParser:
|
|
def __init__(self) -> None:
|
|
self._buffer = bytearray()
|
|
self.synchronized = False
|
|
self.skipped_bytes = 0
|
|
self.header_errors = 0
|
|
self.startup_crc_errors = 0
|
|
self.crc_errors = 0
|
|
|
|
def feed(self, data: bytes) -> list[Frame]:
|
|
self._buffer.extend(data)
|
|
frames: list[Frame] = []
|
|
while True:
|
|
magic_at = self._buffer.find(MAGIC)
|
|
if magic_at < 0:
|
|
keep = min(len(self._buffer), len(MAGIC) - 1)
|
|
self.skipped_bytes += len(self._buffer) - keep
|
|
if keep:
|
|
del self._buffer[:-keep]
|
|
else:
|
|
self._buffer.clear()
|
|
break
|
|
if magic_at:
|
|
self.skipped_bytes += magic_at
|
|
del self._buffer[:magic_at]
|
|
if len(self._buffer) < HEADER_SIZE:
|
|
break
|
|
|
|
fields = HEADER.unpack_from(self._buffer)
|
|
(
|
|
_magic,
|
|
version,
|
|
packet_type,
|
|
header_size,
|
|
record_size,
|
|
record_count,
|
|
flags,
|
|
payload_size,
|
|
packet_sequence,
|
|
base_timestamp_us,
|
|
dropped_sample_count,
|
|
loop_overrun_count,
|
|
expected_crc,
|
|
) = fields
|
|
|
|
valid_shape = (
|
|
version == VERSION
|
|
and header_size == HEADER_SIZE
|
|
and packet_type in (PACKET_TYPE_METADATA, PACKET_TYPE_SAMPLES)
|
|
and payload_size <= max(METADATA_SIZE, SAMPLE_RECORD_SIZE * MAX_RECORDS)
|
|
)
|
|
if packet_type == PACKET_TYPE_METADATA:
|
|
valid_shape = valid_shape and (
|
|
record_size == 0 and record_count == 0 and payload_size == METADATA_SIZE
|
|
)
|
|
elif packet_type == PACKET_TYPE_SAMPLES:
|
|
valid_shape = valid_shape and (
|
|
record_size == SAMPLE_RECORD_SIZE
|
|
and 1 <= record_count <= MAX_RECORDS
|
|
and payload_size == record_size * record_count
|
|
)
|
|
if not valid_shape:
|
|
self.header_errors += 1
|
|
self.skipped_bytes += 1
|
|
del self._buffer[0]
|
|
continue
|
|
|
|
frame_size = HEADER_SIZE + payload_size
|
|
if len(self._buffer) < frame_size:
|
|
break
|
|
raw = bytes(self._buffer[:frame_size])
|
|
actual_crc = zlib.crc32(raw[4:32])
|
|
actual_crc = zlib.crc32(raw[HEADER_SIZE:], actual_crc)
|
|
if actual_crc != expected_crc:
|
|
if self.synchronized:
|
|
self.crc_errors += 1
|
|
else:
|
|
self.startup_crc_errors += 1
|
|
self.skipped_bytes += 1
|
|
del self._buffer[0]
|
|
continue
|
|
|
|
metadata = None
|
|
samples: tuple[Sample, ...] = ()
|
|
payload = raw[HEADER_SIZE:]
|
|
if packet_type == PACKET_TYPE_METADATA:
|
|
values = METADATA.unpack(payload)
|
|
metadata = Metadata(
|
|
sample_rate_hz=values[0],
|
|
accel_range_g=values[1],
|
|
gyro_range_dps=values[2],
|
|
flags=values[3],
|
|
accel_offset_counts=values[4:7],
|
|
accel_counts_per_g=values[7:10],
|
|
gyro_bias_counts=values[10:13],
|
|
gyro_mdps_per_lsb=values[13],
|
|
)
|
|
else:
|
|
decoded: list[Sample] = []
|
|
timestamp_us = base_timestamp_us
|
|
for index in range(record_count):
|
|
values = SAMPLE.unpack_from(payload, index * SAMPLE_RECORD_SIZE)
|
|
if index:
|
|
timestamp_us += values[1] * 10
|
|
decoded.append(
|
|
Sample(
|
|
sequence=values[0],
|
|
timestamp_us=timestamp_us,
|
|
accel=values[2:5],
|
|
gyro=values[5:8],
|
|
accel_status=values[8],
|
|
gyro_status=values[9],
|
|
)
|
|
)
|
|
samples = tuple(decoded)
|
|
|
|
frames.append(
|
|
Frame(
|
|
packet_type=packet_type,
|
|
flags=flags,
|
|
packet_sequence=packet_sequence,
|
|
base_timestamp_us=base_timestamp_us,
|
|
dropped_sample_count=dropped_sample_count,
|
|
loop_overrun_count=loop_overrun_count,
|
|
metadata=metadata,
|
|
samples=samples,
|
|
raw=raw,
|
|
)
|
|
)
|
|
self.synchronized = True
|
|
del self._buffer[:frame_size]
|
|
return frames
|