add framed binary telemetry transport
This commit is contained in:
@@ -0,0 +1,181 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Capture validated TRK1 frames and render their samples to CSV."""
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import glob
|
||||
import signal
|
||||
import sys
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
import serial
|
||||
|
||||
from trikke_protocol import (
|
||||
CSV_COLUMNS,
|
||||
PACKET_TYPE_METADATA,
|
||||
Frame,
|
||||
Metadata,
|
||||
StreamParser,
|
||||
sample_to_csv_row,
|
||||
)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--port", help="serial port; auto-detected when omitted")
|
||||
parser.add_argument("--baud", type=int, default=115200)
|
||||
parser.add_argument("--output", type=Path, help="validated binary .trk output")
|
||||
parser.add_argument("--csv", type=Path, help="decoded CSV output")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def resolve_port(requested_port: str | None) -> str:
|
||||
if requested_port:
|
||||
return requested_port
|
||||
candidates = sorted(glob.glob("/dev/cu.usbmodem*"))
|
||||
if not candidates:
|
||||
raise RuntimeError("no /dev/cu.usbmodem* serial device found")
|
||||
if len(candidates) > 1:
|
||||
raise RuntimeError(
|
||||
f"multiple serial devices found ({', '.join(candidates)}); specify --port"
|
||||
)
|
||||
return candidates[0]
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
try:
|
||||
port = resolve_port(args.port)
|
||||
except RuntimeError as exc:
|
||||
print(f"Port error: {exc}", file=sys.stderr)
|
||||
return 2
|
||||
|
||||
stem = datetime.now().strftime("binary_%Y%m%d_%H%M%S")
|
||||
output = args.output or Path("captures") / f"{stem}.trk"
|
||||
csv_output = args.csv or output.with_suffix(".csv")
|
||||
output.parent.mkdir(parents=True, exist_ok=True)
|
||||
csv_output.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
stop_requested = False
|
||||
|
||||
def request_stop(_signum: int, _frame: object) -> None:
|
||||
nonlocal stop_requested
|
||||
stop_requested = True
|
||||
|
||||
signal.signal(signal.SIGINT, request_stop)
|
||||
signal.signal(signal.SIGTERM, request_stop)
|
||||
|
||||
parser = StreamParser()
|
||||
metadata: Metadata | None = None
|
||||
pending_frames: list[Frame] = []
|
||||
sample_count = 0
|
||||
metadata_count = 0
|
||||
packet_gap_count = 0
|
||||
packet_reset_count = 0
|
||||
sample_gap_count = 0
|
||||
sample_reset_count = 0
|
||||
timing_anomaly_count = 0
|
||||
accel_stale_count = 0
|
||||
accel_overrun_count = 0
|
||||
gyro_stale_count = 0
|
||||
gyro_overrun_count = 0
|
||||
previous_packet_sequence = None
|
||||
previous_sample_sequence = None
|
||||
previous_timestamp_us = None
|
||||
final_dropped_count = 0
|
||||
final_loop_overrun_count = 0
|
||||
|
||||
def render_frame(frame: Frame, writer: csv.writer) -> None:
|
||||
nonlocal sample_count, sample_gap_count, sample_reset_count
|
||||
nonlocal timing_anomaly_count
|
||||
nonlocal accel_stale_count, accel_overrun_count
|
||||
nonlocal gyro_stale_count, gyro_overrun_count
|
||||
nonlocal previous_sample_sequence, previous_timestamp_us
|
||||
if metadata is None:
|
||||
pending_frames.append(frame)
|
||||
return
|
||||
for sample in frame.samples:
|
||||
if previous_sample_sequence is not None:
|
||||
expected = (previous_sample_sequence + 1) & 0xFFFFFFFF
|
||||
if sample.sequence != expected:
|
||||
if sample.sequence > expected:
|
||||
sample_gap_count += sample.sequence - expected
|
||||
else:
|
||||
sample_reset_count += 1
|
||||
if previous_timestamp_us is not None:
|
||||
if sample.timestamp_us - previous_timestamp_us != 10_000:
|
||||
timing_anomaly_count += 1
|
||||
if not sample.accel_status & 0x80:
|
||||
accel_stale_count += 1
|
||||
if sample.accel_status & 0x01:
|
||||
accel_overrun_count += 1
|
||||
if not sample.gyro_status & 0x08:
|
||||
gyro_stale_count += 1
|
||||
if sample.gyro_status & 0x80:
|
||||
gyro_overrun_count += 1
|
||||
writer.writerow(sample_to_csv_row(sample, metadata, frame.loop_overrun_count))
|
||||
previous_sample_sequence = sample.sequence
|
||||
previous_timestamp_us = sample.timestamp_us
|
||||
sample_count += 1
|
||||
if sample_count % 500 == 0:
|
||||
print(f" {sample_count} samples captured", flush=True)
|
||||
|
||||
print(f"Recording {port} to {output} and {csv_output}; press Ctrl-C to stop")
|
||||
try:
|
||||
with serial.Serial(port, args.baud, timeout=0.25) as sensor, output.open(
|
||||
"wb"
|
||||
) as raw_capture, csv_output.open("w", encoding="utf-8", newline="") as decoded:
|
||||
writer = csv.writer(decoded)
|
||||
writer.writerow(CSV_COLUMNS)
|
||||
while not stop_requested:
|
||||
chunk = sensor.read(4096)
|
||||
if not chunk:
|
||||
continue
|
||||
for frame in parser.feed(chunk):
|
||||
raw_capture.write(frame.raw)
|
||||
final_dropped_count = frame.dropped_sample_count
|
||||
final_loop_overrun_count = frame.loop_overrun_count
|
||||
if previous_packet_sequence is not None:
|
||||
expected = (previous_packet_sequence + 1) & 0xFFFFFFFF
|
||||
if frame.packet_sequence != expected:
|
||||
if frame.packet_sequence > expected:
|
||||
packet_gap_count += frame.packet_sequence - expected
|
||||
else:
|
||||
packet_reset_count += 1
|
||||
previous_packet_sequence = frame.packet_sequence
|
||||
if frame.packet_type == PACKET_TYPE_METADATA:
|
||||
metadata = frame.metadata
|
||||
metadata_count += 1
|
||||
for pending in pending_frames:
|
||||
render_frame(pending, writer)
|
||||
pending_frames.clear()
|
||||
else:
|
||||
render_frame(frame, writer)
|
||||
raw_capture.flush()
|
||||
decoded.flush()
|
||||
except serial.SerialException as exc:
|
||||
print(f"Serial error: {exc}", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
print(
|
||||
f"Stopped after {sample_count} samples and {metadata_count} metadata frames; "
|
||||
f"packet_gaps={packet_gap_count}, packet_resets={packet_reset_count}, "
|
||||
f"sample_gaps={sample_gap_count}, sample_resets={sample_reset_count}, "
|
||||
f"timing_anomalies={timing_anomaly_count}, "
|
||||
f"startup_crc_rejects={parser.startup_crc_errors}, "
|
||||
f"stream_crc_errors={parser.crc_errors}, "
|
||||
f"header_errors={parser.header_errors}, skipped_nonframe_bytes={parser.skipped_bytes}"
|
||||
)
|
||||
print(
|
||||
f"Status totals: accel_stale={accel_stale_count}, "
|
||||
f"accel_overrun={accel_overrun_count}, gyro_stale={gyro_stale_count}, "
|
||||
f"gyro_overrun={gyro_overrun_count}, dropped={final_dropped_count}, "
|
||||
f"acquisition_loop_overruns={final_loop_overrun_count}"
|
||||
)
|
||||
print(f"Saved {output} and {csv_output}")
|
||||
return 0 if metadata is not None else 4
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Decode a validated or raw TRK1 byte stream into CSV."""
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
from pathlib import Path
|
||||
|
||||
from trikke_protocol import (
|
||||
CSV_COLUMNS,
|
||||
PACKET_TYPE_METADATA,
|
||||
Frame,
|
||||
StreamParser,
|
||||
sample_to_csv_row,
|
||||
)
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("input", type=Path)
|
||||
parser.add_argument("output", type=Path)
|
||||
args = parser.parse_args()
|
||||
|
||||
stream = StreamParser()
|
||||
frames: list[Frame] = []
|
||||
with args.input.open("rb") as source:
|
||||
while chunk := source.read(64 * 1024):
|
||||
frames.extend(stream.feed(chunk))
|
||||
|
||||
metadata = next(
|
||||
(frame.metadata for frame in frames if frame.packet_type == PACKET_TYPE_METADATA),
|
||||
None,
|
||||
)
|
||||
if metadata is None:
|
||||
print("No valid metadata frame found")
|
||||
return 2
|
||||
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
sample_count = 0
|
||||
with args.output.open("w", encoding="utf-8", newline="") as target:
|
||||
writer = csv.writer(target)
|
||||
writer.writerow(CSV_COLUMNS)
|
||||
for frame in frames:
|
||||
if frame.packet_type == PACKET_TYPE_METADATA:
|
||||
if frame.metadata is not None:
|
||||
metadata = frame.metadata
|
||||
continue
|
||||
for sample in frame.samples:
|
||||
writer.writerow(
|
||||
sample_to_csv_row(sample, metadata, frame.loop_overrun_count)
|
||||
)
|
||||
sample_count += 1
|
||||
|
||||
print(
|
||||
f"Decoded {sample_count} samples from {len(frames)} frames; "
|
||||
f"startup_crc_rejects={stream.startup_crc_errors}, "
|
||||
f"stream_crc_errors={stream.crc_errors}, header_errors={stream.header_errors}, "
|
||||
f"skipped_nonframe_bytes={stream.skipped_bytes}; saved {args.output}"
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,252 @@
|
||||
"""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
|
||||
Reference in New Issue
Block a user