add framed binary telemetry transport

2026-08-17 11:13:53 -04:00
parent a5c3087ee4
commit aceaa2b270
13 changed files with 1320 additions and 110 deletions
@@ -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())