add reliable BLE telemetry transport

2026-08-18 06:24:10 -04:00
parent 00f52ecf0f
commit 7617010d8e
26 changed files with 1499 additions and 47 deletions
@@ -175,6 +175,18 @@ def main() -> int:
        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}"
+        )
    saved = f"Saved {output} and {csv_output}"
    if args.wire is not None:
        saved += f"; raw wire saved to {args.wire}"
@@ -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())
@@ -76,6 +76,18 @@ def main() -> int:
        f"acquisition_loop_overruns={integrity.final_loop_overrun_count}, "
        f"trailing_partial_bytes={stream.buffered_bytes}; saved {args.output}"
    )
+    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


@@ -0,0 +1,66 @@
+"""BLE fragment reassembly and acknowledgement helpers for TRK1 frames."""
+
+from __future__ import annotations
+
+import struct
+
+BLE_FRAGMENT_HEADER = struct.Struct("<IHH")
+BLE_ACK = struct.Struct("<4sI")
+BLE_MIN_FRAME_SIZE = 36
+BLE_MAX_FRAME_SIZE = 196
+
+
+class BleFrameReassembler:
+    def __init__(self) -> None:
+        self.rejected_fragment_count = 0
+        self._sequence: int | None = None
+        self._total_size = 0
+        self._frame = bytearray()
+
+    def reset(self) -> None:
+        self._sequence = None
+        self._total_size = 0
+        self._frame.clear()
+
+    def feed(self, fragment: bytes) -> bytes | None:
+        if len(fragment) <= BLE_FRAGMENT_HEADER.size:
+            self.rejected_fragment_count += 1
+            self.reset()
+            return None
+        sequence, offset, total_size = BLE_FRAGMENT_HEADER.unpack_from(fragment)
+        data = fragment[BLE_FRAGMENT_HEADER.size :]
+        if (
+            total_size < BLE_MIN_FRAME_SIZE
+            or total_size > BLE_MAX_FRAME_SIZE
+            or offset >= total_size
+            or offset + len(data) > total_size
+        ):
+            self.rejected_fragment_count += 1
+            self.reset()
+            return None
+
+        # Offset zero is an explicit replay boundary, including when the same
+        # packet restarts after an ACK timeout or reconnect.
+        if offset == 0:
+            self._sequence = sequence
+            self._total_size = total_size
+            self._frame = bytearray()
+        if (
+            self._sequence != sequence
+            or self._total_size != total_size
+            or offset != len(self._frame)
+        ):
+            self.rejected_fragment_count += 1
+            self.reset()
+            return None
+
+        self._frame.extend(data)
+        if len(self._frame) != self._total_size:
+            return None
+        frame = bytes(self._frame)
+        self.reset()
+        return frame
+
+
+def encode_ack(packet_sequence: int) -> bytes:
+    return BLE_ACK.pack(b"ACK1", packet_sequence & 0xFFFFFFFF)
@@ -12,15 +12,18 @@ VERSION = 1
 HEADER_SIZE = 36
 SAMPLE_RECORD_SIZE = 20
 METADATA_SIZE = 48
+STATUS_SIZE = 32
 MAX_RECORDS = 8

 PACKET_TYPE_METADATA = 1
 PACKET_TYPE_SAMPLES = 2
+PACKET_TYPE_STATUS = 3
 PACKET_FLAG_TIMESTAMP_DELTA_SATURATED = 0x01

 HEADER = struct.Struct("<4sBBBBBBHIQIII")
 METADATA = struct.Struct("<HHHH10f")
 SAMPLE = struct.Struct("<IHhhhhhhBB")
+STATUS = struct.Struct("<HH7I")

 CSV_COLUMNS = [
    "sequence",
@@ -71,6 +74,17 @@ class Sample:
    gyro_status: int


+@dataclass(frozen=True)
+class Status:
+    sensor_read_failure_count: int
+    queue_overflow_count: int
+    transport_begin_retry_count: int
+    transport_disconnect_count: int
+    transport_send_failure_count: int
+    transport_replay_count: int
+    transport_invalid_ack_count: int
+
+
@dataclass(frozen=True)
 class Frame:
    packet_type: int
@@ -80,6 +94,7 @@ class Frame:
    dropped_sample_count: int
    loop_overrun_count: int
    metadata: Metadata | None
+    status: Status | None
    samples: tuple[Sample, ...]
    raw: bytes

@@ -98,6 +113,7 @@ class IntegrityTracker:
    gyro_overrun_count: int = 0
    final_dropped_sample_count: int = 0
    final_loop_overrun_count: int = 0
+    final_status: Status | None = None
    _previous_packet_sequence: int | None = None
    _previous_sample_sequence: int | None = None
    _previous_timestamp_us: int | None = None
@@ -128,6 +144,8 @@ class IntegrityTracker:
            self.timestamp_saturation_frame_count += 1
        self.final_dropped_sample_count = frame.dropped_sample_count
        self.final_loop_overrun_count = frame.loop_overrun_count
+        if frame.status is not None:
+            self.final_status = frame.status

        for sample in frame.samples:
            if self._previous_sample_sequence is not None:
@@ -241,8 +259,16 @@ class StreamParser:
            valid_shape = (
                version == VERSION
                and header_size == HEADER_SIZE
-                and packet_type in (PACKET_TYPE_METADATA, PACKET_TYPE_SAMPLES)
-                and payload_size <= max(METADATA_SIZE, SAMPLE_RECORD_SIZE * MAX_RECORDS)
+                and packet_type in (
+                    PACKET_TYPE_METADATA,
+                    PACKET_TYPE_SAMPLES,
+                    PACKET_TYPE_STATUS,
+                )
+                and payload_size <= max(
+                    METADATA_SIZE,
+                    STATUS_SIZE,
+                    SAMPLE_RECORD_SIZE * MAX_RECORDS,
+                )
            )
            if packet_type == PACKET_TYPE_METADATA:
                valid_shape = valid_shape and (
@@ -254,6 +280,12 @@ class StreamParser:
                    and 1 <= record_count <= MAX_RECORDS
                    and payload_size == record_size * record_count
                )
+            elif packet_type == PACKET_TYPE_STATUS:
+                valid_shape = valid_shape and (
+                    record_size == 0
+                    and record_count == 0
+                    and payload_size == STATUS_SIZE
+                )
            if not valid_shape:
                self.header_errors += 1
                self.skipped_bytes += 1
@@ -276,6 +308,7 @@ class StreamParser:
                continue

            metadata = None
+            status = None
            samples: tuple[Sample, ...] = ()
            payload = raw[HEADER_SIZE:]
            if packet_type == PACKET_TYPE_METADATA:
@@ -290,7 +323,7 @@ class StreamParser:
                    gyro_bias_counts=values[10:13],
                    gyro_mdps_per_lsb=values[13],
                )
-            else:
+            elif packet_type == PACKET_TYPE_SAMPLES:
                decoded: list[Sample] = []
                timestamp_us = base_timestamp_us
                for index in range(record_count):
@@ -308,6 +341,14 @@ class StreamParser:
                        )
                    )
                samples = tuple(decoded)
+            else:
+                values = STATUS.unpack(payload)
+                if values[0] != 1 or values[1] != STATUS_SIZE:
+                    self.header_errors += 1
+                    self.skipped_bytes += 1
+                    del self._buffer[0]
+                    continue
+                status = Status(*values[2:])

            frames.append(
                Frame(
@@ -318,6 +359,7 @@ class StreamParser:
                    dropped_sample_count=dropped_sample_count,
                    loop_overrun_count=loop_overrun_count,
                    metadata=metadata,
+                    status=status,
                    samples=samples,
                    raw=raw,
                )