add reliable BLE telemetry transport
This commit is contained in:
@@ -0,0 +1,220 @@
|
||||
#!/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"
|
||||
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
|
||||
)
|
||||
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"sample_gaps={integrity.sample_gap_count}, "
|
||||
f"crc_errors={parser.crc_errors}, "
|
||||
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())
|
||||
Reference in New Issue
Block a user