harden USB telemetry transport
This commit is contained in:
+46
-65
@@ -6,6 +6,7 @@ import csv
|
||||
import glob
|
||||
import signal
|
||||
import sys
|
||||
from contextlib import ExitStack
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
@@ -13,9 +14,9 @@ import serial
|
||||
|
||||
from trikke_protocol import (
|
||||
CSV_COLUMNS,
|
||||
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED,
|
||||
PACKET_TYPE_METADATA,
|
||||
Frame,
|
||||
IntegrityTracker,
|
||||
Metadata,
|
||||
StreamParser,
|
||||
sample_to_csv_row,
|
||||
@@ -28,6 +29,11 @@ def parse_args() -> argparse.Namespace:
|
||||
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")
|
||||
parser.add_argument(
|
||||
"--wire",
|
||||
type=Path,
|
||||
help="optional byte-for-byte serial capture, including startup text",
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
@@ -57,6 +63,8 @@ def main() -> int:
|
||||
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)
|
||||
if args.wire is not None:
|
||||
args.wire.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
stop_requested = False
|
||||
|
||||
@@ -72,83 +80,46 @@ def main() -> int:
|
||||
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
|
||||
integrity = IntegrityTracker()
|
||||
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
|
||||
nonlocal sample_count
|
||||
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:
|
||||
with ExitStack() as stack:
|
||||
sensor = stack.enter_context(
|
||||
serial.Serial(port, args.baud, timeout=0.25)
|
||||
)
|
||||
raw_capture = stack.enter_context(output.open("wb"))
|
||||
decoded = stack.enter_context(
|
||||
csv_output.open("w", encoding="utf-8", newline="")
|
||||
)
|
||||
wire_capture = (
|
||||
stack.enter_context(args.wire.open("wb"))
|
||||
if args.wire is not None
|
||||
else None
|
||||
)
|
||||
writer = csv.writer(decoded)
|
||||
writer.writerow(CSV_COLUMNS)
|
||||
while not stop_requested:
|
||||
chunk = sensor.read(4096)
|
||||
if not chunk:
|
||||
continue
|
||||
if wire_capture is not None:
|
||||
wire_capture.write(chunk)
|
||||
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
|
||||
integrity.observe(frame)
|
||||
if frame.packet_type == PACKET_TYPE_METADATA:
|
||||
metadata = frame.metadata
|
||||
metadata_count += 1
|
||||
@@ -159,16 +130,21 @@ def main() -> int:
|
||||
render_frame(frame, writer)
|
||||
raw_capture.flush()
|
||||
decoded.flush()
|
||||
if wire_capture is not None:
|
||||
wire_capture.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"packet_gaps={integrity.packet_gap_count}, "
|
||||
f"packet_resets={integrity.packet_reset_count}, "
|
||||
f"sample_gaps={integrity.sample_gap_count}, "
|
||||
f"sample_resets={integrity.sample_reset_count}, "
|
||||
f"timing_anomalies={integrity.timing_anomaly_count}, "
|
||||
"timestamp_saturation_frames="
|
||||
f"{integrity.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}, "
|
||||
@@ -176,12 +152,17 @@ def main() -> int:
|
||||
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}"
|
||||
f"Status totals: accel_stale={integrity.accel_stale_count}, "
|
||||
f"accel_overrun={integrity.accel_overrun_count}, "
|
||||
f"gyro_stale={integrity.gyro_stale_count}, "
|
||||
f"gyro_overrun={integrity.gyro_overrun_count}, "
|
||||
f"dropped={integrity.final_dropped_sample_count}, "
|
||||
f"acquisition_loop_overruns={integrity.final_loop_overrun_count}"
|
||||
)
|
||||
print(f"Saved {output} and {csv_output}")
|
||||
saved = f"Saved {output} and {csv_output}"
|
||||
if args.wire is not None:
|
||||
saved += f"; raw wire saved to {args.wire}"
|
||||
print(saved)
|
||||
if serial_error is not None:
|
||||
return 1
|
||||
return 0 if metadata is not None else 4
|
||||
|
||||
Reference in New Issue
Block a user