192 lines
7.3 KiB
Python
192 lines
7.3 KiB
Python
#!/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_FLAG_TIMESTAMP_DELTA_SATURATED,
|
|
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
|
|
timestamp_saturation_frame_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
|
|
serial_error: serial.SerialException | None = None
|
|
|
|
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.flags & PACKET_FLAG_TIMESTAMP_DELTA_SATURATED:
|
|
timestamp_saturation_frame_count += 1
|
|
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)
|
|
serial_error = exc
|
|
|
|
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"timestamp_saturation_frames={timestamp_saturation_frame_count}, "
|
|
f"startup_crc_rejects={parser.startup_crc_errors}, "
|
|
f"stream_crc_errors={parser.crc_errors}, "
|
|
f"header_errors={parser.header_errors}, "
|
|
f"skipped_nonframe_bytes={parser.skipped_bytes}, "
|
|
f"trailing_partial_bytes={parser.buffered_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}")
|
|
if serial_error is not None:
|
|
return 1
|
|
return 0 if metadata is not None else 4
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|