add reliable BLE telemetry transport

tools/trikke_protocol.py

@@ -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,
                 )