232 lines
9.2 KiB
Python
232 lines
9.2 KiB
Python
#!/usr/bin/env python3
|
|
"""Capture acknowledged TRK1 telemetry from the Trikke BLE service."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import asyncio
|
|
import csv
|
|
import os
|
|
import signal
|
|
from contextlib import ExitStack
|
|
from datetime import datetime
|
|
from pathlib import Path
|
|
|
|
from trikke_ble import BleFrameReassembler, encode_ack
|
|
from trikke_protocol import (
|
|
CSV_COLUMNS,
|
|
PACKET_TYPE_METADATA,
|
|
Frame,
|
|
IntegrityTracker,
|
|
Metadata,
|
|
StreamParser,
|
|
sample_to_csv_row,
|
|
)
|
|
|
|
DEVICE_NAME = "TrikkeSensor"
|
|
SERVICE_UUID = "7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c10"
|
|
DATA_UUID = "7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c11"
|
|
ACK_UUID = "7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c12"
|
|
|
|
|
|
def parse_args() -> argparse.Namespace:
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("--address", help="BLE address/identifier; scan by name when omitted")
|
|
parser.add_argument("--name", default=DEVICE_NAME)
|
|
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()
|
|
|
|
|
|
async def capture(args: argparse.Namespace) -> int:
|
|
try:
|
|
from bleak import BleakClient, BleakScanner
|
|
from bleak.exc import BleakError
|
|
except ImportError:
|
|
print("BLE capture requires bleak: python3 -m pip install -r requirements.txt")
|
|
return 2
|
|
|
|
stem = datetime.now().strftime("ble_%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)
|
|
|
|
device = args.address
|
|
|
|
stop = asyncio.Event()
|
|
loop = asyncio.get_running_loop()
|
|
for signum in (signal.SIGINT, signal.SIGTERM):
|
|
try:
|
|
loop.add_signal_handler(signum, stop.set)
|
|
except NotImplementedError:
|
|
pass
|
|
|
|
fragments: asyncio.Queue[bytes] = asyncio.Queue(maxsize=512)
|
|
callback_drop_count = 0
|
|
|
|
def on_fragment(_characteristic: object, data: bytearray) -> None:
|
|
payload = bytes(data)
|
|
|
|
def enqueue() -> None:
|
|
nonlocal callback_drop_count
|
|
try:
|
|
fragments.put_nowait(payload)
|
|
except asyncio.QueueFull:
|
|
callback_drop_count += 1
|
|
|
|
loop.call_soon_threadsafe(enqueue)
|
|
|
|
reassembler = BleFrameReassembler()
|
|
parser = StreamParser()
|
|
integrity = IntegrityTracker()
|
|
metadata: Metadata | None = None
|
|
pending_frames: list[Frame] = []
|
|
last_persisted_sequence: int | None = None
|
|
last_persisted_raw: bytes | None = None
|
|
sample_count = 0
|
|
frame_count = 0
|
|
|
|
with ExitStack() as stack:
|
|
raw_capture = stack.enter_context(output.open("wb"))
|
|
decoded = stack.enter_context(csv_output.open("w", encoding="utf-8", newline=""))
|
|
writer = csv.writer(decoded)
|
|
writer.writerow(CSV_COLUMNS)
|
|
print(f"Recording to {output} and {csv_output}; press Ctrl-C to stop")
|
|
while not stop.is_set():
|
|
try:
|
|
if device is None:
|
|
print(f"Scanning for {args.name}...")
|
|
device = await BleakScanner.find_device_by_name(
|
|
args.name,
|
|
timeout=5.0,
|
|
service_uuids=[SERVICE_UUID],
|
|
)
|
|
if device is None:
|
|
await asyncio.sleep(0.5)
|
|
continue
|
|
print(f"Connecting to {device}...")
|
|
async with BleakClient(device) as client:
|
|
reassembler.reset()
|
|
while not fragments.empty():
|
|
fragments.get_nowait()
|
|
await client.start_notify(DATA_UUID, on_fragment)
|
|
print("BLE connected and subscribed")
|
|
while not stop.is_set() and client.is_connected:
|
|
try:
|
|
fragment = await asyncio.wait_for(
|
|
fragments.get(), timeout=0.25
|
|
)
|
|
except TimeoutError:
|
|
continue
|
|
assembled = reassembler.feed(fragment)
|
|
if assembled is None:
|
|
continue
|
|
|
|
frames = parser.feed(assembled)
|
|
if len(frames) != 1 or frames[0].raw != assembled:
|
|
continue
|
|
frame = frames[0]
|
|
if (
|
|
frame.packet_sequence == last_persisted_sequence
|
|
and frame.raw == last_persisted_raw
|
|
):
|
|
await client.write_gatt_char(
|
|
ACK_UUID,
|
|
encode_ack(frame.packet_sequence),
|
|
response=True,
|
|
)
|
|
continue
|
|
|
|
raw_capture.write(frame.raw)
|
|
raw_capture.flush()
|
|
os.fsync(raw_capture.fileno())
|
|
integrity.observe(frame)
|
|
if frame.packet_type == PACKET_TYPE_METADATA:
|
|
metadata = frame.metadata
|
|
for pending in pending_frames:
|
|
for sample in pending.samples:
|
|
writer.writerow(sample_to_csv_row(
|
|
sample,
|
|
metadata,
|
|
pending.loop_overrun_count,
|
|
))
|
|
sample_count += 1
|
|
pending_frames.clear()
|
|
elif metadata is None:
|
|
pending_frames.append(frame)
|
|
else:
|
|
for sample in frame.samples:
|
|
writer.writerow(sample_to_csv_row(
|
|
sample, metadata, frame.loop_overrun_count
|
|
))
|
|
sample_count += 1
|
|
decoded.flush()
|
|
|
|
# The binary stream is authoritative and fsynced before
|
|
# ACK. A lost ACK is safe: replay is deduped above.
|
|
last_persisted_sequence = frame.packet_sequence
|
|
last_persisted_raw = frame.raw
|
|
await client.write_gatt_char(
|
|
ACK_UUID,
|
|
encode_ack(frame.packet_sequence),
|
|
response=True,
|
|
)
|
|
frame_count += 1
|
|
if client.is_connected:
|
|
await client.stop_notify(DATA_UUID)
|
|
except (BleakError, OSError) as error:
|
|
if not stop.is_set():
|
|
print(f"BLE interrupted ({error}); reconnecting")
|
|
if args.address is None:
|
|
device = None
|
|
if not stop.is_set():
|
|
await asyncio.sleep(0.5)
|
|
|
|
print(
|
|
f"Stopped after {frame_count} frames and {sample_count} samples; "
|
|
f"fragment_rejects={reassembler.rejected_fragment_count}, "
|
|
f"callback_drops={callback_drop_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}, "
|
|
f"skipped_nonframe_bytes={parser.skipped_bytes}, "
|
|
f"trailing_partial_bytes={parser.buffered_bytes}"
|
|
)
|
|
print(
|
|
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}"
|
|
)
|
|
if integrity.final_status is not None:
|
|
status = integrity.final_status
|
|
print(
|
|
"Cause totals: "
|
|
f"sensor_read_failures={status.sensor_read_failure_count}, "
|
|
f"queue_overflows={status.queue_overflow_count}, "
|
|
f"transport_begin_retries={status.transport_begin_retry_count}, "
|
|
f"transport_disconnects={status.transport_disconnect_count}, "
|
|
f"transport_send_failures={status.transport_send_failure_count}, "
|
|
f"transport_replays={status.transport_replay_count}, "
|
|
f"transport_invalid_acks={status.transport_invalid_ack_count}"
|
|
)
|
|
return 0 if metadata is not None else 4
|
|
|
|
|
|
def main() -> int:
|
|
return asyncio.run(capture(parse_args()))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|