add reliable BLE telemetry transport

This commit is contained in:
Jay
2026-08-18 06:24:10 -04:00
parent 00f52ecf0f
commit 7617010d8e
26 changed files with 1499 additions and 47 deletions
+1
View File
@@ -1,4 +1,5 @@
build/ build/
build-usb/
sdkconfig sdkconfig
captures/ captures/
sdkconfig.old sdkconfig.old
+37 -7
View File
@@ -7,11 +7,14 @@ This milestone does four things:
1. Detects and verifies both sensors by their identification registers. 1. Detects and verifies both sensors by their identification registers.
2. Configures each sensor for a nominal 100 Hz raw output rate. 2. Configures each sensor for a nominal 100 Hz raw output rate.
3. Emits framed, timestamped binary readings over the XIAO USB connection. 3. Emits framed, timestamped binary readings over reliable BLE, with the
audited direct-USB path retained as a build option.
4. Maps both sensors into a shared enclosure frame and carries the metadata 4. Maps both sensors into a shared enclosure frame and carries the metadata
needed to derive calibrated readings without replacing raw data. needed to derive calibrated readings without replacing raw data.
BLE transport and phone-side storage come after the wired sensor path is proven. The wired sensor path is proven. This milestone adds the first reliable BLE
transport and a macOS-compatible reference capture client; phone-side storage
remains the next consumer implementation.
## Wiring ## Wiring
@@ -28,7 +31,7 @@ Both breakouts share SDA, SCL, 3V3, and GND. The firmware checks both possible
- ADXL345: `0x53` or `0x1D`; expected `DEVID` is `0xE5`. - ADXL345: `0x53` or `0x1D`; expected `DEVID` is `0xE5`.
- L3G4200D: `0x69` or `0x68`; expected `WHO_AM_I` is `0xD3`. - L3G4200D: `0x69` or `0x68`; expected `WHO_AM_I` is `0xD3`.
The XIAO ESP32-C3 external antenna is installed for the upcoming BLE transport. The XIAO ESP32-C3 external antenna is installed for the BLE transport.
## Sensor configuration ## Sensor configuration
@@ -73,7 +76,14 @@ records into versioned `TRK1` frames, isolating acquisition from brief transport
stalls. CRC, packet and sample sequences, timestamps, and cumulative stalls. CRC, packet and sample sequences, timestamps, and cumulative
loss/overrun counters make permanent loss detectable by the receiver. loss/overrun counters make permanent loss detectable by the receiver.
USB telemetry now uses ESP-IDF's interrupt-driven USB Serial/JTAG driver behind The default build exposes a custom NimBLE GATT service named `TrikkeSensor`.
Each unchanged `TRK1` frame is fragmented as needed, persisted by the receiver,
and then acknowledged by exact packet sequence. A missing ACK causes a replay;
disconnect or subscription loss retains the same frame and restarts it from byte
zero after reconnection. The receiver deduplicates these deliberate replays.
The preserved USB telemetry option uses ESP-IDF's interrupt-driven USB
Serial/JTAG driver behind
a transport-neutral state machine. A complete frame is submitted atomically to a transport-neutral state machine. A complete frame is submitted atomically to
the driver ring and remains pending across bounded drain timeouts; firmware does the driver ring and remains pending across bounded drain timeouts; firmware does
not resubmit it ambiguously or dequeue another frame. The 512-sample queue not resubmit it ambiguously or dequeue another frame. The 512-sample queue
@@ -81,8 +91,8 @@ therefore also protects a connected endpoint that temporarily stops draining.
USB drain confirms that bytes left the device endpoint, not that the capture USB drain confirms that bytes left the device endpoint, not that the capture
application persisted them. Packet/sample sequences and CRC expose loss after application persisted them. Packet/sample sequences and CRC expose loss after
the fact. End-to-end receiver acknowledgements and replay remain part of the BLE the fact. BLE `COMPLETE` instead means the receiver acknowledged the exact frame
transport milestone. after persistence.
Measured end-to-end framing overhead is about 2.47 kB/s at 100 Hz, or 8.47 Measured end-to-end framing overhead is about 2.47 kB/s at 100 Hz, or 8.47
MiB/hour before BLE link overhead. MiB/hour before BLE link overhead.
@@ -103,7 +113,27 @@ idf.py build
idf.py -p /dev/cu.usbmodem1134101 flash idf.py -p /dev/cu.usbmodem1134101 flash
``` ```
## USB output BLE is the default. `idf.py menuconfig` -> `Telemetry transport` can select the
preserved direct USB transport for wired regression work. A separate build tree
can verify that selection without disturbing the normal BLE configuration:
```sh
idf.py -B build-usb -D SDKCONFIG=build-usb/sdkconfig \
-D 'SDKCONFIG_DEFAULTS=sdkconfig.defaults;sdkconfig.usb.defaults' build
```
Install the reference host dependencies and capture BLE telemetry with:
```sh
python3 -m pip install -r requirements.txt
python3 tools/capture_ble.py
```
The client scans for `TrikkeSensor`, stores only complete CRC-valid `TRK1`
frames, flushes the binary and CSV outputs, and only then writes the application
ACK. See [the BLE transport specification](docs/ble-transport-v1.md).
## TRK1 output and USB validation
After readable startup metadata, the device emits framed binary. Each sample is a After readable startup metadata, the device emits framed binary. Each sample is a
20-byte record containing mapped raw sensor counts, timing, sequence, and the two 20-byte record containing mapped raw sensor counts, timing, sequence, and the two
+31 -5
View File
@@ -11,7 +11,7 @@ combined by an underlying byte transport.
| ---: | ---: | --- | | ---: | ---: | --- |
| 0 | 4 | ASCII magic `TRK1` | | 0 | 4 | ASCII magic `TRK1` |
| 4 | 1 | Wire version (`1`) | | 4 | 1 | Wire version (`1`) |
| 5 | 1 | Packet type: metadata `1`, samples `2` | | 5 | 1 | Packet type: metadata `1`, samples `2`, status `3` |
| 6 | 1 | Header size (`36`) | | 6 | 1 | Header size (`36`) |
| 7 | 1 | Record size (`0` or `20`) | | 7 | 1 | Record size (`0` or `20`) |
| 8 | 1 | Record count (`0` or 18) | | 8 | 1 | Record count (`0` or 18) |
@@ -51,7 +51,11 @@ timestamp is reconstructed by cumulatively adding its delta. Firmware ends the
current packet before a delta exceeds the representable 655.35 ms range, making current packet before a delta exceeds the representable 655.35 ms range, making
the next sample the exact base timestamp of a new packet. As a defensive encoder the next sample the exact base timestamp of a new packet. As a defensive encoder
fallback, an unrepresentable delta is stored as `0xFFFF` and sets packet flag bit fallback, an unrepresentable delta is stored as `0xFFFF` and sets packet flag bit
0. Sample sequence gaps remain detectable independently. 0. Because intra-packet deltas are rounded to 10 us while each packet base keeps
the exact ESP timer value, an integrity check for an exact 10,000 us interval can
report up to +/-5 us at packet boundaries; changing the transport's packet size
changes how often that harmless quantization boundary appears. Sample sequence
gaps remain detectable independently.
Mapped raw counts are authoritative. The original sensor-native axes can be Mapped raw counts are authoritative. The original sensor-native axes can be
reconstructed because the mappings are lossless: reconstructed because the mappings are lossless:
@@ -76,6 +80,28 @@ A byte-stream receiver may begin inside an incomplete frame. It discards bytes
until a magic/header/CRC combination validates. Host tools report any rejection until a magic/header/CRC combination validates. Host tools report any rejection
before that first valid frame separately from CRC failures after synchronization. before that first valid frame separately from CRC failures after synchronization.
## Status payload (32 bytes)
Status frames use packet type `3`, record size/count zero, and payload version
`1`. They are emitted at startup and approximately every five seconds. Offsets
0 and 2 are uint16 payload version and payload size; the remaining fields are
cumulative uint32 counters:
| Offset | Field |
| ---: | --- |
| 4 | Sensor read failures |
| 8 | Sample-queue overflows |
| 12 | Initial transport submissions that accepted zero bytes |
| 16 | BLE disconnects after a connection was established |
| 20 | BLE notification enqueue/send failures |
| 24 | BLE frame replays after disconnect, subscription change, or ACK timeout |
| 28 | Malformed, stale, premature, or wrong-connection ACK writes |
The header's cumulative dropped-sample count remains the sum of sensor read
failures and queue overflows, preserving version-1 receiver compatibility while
the status payload makes the causes independently observable. USB-specific BLE
counters remain zero.
## Buffering ## Buffering
Acquisition runs in a dedicated higher-priority task and writes complete samples Acquisition runs in a dedicated higher-priority task and writes complete samples
@@ -106,9 +132,9 @@ USB drain is not end-to-end application delivery confirmation. A host process
may attach after earlier frames have already left the endpoint, or fail after may attach after earlier frames have already left the endpoint, or fail after
the endpoint accepts them. CRC and sequence checks make resulting loss visible, the endpoint accepts them. CRC and sequence checks make resulting loss visible,
but an application acknowledgement and replay window are still required to but an application acknowledgement and replay window are still required to
guarantee receipt. Accordingly, USB `COMPLETE` means endpoint drain, while the guarantee receipt. Accordingly, USB `COMPLETE` means endpoint drain, while
planned reliable BLE backend will reserve `COMPLETE` for an application ACK of reliable BLE reserves `COMPLETE` for an application ACK of the exact frame. See
the exact frame. `ble-transport-v1.md` for fragmentation, replay, and UUIDs.
Receivers report bytes left in an incomplete trailing frame when capture ends. Receivers report bytes left in an incomplete trailing frame when capture ends.
Those bytes cannot pass CRC validation and are not silently admitted as samples. Those bytes cannot pass CRC validation and are not silently admitted as samples.
+59
View File
@@ -0,0 +1,59 @@
# Reliable BLE Transport — Version 1
BLE carries the unchanged, CRC-protected `TRK1` frames defined in
`binary-record-v1.md`. The default peripheral name is `TrikkeSensor`.
## GATT service
| Purpose | UUID | Properties |
| --- | --- | --- |
| Service | `7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c10` | Primary service |
| Data | `7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c11` | Notify |
| ACK | `7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c12` | Write, write without response |
The firmware prefers a 256-byte ATT MTU, allowing the largest 196-byte `TRK1`
frame and its eight-byte BLE envelope to fit in one notification. Smaller MTUs
remain protocol-compatible; firmware sends at most eight fragments per bounded
poll, although sustained 100 Hz delivery still depends on the negotiated link.
## Data notification envelope
Every notification starts with an eight-byte little-endian envelope:
| Offset | Size | Field |
| ---: | ---: | --- |
| 0 | 4 | `TRK1` packet sequence |
| 4 | 2 | Byte offset within the complete `TRK1` frame |
| 6 | 2 | Complete `TRK1` frame size |
| 8 | remaining | Consecutive frame bytes at that offset |
Offset zero starts or restarts a frame. A receiver appends only consecutive
offsets for the same sequence and total size, then validates the complete
`TRK1` header and CRC. A malformed or missing fragment is not acknowledged.
## Application ACK and replay
After validating and persisting a frame, the receiver writes exactly eight bytes
to the ACK characteristic: ASCII `ACK1`, then the acknowledged packet sequence
as little-endian uint32. Firmware accepts an ACK only for the frame it currently
owns and only from the active subscribed connection.
`COMPLETE` is not reported to the output task until that ACK arrives. Until then:
- a one-second ACK timeout replays the frame from offset zero;
- disconnect or notification unsubscription preserves the frame;
- the next subscription replays it from offset zero;
- BLE polling returns `PENDING`, never `RETRY`, after ownership begins.
The ACK itself can be lost after the receiver persisted the frame. Receivers
therefore compare the sequence and raw bytes with their last persisted frame,
avoid writing a duplicate, and ACK the replay again. The reference
`tools/capture_ble.py` implements this ordering.
BLE notification success only means the fragment entered the stack. The `ACK1`
write is the end-to-end boundary. It deliberately confirms application
persistence rather than radio or ATT delivery alone.
Version 1 is an unauthenticated, single-connection prototype service. It does
not yet provide pairing, authorization, or confidentiality against a nearby
peer; those are separate from the loss/replay guarantees above.
+14 -4
View File
@@ -1,7 +1,17 @@
set(trikke_sources
"trikke_sensor_main.c" "trikke_protocol.c" "trikke_transport.c")
set(trikke_requires
adxl345 l3g4200d esp_timer esp_driver_gpio esp_driver_i2c
bt nvs_flash esp_driver_usb_serial_jtag vfs)
if(CONFIG_TRIKKE_TRANSPORT_BLE)
list(APPEND trikke_sources "trikke_ble_protocol.c" "trikke_ble_transport.c")
else()
list(APPEND trikke_sources "trikke_usb_transport.c")
endif()
idf_component_register( idf_component_register(
SRCS "trikke_sensor_main.c" "trikke_protocol.c" "trikke_transport.c" SRCS ${trikke_sources}
"trikke_usb_transport.c"
INCLUDE_DIRS "." INCLUDE_DIRS "."
REQUIRES adxl345 l3g4200d esp_timer esp_driver_gpio esp_driver_i2c REQUIRES ${trikke_requires}
esp_driver_usb_serial_jtag vfs
) )
+15
View File
@@ -0,0 +1,15 @@
choice TRIKKE_TRANSPORT
prompt "Telemetry transport"
default TRIKKE_TRANSPORT_BLE
config TRIKKE_TRANSPORT_BLE
bool "Reliable BLE"
help
Stream TRK1 frames over the Trikke GATT service and retain each
frame until the receiver writes its application acknowledgement.
config TRIKKE_TRANSPORT_USB
bool "Direct USB Serial/JTAG"
help
Preserve the audited direct USB transport for wired validation.
endchoice
+91
View File
@@ -0,0 +1,91 @@
#include "trikke_ble_protocol.h"
#include <string.h>
#include "trikke_protocol.h"
static uint16_t get_u16_le(const uint8_t *input)
{
return (uint16_t)input[0] | ((uint16_t)input[1] << 8);
}
static uint32_t get_u32_le(const uint8_t *input)
{
return (uint32_t)input[0] | ((uint32_t)input[1] << 8) |
((uint32_t)input[2] << 16) | ((uint32_t)input[3] << 24);
}
static void put_u16_le(uint8_t *output, uint16_t value)
{
output[0] = (uint8_t)value;
output[1] = (uint8_t)(value >> 8);
}
static void put_u32_le(uint8_t *output, uint32_t value)
{
output[0] = (uint8_t)value;
output[1] = (uint8_t)(value >> 8);
output[2] = (uint8_t)(value >> 16);
output[3] = (uint8_t)(value >> 24);
}
static bool packet_shape_is_valid(const uint8_t *packet, size_t packet_size)
{
if (packet == NULL || packet_size < TRIKKE_WIRE_HEADER_SIZE ||
packet_size > TRIKKE_WIRE_MAX_PACKET_SIZE ||
memcmp(packet, "TRK1", 4) != 0 ||
packet[4] != TRIKKE_WIRE_VERSION ||
packet[6] != TRIKKE_WIRE_HEADER_SIZE) {
return false;
}
const size_t encoded_size =
TRIKKE_WIRE_HEADER_SIZE + get_u16_le(packet + 10);
return encoded_size == packet_size;
}
size_t trikke_ble_encode_fragment(
uint8_t *output,
size_t output_size,
const uint8_t *packet,
size_t packet_size,
size_t packet_offset,
size_t att_payload_capacity)
{
if (output == NULL || !packet_shape_is_valid(packet, packet_size) ||
packet_offset >= packet_size || packet_size > UINT16_MAX ||
packet_offset > UINT16_MAX ||
att_payload_capacity <= TRIKKE_BLE_FRAGMENT_HEADER_SIZE) {
return 0;
}
size_t data_size =
att_payload_capacity - TRIKKE_BLE_FRAGMENT_HEADER_SIZE;
const size_t remaining = packet_size - packet_offset;
if (data_size > remaining) {
data_size = remaining;
}
const size_t fragment_size = TRIKKE_BLE_FRAGMENT_HEADER_SIZE + data_size;
if (output_size < fragment_size) {
return 0;
}
put_u32_le(output, get_u32_le(packet + 12));
put_u16_le(output + 4, (uint16_t)packet_offset);
put_u16_le(output + 6, (uint16_t)packet_size);
memcpy(output + TRIKKE_BLE_FRAGMENT_HEADER_SIZE,
packet + packet_offset, data_size);
return fragment_size;
}
bool trikke_ble_decode_ack(
const uint8_t *ack,
size_t ack_size,
uint32_t *packet_sequence)
{
if (ack == NULL || packet_sequence == NULL ||
ack_size != TRIKKE_BLE_ACK_SIZE || memcmp(ack, "ACK1", 4) != 0) {
return false;
}
*packet_sequence = get_u32_le(ack + 4);
return true;
}
+34
View File
@@ -0,0 +1,34 @@
#pragma once
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#define TRIKKE_BLE_FRAGMENT_HEADER_SIZE 8
#define TRIKKE_BLE_ACK_SIZE 8
// BLE data notifications carry a little-endian packet sequence, byte offset,
// total TRK1 frame size, then the frame bytes at that offset. The unchanged
// TRK1 CRC remains the end-to-end integrity check after reassembly.
size_t trikke_ble_encode_fragment(
uint8_t *output,
size_t output_size,
const uint8_t *packet,
size_t packet_size,
size_t packet_offset,
size_t att_payload_capacity);
// An application acknowledgement is ASCII "ACK1" followed by the exact
// little-endian TRK1 packet sequence that the receiver persisted.
bool trikke_ble_decode_ack(
const uint8_t *ack,
size_t ack_size,
uint32_t *packet_sequence);
#ifdef __cplusplus
}
#endif
+479
View File
@@ -0,0 +1,479 @@
#include "trikke_ble_transport.h"
#include <string.h>
#include "host/ble_att.h"
#include "host/ble_gap.h"
#include "host/ble_gatt.h"
#include "host/ble_hs.h"
#include "host/ble_uuid.h"
#include "host/util/util.h"
#include "esp_timer.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "nvs_flash.h"
#include "os/os_mbuf.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "trikke_ble_protocol.h"
#include "trikke_protocol.h"
#define TRIKKE_BLE_DEVICE_NAME "TrikkeSensor"
#define TRIKKE_BLE_MAX_ATT_PAYLOAD 253
#define TRIKKE_BLE_FRAGMENTS_PER_POLL 8
#define TRIKKE_BLE_ACK_TIMEOUT_US 1000000
static trikke_ble_transport_t *s_ble;
static uint8_t s_own_address_type;
static uint16_t s_data_value_handle;
static uint16_t s_ack_value_handle;
// 7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c10 and adjacent characteristic UUIDs.
static const ble_uuid128_t TRIKKE_SERVICE_UUID =
BLE_UUID128_INIT(0x10, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static const ble_uuid128_t TRIKKE_DATA_UUID =
BLE_UUID128_INIT(0x11, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static const ble_uuid128_t TRIKKE_ACK_UUID =
BLE_UUID128_INIT(0x12, 0x9c, 0x1e, 0x2a, 0x4c, 0x3d, 0xbe, 0x8f,
0x9b, 0x4a, 0x5b, 0xf7, 0x00, 0xa0, 0x2e, 0x7d);
static uint32_t get_u32_le(const uint8_t *input)
{
return (uint32_t)input[0] | ((uint32_t)input[1] << 8) |
((uint32_t)input[2] << 16) | ((uint32_t)input[3] << 24);
}
static int ack_access(
uint16_t connection_handle,
uint16_t attribute_handle,
struct ble_gatt_access_ctxt *context,
void *argument)
{
(void)attribute_handle;
(void)argument;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL || context->op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
return BLE_ATT_ERR_UNLIKELY;
}
uint8_t ack[TRIKKE_BLE_ACK_SIZE] = {0};
uint16_t ack_size = 0;
if (OS_MBUF_PKTLEN(context->om) != TRIKKE_BLE_ACK_SIZE ||
ble_hs_mbuf_to_flat(context->om, ack, sizeof(ack), &ack_size) != 0) {
portENTER_CRITICAL(&ble->lock);
++ble->counters.invalid_ack_count;
portEXIT_CRITICAL(&ble->lock);
return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN;
}
uint32_t acknowledged_sequence = 0;
const bool valid_shape =
trikke_ble_decode_ack(ack, ack_size, &acknowledged_sequence);
portENTER_CRITICAL(&ble->lock);
const bool accepted = valid_shape && ble->frame_active &&
ble->frame_fully_sent_once &&
ble->connected && ble->subscribed &&
ble->connection_handle == connection_handle &&
ble->frame_epoch == ble->delivery_epoch &&
acknowledged_sequence == ble->frame_sequence;
if (accepted) {
ble->ack_received = true;
} else {
++ble->counters.invalid_ack_count;
}
portEXIT_CRITICAL(&ble->lock);
return accepted ? 0 : BLE_ATT_ERR_UNLIKELY;
}
static const struct ble_gatt_svc_def TRIKKE_GATT_SERVICES[] = {
{
.type = BLE_GATT_SVC_TYPE_PRIMARY,
.uuid = &TRIKKE_SERVICE_UUID.u,
.characteristics = (struct ble_gatt_chr_def[]) {
{
.uuid = &TRIKKE_DATA_UUID.u,
.flags = BLE_GATT_CHR_F_NOTIFY,
.val_handle = &s_data_value_handle,
},
{
.uuid = &TRIKKE_ACK_UUID.u,
.access_cb = ack_access,
.flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP,
.val_handle = &s_ack_value_handle,
},
{0},
},
},
{0},
};
static int gap_event(struct ble_gap_event *event, void *argument);
static int advertise(void)
{
struct ble_hs_adv_fields fields = {0};
fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
fields.uuids128 = (ble_uuid128_t *)&TRIKKE_SERVICE_UUID;
fields.num_uuids128 = 1;
fields.uuids128_is_complete = 1;
int result = ble_gap_adv_set_fields(&fields);
if (result != 0) {
return result;
}
const char *name = ble_svc_gap_device_name();
struct ble_hs_adv_fields response = {0};
response.name = (uint8_t *)name;
response.name_len = strlen(name);
response.name_is_complete = 1;
result = ble_gap_adv_rsp_set_fields(&response);
if (result != 0) {
return result;
}
const struct ble_gap_adv_params parameters = {
.conn_mode = BLE_GAP_CONN_MODE_UND,
.disc_mode = BLE_GAP_DISC_MODE_GEN,
};
return ble_gap_adv_start(s_own_address_type, NULL, BLE_HS_FOREVER,
&parameters, gap_event, NULL);
}
static void on_reset(int reason)
{
(void)reason;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL) {
return;
}
portENTER_CRITICAL(&ble->lock);
if (ble->connected) {
++ble->counters.disconnect_count;
}
ble->connected = false;
ble->subscribed = false;
ble->connection_handle = BLE_HS_CONN_HANDLE_NONE;
++ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
}
static void on_sync(void)
{
if (ble_hs_util_ensure_addr(0) != 0 ||
ble_hs_id_infer_auto(0, &s_own_address_type) != 0) {
return;
}
(void)advertise();
}
static int gap_event(struct ble_gap_event *event, void *argument)
{
(void)argument;
trikke_ble_transport_t *ble = s_ble;
if (ble == NULL) {
return 0;
}
switch (event->type) {
case BLE_GAP_EVENT_CONNECT:
if (event->connect.status == 0) {
portENTER_CRITICAL(&ble->lock);
ble->connected = true;
ble->subscribed = false;
ble->connection_handle = event->connect.conn_handle;
++ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
} else {
(void)advertise();
}
return 0;
case BLE_GAP_EVENT_DISCONNECT:
portENTER_CRITICAL(&ble->lock);
if (ble->connected) {
++ble->counters.disconnect_count;
}
ble->connected = false;
ble->subscribed = false;
ble->connection_handle = BLE_HS_CONN_HANDLE_NONE;
++ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
(void)advertise();
return 0;
case BLE_GAP_EVENT_SUBSCRIBE:
if (event->subscribe.attr_handle == s_data_value_handle) {
portENTER_CRITICAL(&ble->lock);
const bool subscribed = event->subscribe.cur_notify != 0;
if (subscribed != ble->subscribed) {
ble->subscribed = subscribed;
++ble->delivery_epoch;
}
portEXIT_CRITICAL(&ble->lock);
}
return 0;
case BLE_GAP_EVENT_ADV_COMPLETE:
(void)advertise();
return 0;
default:
return 0;
}
}
static void host_task(void *argument)
{
(void)argument;
nimble_port_run();
nimble_port_freertos_deinit();
}
static trikke_transport_status_t ble_begin_packet(
void *context,
const uint8_t *packet,
size_t packet_size)
{
trikke_ble_transport_t *ble = context;
if (ble == NULL || packet == NULL ||
packet_size < TRIKKE_WIRE_HEADER_SIZE ||
packet_size > TRIKKE_WIRE_MAX_PACKET_SIZE) {
return TRIKKE_TRANSPORT_FATAL;
}
portENTER_CRITICAL(&ble->lock);
if (!ble->initialized || ble->frame_active) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_FATAL;
}
if (!ble->connected || !ble->subscribed) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_RETRY;
}
ble->frame_active = true;
ble->frame_fully_sent_once = false;
ble->ack_received = false;
ble->frame = packet;
ble->frame_size = packet_size;
ble->next_offset = 0;
ble->ack_deadline_us = 0;
ble->frame_sequence = get_u32_le(packet + 12);
ble->frame_epoch = ble->delivery_epoch;
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
static trikke_transport_status_t ble_poll_once(void *context)
{
trikke_ble_transport_t *ble = context;
if (ble == NULL) {
return TRIKKE_TRANSPORT_FATAL;
}
uint16_t connection_handle = BLE_HS_CONN_HANDLE_NONE;
uint32_t delivery_epoch = 0;
const uint8_t *frame = NULL;
size_t frame_size = 0;
size_t next_offset = 0;
portENTER_CRITICAL(&ble->lock);
if (!ble->initialized || !ble->frame_active) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_FATAL;
}
if (ble->ack_received) {
ble->frame_active = false;
ble->frame_fully_sent_once = false;
ble->ack_received = false;
ble->frame = NULL;
ble->frame_size = 0;
ble->next_offset = 0;
ble->ack_deadline_us = 0;
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_COMPLETE;
}
if (!ble->connected || !ble->subscribed) {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
if (ble->frame_epoch != ble->delivery_epoch) {
if (ble->next_offset != 0) {
++ble->counters.replay_count;
}
ble->next_offset = 0;
ble->frame_epoch = ble->delivery_epoch;
ble->ack_deadline_us = 0;
}
if (ble->next_offset == ble->frame_size) {
if (esp_timer_get_time() >= ble->ack_deadline_us) {
ble->next_offset = 0;
ble->ack_deadline_us = 0;
++ble->counters.replay_count;
} else {
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
}
connection_handle = ble->connection_handle;
delivery_epoch = ble->delivery_epoch;
frame = ble->frame;
frame_size = ble->frame_size;
next_offset = ble->next_offset;
portEXIT_CRITICAL(&ble->lock);
const uint16_t mtu = ble_att_mtu(connection_handle);
if (mtu <= 3 + TRIKKE_BLE_FRAGMENT_HEADER_SIZE) {
return TRIKKE_TRANSPORT_FATAL;
}
size_t att_payload_capacity = mtu - 3;
if (att_payload_capacity > TRIKKE_BLE_MAX_ATT_PAYLOAD) {
att_payload_capacity = TRIKKE_BLE_MAX_ATT_PAYLOAD;
}
uint8_t fragment[TRIKKE_BLE_MAX_ATT_PAYLOAD] = {0};
const size_t fragment_size = trikke_ble_encode_fragment(
fragment, sizeof(fragment), frame, frame_size, next_offset,
att_payload_capacity);
if (fragment_size == 0) {
return TRIKKE_TRANSPORT_FATAL;
}
struct os_mbuf *notification =
ble_hs_mbuf_from_flat(fragment, fragment_size);
int result = BLE_HS_ENOMEM;
if (notification != NULL) {
result = ble_gatts_notify_custom(
connection_handle, s_data_value_handle, notification);
}
if (result != 0) {
portENTER_CRITICAL(&ble->lock);
++ble->counters.send_failure_count;
portEXIT_CRITICAL(&ble->lock);
if (result == BLE_HS_ENOMEM || result == BLE_HS_EBUSY ||
result == BLE_HS_EAGAIN || result == BLE_HS_ENOTCONN) {
return TRIKKE_TRANSPORT_PENDING;
}
return TRIKKE_TRANSPORT_FATAL;
}
const size_t sent_data_size =
fragment_size - TRIKKE_BLE_FRAGMENT_HEADER_SIZE;
portENTER_CRITICAL(&ble->lock);
if (ble->frame_active && ble->connected && ble->subscribed &&
ble->connection_handle == connection_handle &&
ble->delivery_epoch == delivery_epoch &&
ble->next_offset == next_offset) {
ble->next_offset += sent_data_size;
if (ble->next_offset == ble->frame_size) {
ble->frame_fully_sent_once = true;
ble->ack_deadline_us =
esp_timer_get_time() + TRIKKE_BLE_ACK_TIMEOUT_US;
}
}
portEXIT_CRITICAL(&ble->lock);
return TRIKKE_TRANSPORT_PENDING;
}
static trikke_transport_status_t ble_poll_packet(void *context)
{
trikke_ble_transport_t *ble = context;
if (ble == NULL) {
return TRIKKE_TRANSPORT_FATAL;
}
for (unsigned fragment = 0;
fragment < TRIKKE_BLE_FRAGMENTS_PER_POLL;
++fragment) {
portENTER_CRITICAL(&ble->lock);
const size_t offset_before_poll = ble->next_offset;
portEXIT_CRITICAL(&ble->lock);
const trikke_transport_status_t status = ble_poll_once(context);
if (status != TRIKKE_TRANSPORT_PENDING) {
return status;
}
// Keep a small-MTU connection useful without turning one poll into an
// unbounded loop. Stop as soon as the backend is waiting on either the
// connection/subscription or the receiver's application ACK.
portENTER_CRITICAL(&ble->lock);
const bool waiting = !ble->connected || !ble->subscribed ||
ble->next_offset == ble->frame_size;
const bool made_progress = ble->next_offset != offset_before_poll;
portEXIT_CRITICAL(&ble->lock);
if (waiting || !made_progress) {
return TRIKKE_TRANSPORT_PENDING;
}
}
return TRIKKE_TRANSPORT_PENDING;
}
esp_err_t trikke_ble_transport_init(
trikke_ble_transport_t *ble,
trikke_transport_t *transport)
{
if (ble == NULL || transport == NULL) {
return ESP_ERR_INVALID_ARG;
}
if (s_ble != NULL || ble->initialized) {
return ESP_ERR_INVALID_STATE;
}
memset(ble, 0, sizeof(*ble));
ble->lock = (portMUX_TYPE)portMUX_INITIALIZER_UNLOCKED;
ble->connection_handle = BLE_HS_CONN_HANDLE_NONE;
s_ble = ble;
esp_err_t error = nvs_flash_init();
if (error == ESP_ERR_NVS_NO_FREE_PAGES ||
error == ESP_ERR_NVS_NEW_VERSION_FOUND) {
error = nvs_flash_erase();
if (error == ESP_OK) {
error = nvs_flash_init();
}
}
if (error != ESP_OK) {
s_ble = NULL;
return error;
}
error = nimble_port_init();
if (error != ESP_OK) {
s_ble = NULL;
return error;
}
ble_hs_cfg.reset_cb = on_reset;
ble_hs_cfg.sync_cb = on_sync;
ble_svc_gap_init();
ble_svc_gatt_init();
int result = ble_gatts_count_cfg(TRIKKE_GATT_SERVICES);
if (result == 0) {
result = ble_gatts_add_svcs(TRIKKE_GATT_SERVICES);
}
if (result == 0) {
result = ble_svc_gap_device_name_set(TRIKKE_BLE_DEVICE_NAME);
}
if (result != 0) {
(void)nimble_port_deinit();
s_ble = NULL;
return ESP_FAIL;
}
ble->initialized = true;
transport->context = ble;
transport->begin = ble_begin_packet;
transport->poll = ble_poll_packet;
nimble_port_freertos_init(host_task);
return ESP_OK;
}
void trikke_ble_transport_get_counters(
trikke_ble_transport_t *ble,
trikke_ble_transport_counters_t *counters)
{
if (ble == NULL || counters == NULL) {
return;
}
portENTER_CRITICAL(&ble->lock);
*counters = ble->counters;
portEXIT_CRITICAL(&ble->lock);
}
+51
View File
@@ -0,0 +1,51 @@
#pragma once
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#include "trikke_transport.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
uint32_t disconnect_count;
uint32_t send_failure_count;
uint32_t replay_count;
uint32_t invalid_ack_count;
} trikke_ble_transport_counters_t;
typedef struct {
portMUX_TYPE lock;
bool initialized;
bool connected;
bool subscribed;
bool frame_active;
bool frame_fully_sent_once;
bool ack_received;
uint16_t connection_handle;
uint32_t delivery_epoch;
uint32_t frame_epoch;
uint32_t frame_sequence;
const uint8_t *frame;
size_t frame_size;
size_t next_offset;
int64_t ack_deadline_us;
trikke_ble_transport_counters_t counters;
} trikke_ble_transport_t;
esp_err_t trikke_ble_transport_init(
trikke_ble_transport_t *ble,
trikke_transport_t *transport);
void trikke_ble_transport_get_counters(
trikke_ble_transport_t *ble,
trikke_ble_transport_counters_t *counters);
#ifdef __cplusplus
}
#endif
+34
View File
@@ -202,3 +202,37 @@ size_t trikke_encode_sample_packet(
put_u32_le(output + 32, packet_crc32(output, payload_size)); put_u32_le(output + 32, packet_crc32(output, payload_size));
return packet_size; return packet_size;
} }
size_t trikke_encode_status_packet(
uint8_t *output,
size_t output_size,
uint32_t packet_sequence,
int64_t timestamp_us,
uint32_t dropped_sample_count,
uint32_t loop_overrun_count,
const trikke_wire_status_t *status)
{
const size_t packet_size =
TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE;
if (output == NULL || status == NULL || output_size < packet_size) {
return 0;
}
encode_header(output, TRIKKE_PACKET_TYPE_STATUS, 0, 0, 0,
TRIKKE_WIRE_STATUS_SIZE, packet_sequence, timestamp_us,
dropped_sample_count, loop_overrun_count);
uint8_t *payload = output + TRIKKE_WIRE_HEADER_SIZE;
put_u16_le(payload, 1); // Status payload version.
put_u16_le(payload + 2, TRIKKE_WIRE_STATUS_SIZE);
put_u32_le(payload + 4, status->sensor_read_failure_count);
put_u32_le(payload + 8, status->queue_overflow_count);
put_u32_le(payload + 12, status->transport_begin_retry_count);
put_u32_le(payload + 16, status->transport_disconnect_count);
put_u32_le(payload + 20, status->transport_send_failure_count);
put_u32_le(payload + 24, status->transport_replay_count);
put_u32_le(payload + 28, status->transport_invalid_ack_count);
put_u32_le(output + 32, packet_crc32(output, TRIKKE_WIRE_STATUS_SIZE));
return packet_size;
}
+21
View File
@@ -8,6 +8,7 @@
#define TRIKKE_WIRE_HEADER_SIZE 36 #define TRIKKE_WIRE_HEADER_SIZE 36
#define TRIKKE_WIRE_SAMPLE_RECORD_SIZE 20 #define TRIKKE_WIRE_SAMPLE_RECORD_SIZE 20
#define TRIKKE_WIRE_METADATA_SIZE 48 #define TRIKKE_WIRE_METADATA_SIZE 48
#define TRIKKE_WIRE_STATUS_SIZE 32
#define TRIKKE_WIRE_MAX_RECORDS 8 #define TRIKKE_WIRE_MAX_RECORDS 8
#define TRIKKE_WIRE_MAX_PACKET_SIZE \ #define TRIKKE_WIRE_MAX_PACKET_SIZE \
(TRIKKE_WIRE_HEADER_SIZE + \ (TRIKKE_WIRE_HEADER_SIZE + \
@@ -15,6 +16,7 @@
#define TRIKKE_PACKET_TYPE_METADATA 1 #define TRIKKE_PACKET_TYPE_METADATA 1
#define TRIKKE_PACKET_TYPE_SAMPLES 2 #define TRIKKE_PACKET_TYPE_SAMPLES 2
#define TRIKKE_PACKET_TYPE_STATUS 3
#define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01 #define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01
#define TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US 10 #define TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US 10
@@ -50,6 +52,16 @@ typedef struct {
float gyro_mdps_per_lsb; float gyro_mdps_per_lsb;
} trikke_wire_metadata_t; } trikke_wire_metadata_t;
typedef struct {
uint32_t sensor_read_failure_count;
uint32_t queue_overflow_count;
uint32_t transport_begin_retry_count;
uint32_t transport_disconnect_count;
uint32_t transport_send_failure_count;
uint32_t transport_replay_count;
uint32_t transport_invalid_ack_count;
} trikke_wire_status_t;
bool trikke_wire_timestamp_delta_fits( bool trikke_wire_timestamp_delta_fits(
int64_t previous_timestamp_us, int64_t previous_timestamp_us,
int64_t timestamp_us); int64_t timestamp_us);
@@ -71,3 +83,12 @@ size_t trikke_encode_sample_packet(
uint32_t loop_overrun_count, uint32_t loop_overrun_count,
const trikke_wire_sample_t *samples, const trikke_wire_sample_t *samples,
size_t sample_count); size_t sample_count);
size_t trikke_encode_status_packet(
uint8_t *output,
size_t output_size,
uint32_t packet_sequence,
int64_t timestamp_us,
uint32_t dropped_sample_count,
uint32_t loop_overrun_count,
const trikke_wire_status_t *status);
+93 -19
View File
@@ -15,7 +15,11 @@
#include "l3g4200d.h" #include "l3g4200d.h"
#include "trikke_protocol.h" #include "trikke_protocol.h"
#include "trikke_transport.h" #include "trikke_transport.h"
#if CONFIG_TRIKKE_TRANSPORT_BLE
#include "trikke_ble_transport.h"
#else
#include "trikke_usb_transport.h" #include "trikke_usb_transport.h"
#endif
// Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7. // Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7.
#define TRIKKE_I2C_PORT I2C_NUM_0 #define TRIKKE_I2C_PORT I2C_NUM_0
@@ -26,6 +30,7 @@
#define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ) #define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ)
#define TRIKKE_SAMPLE_QUEUE_DEPTH 512 #define TRIKKE_SAMPLE_QUEUE_DEPTH 512
#define TRIKKE_METADATA_INTERVAL_PACKETS 64 #define TRIKKE_METADATA_INTERVAL_PACKETS 64
#define TRIKKE_STATUS_INTERVAL_PACKETS 64
#define TRIKKE_TRANSPORT_RETRY_DELAY_MS 10 #define TRIKKE_TRANSPORT_RETRY_DELAY_MS 10
// Software calibration from the 2026-08-17 enclosure six-face capture. // Software calibration from the 2026-08-17 enclosure six-face capture.
@@ -54,10 +59,15 @@ typedef struct {
adxl345_t accelerometer; adxl345_t accelerometer;
l3g4200d_t gyroscope; l3g4200d_t gyroscope;
QueueHandle_t sample_queue; QueueHandle_t sample_queue;
atomic_uint_least32_t dropped_sample_count; atomic_uint_least32_t sensor_read_failure_count;
atomic_uint_least32_t queue_overflow_count;
atomic_uint_least32_t loop_overrun_count; atomic_uint_least32_t loop_overrun_count;
trikke_transport_t transport; trikke_transport_t transport;
#if CONFIG_TRIKKE_TRANSPORT_BLE
trikke_ble_transport_t ble_transport;
#else
trikke_usb_transport_t usb_transport; trikke_usb_transport_t usb_transport;
#endif
} trikke_context_t; } trikke_context_t;
static trikke_context_t s_context; static trikke_context_t s_context;
@@ -109,6 +119,34 @@ static trikke_axes_sample_t map_gyro_to_enclosure(const l3g4200d_sample_t *nativ
}; };
} }
static uint32_t dropped_sample_count(const trikke_context_t *context)
{
return atomic_load(&context->sensor_read_failure_count) +
atomic_load(&context->queue_overflow_count);
}
static trikke_wire_status_t status_snapshot(
trikke_context_t *context,
const trikke_transport_sender_t *sender)
{
trikke_wire_status_t status = {
.sensor_read_failure_count =
atomic_load(&context->sensor_read_failure_count),
.queue_overflow_count = atomic_load(&context->queue_overflow_count),
.transport_begin_retry_count = sender->begin_retry_count,
};
#if CONFIG_TRIKKE_TRANSPORT_BLE
trikke_ble_transport_counters_t ble_counters = {0};
trikke_ble_transport_get_counters(
&context->ble_transport, &ble_counters);
status.transport_disconnect_count = ble_counters.disconnect_count;
status.transport_send_failure_count = ble_counters.send_failure_count;
status.transport_replay_count = ble_counters.replay_count;
status.transport_invalid_ack_count = ble_counters.invalid_ack_count;
#endif
return status;
}
static void acquisition_task(void *argument) static void acquisition_task(void *argument)
{ {
trikke_context_t *context = argument; trikke_context_t *context = argument;
@@ -146,10 +184,10 @@ static void acquisition_task(void *argument)
.gyro_status = gyro_status, .gyro_status = gyro_status,
}; };
if (xQueueSend(context->sample_queue, &sample, 0) != pdPASS) { if (xQueueSend(context->sample_queue, &sample, 0) != pdPASS) {
atomic_fetch_add(&context->dropped_sample_count, 1); atomic_fetch_add(&context->queue_overflow_count, 1);
} }
} else { } else {
atomic_fetch_add(&context->dropped_sample_count, 1); atomic_fetch_add(&context->sensor_read_failure_count, 1);
} }
++sequence; ++sequence;
@@ -207,10 +245,17 @@ static void output_task(void *argument)
size_t packet_size = trikke_encode_metadata_packet( size_t packet_size = trikke_encode_metadata_packet(
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(), packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
atomic_load(&context->dropped_sample_count), dropped_sample_count(context),
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA); atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
write_binary_packet_until_sent(context, &sender, packet, packet_size); write_binary_packet_until_sent(context, &sender, packet, packet_size);
trikke_wire_status_t status = status_snapshot(context, &sender);
packet_size = trikke_encode_status_packet(
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
dropped_sample_count(context),
atomic_load(&context->loop_overrun_count), &status);
write_binary_packet_until_sent(context, &sender, packet, packet_size);
while (true) { while (true) {
trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0}; trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0};
size_t sample_count = 0; size_t sample_count = 0;
@@ -244,15 +289,26 @@ static void output_task(void *argument)
sample_packet_count % TRIKKE_METADATA_INTERVAL_PACKETS == 0) { sample_packet_count % TRIKKE_METADATA_INTERVAL_PACKETS == 0) {
packet_size = trikke_encode_metadata_packet( packet_size = trikke_encode_metadata_packet(
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(), packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
atomic_load(&context->dropped_sample_count), dropped_sample_count(context),
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA); atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
write_binary_packet_until_sent( write_binary_packet_until_sent(
context, &sender, packet, packet_size); context, &sender, packet, packet_size);
} }
if (sample_packet_count > 0 &&
sample_packet_count % TRIKKE_STATUS_INTERVAL_PACKETS == 0) {
status = status_snapshot(context, &sender);
packet_size = trikke_encode_status_packet(
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
dropped_sample_count(context),
atomic_load(&context->loop_overrun_count), &status);
write_binary_packet_until_sent(
context, &sender, packet, packet_size);
}
packet_size = trikke_encode_sample_packet( packet_size = trikke_encode_sample_packet(
packet, sizeof(packet), packet_sequence++, packet, sizeof(packet), packet_sequence++,
atomic_load(&context->dropped_sample_count), dropped_sample_count(context),
atomic_load(&context->loop_overrun_count), samples, sample_count); atomic_load(&context->loop_overrun_count), samples, sample_count);
write_binary_packet_until_sent(context, &sender, packet, packet_size); write_binary_packet_until_sent(context, &sender, packet, packet_size);
++sample_packet_count; ++sample_packet_count;
@@ -338,27 +394,22 @@ void app_main(void)
"records_per_packet=%d\n", "records_per_packet=%d\n",
TRIKKE_WIRE_VERSION, TRIKKE_WIRE_SAMPLE_RECORD_SIZE, TRIKKE_WIRE_VERSION, TRIKKE_WIRE_SAMPLE_RECORD_SIZE,
TRIKKE_WIRE_MAX_RECORDS); TRIKKE_WIRE_MAX_RECORDS);
#if CONFIG_TRIKKE_TRANSPORT_BLE
printf("# transport=ble,device_name=TrikkeSensor\n");
#else
printf("# transport=usb_serial_jtag\n");
#endif
fflush(stdout); fflush(stdout);
#if !CONFIG_TRIKKE_TRANSPORT_BLE
// The console defaults to CRLF conversion, which would insert bytes into // The console defaults to CRLF conversion, which would insert bytes into
// binary frames whenever a payload byte equals LF. // binary frames whenever a payload byte equals LF.
usb_serial_jtag_vfs_set_tx_line_endings(ESP_LINE_ENDINGS_LF); usb_serial_jtag_vfs_set_tx_line_endings(ESP_LINE_ENDINGS_LF);
#endif
err = trikke_usb_transport_init(
&s_context.usb_transport, &s_context.transport);
if (err != ESP_OK) {
ESP_LOGE(TAG, "USB transport initialization failed: %s",
esp_err_to_name(err));
l3g4200d_deinit(&s_context.gyroscope);
adxl345_deinit(&s_context.accelerometer);
i2c_del_master_bus(bus);
return;
}
s_context.sample_queue = s_context.sample_queue =
xQueueCreate(TRIKKE_SAMPLE_QUEUE_DEPTH, sizeof(trikke_wire_sample_t)); xQueueCreate(TRIKKE_SAMPLE_QUEUE_DEPTH, sizeof(trikke_wire_sample_t));
if (s_context.sample_queue == NULL) { if (s_context.sample_queue == NULL) {
trikke_usb_transport_deinit(&s_context.usb_transport);
ESP_LOGE(TAG, "sample queue allocation failed"); ESP_LOGE(TAG, "sample queue allocation failed");
l3g4200d_deinit(&s_context.gyroscope); l3g4200d_deinit(&s_context.gyroscope);
adxl345_deinit(&s_context.accelerometer); adxl345_deinit(&s_context.accelerometer);
@@ -379,7 +430,6 @@ void app_main(void)
vTaskDelete(acquisition_task_handle); vTaskDelete(acquisition_task_handle);
} }
vQueueDelete(s_context.sample_queue); vQueueDelete(s_context.sample_queue);
trikke_usb_transport_deinit(&s_context.usb_transport);
ESP_LOGE(TAG, "telemetry task creation failed"); ESP_LOGE(TAG, "telemetry task creation failed");
l3g4200d_deinit(&s_context.gyroscope); l3g4200d_deinit(&s_context.gyroscope);
adxl345_deinit(&s_context.accelerometer); adxl345_deinit(&s_context.accelerometer);
@@ -387,6 +437,30 @@ void app_main(void)
return; return;
} }
#if CONFIG_TRIKKE_TRANSPORT_BLE
err = trikke_ble_transport_init(
&s_context.ble_transport, &s_context.transport);
#else
err = trikke_usb_transport_init(
&s_context.usb_transport, &s_context.transport);
#endif
if (err != ESP_OK) {
vTaskDelete(output_task_handle);
vTaskDelete(acquisition_task_handle);
vQueueDelete(s_context.sample_queue);
#if CONFIG_TRIKKE_TRANSPORT_BLE
ESP_LOGE(TAG, "BLE transport initialization failed: %s",
esp_err_to_name(err));
#else
ESP_LOGE(TAG, "USB transport initialization failed: %s",
esp_err_to_name(err));
#endif
l3g4200d_deinit(&s_context.gyroscope);
adxl345_deinit(&s_context.accelerometer);
i2c_del_master_bus(bus);
return;
}
// No text may share the byte stream once framed binary output begins. // No text may share the byte stream once framed binary output begins.
esp_log_level_set("*", ESP_LOG_NONE); esp_log_level_set("*", ESP_LOG_NONE);
xTaskNotifyGive(output_task_handle); xTaskNotifyGive(output_task_handle);
+2
View File
@@ -10,6 +10,7 @@ void trikke_transport_sender_init(trikke_transport_sender_t *sender)
{ {
if (sender != NULL) { if (sender != NULL) {
sender->pending = false; sender->pending = false;
sender->begin_retry_count = 0;
} }
} }
@@ -44,6 +45,7 @@ trikke_transport_status_t trikke_transport_sender_step(
sender->pending = true; sender->pending = true;
return TRIKKE_TRANSPORT_FATAL; return TRIKKE_TRANSPORT_FATAL;
} }
++sender->begin_retry_count;
sender->pending = false; sender->pending = false;
} else { } else {
sender->pending = was_pending; sender->pending = was_pending;
+1
View File
@@ -30,6 +30,7 @@ typedef struct {
typedef struct { typedef struct {
bool pending; bool pending;
uint32_t begin_retry_count;
} trikke_transport_sender_t; } trikke_transport_sender_t;
void trikke_transport_sender_init(trikke_transport_sender_t *sender); void trikke_transport_sender_init(trikke_transport_sender_t *sender);
+2
View File
@@ -0,0 +1,2 @@
bleak>=3.0,<4
pyserial>=3.5,<4
+10
View File
@@ -10,3 +10,13 @@ CONFIG_FREERTOS_HZ=1000
# The XIAO ESP32-C3 carries 4 MB of flash. # The XIAO ESP32-C3 carries 4 MB of flash.
CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
# BLE is the prototype's normal telemetry path. The project Kconfig can switch
# a validation build back to the preserved direct USB transport.
CONFIG_TRIKKE_TRANSPORT_BLE=y
CONFIG_BT_ENABLED=y
CONFIG_BT_NIMBLE_ENABLED=y
CONFIG_BT_NIMBLE_ROLE_CENTRAL=n
CONFIG_BT_NIMBLE_ROLE_OBSERVER=n
CONFIG_BT_NIMBLE_MAX_CONNECTIONS=1
CONFIG_BT_NIMBLE_ATT_PREFERRED_MTU=256
+4
View File
@@ -0,0 +1,4 @@
# Layer this after sdkconfig.defaults for a reproducible wired validation build.
# CONFIG_TRIKKE_TRANSPORT_BLE is not set
CONFIG_TRIKKE_TRANSPORT_USB=y
# CONFIG_BT_ENABLED is not set
+70
View File
@@ -0,0 +1,70 @@
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "trikke_ble_protocol.h"
#include "trikke_protocol.h"
static int fail(int code, const char *message)
{
fprintf(stderr, "BLE protocol fixture failure %d: %s\n", code, message);
return code;
}
static void put_u16_le(uint8_t *output, uint16_t value)
{
output[0] = (uint8_t)value;
output[1] = (uint8_t)(value >> 8);
}
static void put_u32_le(uint8_t *output, uint32_t value)
{
output[0] = (uint8_t)value;
output[1] = (uint8_t)(value >> 8);
output[2] = (uint8_t)(value >> 16);
output[3] = (uint8_t)(value >> 24);
}
int main(void)
{
uint8_t packet[TRIKKE_WIRE_HEADER_SIZE + 16] = {0};
memcpy(packet, "TRK1", 4);
packet[4] = TRIKKE_WIRE_VERSION;
packet[6] = TRIKKE_WIRE_HEADER_SIZE;
put_u16_le(packet + 10, 16);
put_u32_le(packet + 12, 0x78563412);
for (size_t i = TRIKKE_WIRE_HEADER_SIZE; i < sizeof(packet); ++i) {
packet[i] = (uint8_t)i;
}
uint8_t fragment[32] = {0};
size_t size = trikke_ble_encode_fragment(
fragment, sizeof(fragment), packet, sizeof(packet), 0, 20);
if (size != 20 || memcmp(fragment, "\x12\x34\x56\x78\x00\x00\x34\x00", 8) != 0 ||
memcmp(fragment + 8, packet, 12) != 0) {
return fail(1, "first fragment envelope");
}
size = trikke_ble_encode_fragment(
fragment, sizeof(fragment), packet, sizeof(packet), 48, 20);
if (size != 12 || fragment[4] != 48 ||
memcmp(fragment + 8, packet + 48, 4) != 0) {
return fail(2, "last fragment envelope");
}
uint8_t ack[TRIKKE_BLE_ACK_SIZE] = {'A', 'C', 'K', '1', 0x12, 0x34, 0x56, 0x78};
uint32_t sequence = 0;
if (!trikke_ble_decode_ack(ack, sizeof(ack), &sequence) ||
sequence != 0x78563412) {
return fail(3, "ACK decoding");
}
ack[0] = 'N';
if (trikke_ble_decode_ack(ack, sizeof(ack), &sequence) ||
trikke_ble_encode_fragment(fragment, sizeof(fragment), packet,
sizeof(packet), sizeof(packet), 20) != 0 ||
trikke_ble_encode_fragment(fragment, sizeof(fragment), packet,
sizeof(packet), 0, 8) != 0) {
return fail(4, "invalid input rejection");
}
return 0;
}
+22 -3
View File
@@ -88,10 +88,26 @@ int main(void)
return fail(4, "saturated timestamp encoding or output"); return fail(4, "saturated timestamp encoding or output");
} }
const trikke_wire_status_t status = {
.sensor_read_failure_count = 5,
.queue_overflow_count = 6,
.transport_begin_retry_count = 7,
.transport_disconnect_count = 8,
.transport_send_failure_count = 9,
.transport_replay_count = 10,
.transport_invalid_ack_count = 11,
};
size = trikke_encode_status_packet(packet, sizeof(packet), 45, 5000000,
11, 12, &status);
if (size != TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE ||
fwrite(packet, 1, size, stdout) != size) {
return fail(5, "status encoding or output");
}
if (!trikke_wire_timestamp_delta_fits(0, 655350) || if (!trikke_wire_timestamp_delta_fits(0, 655350) ||
trikke_wire_timestamp_delta_fits(0, 655351) || trikke_wire_timestamp_delta_fits(0, 655351) ||
trikke_wire_timestamp_delta_fits(1, 0)) { trikke_wire_timestamp_delta_fits(1, 0)) {
return fail(5, "timestamp-delta boundary contract"); return fail(6, "timestamp-delta boundary contract");
} }
if (trikke_encode_metadata_packet( if (trikke_encode_metadata_packet(
packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE - 1, packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE - 1,
@@ -103,8 +119,11 @@ int main(void)
TRIKKE_WIRE_MAX_RECORDS + 1) != 0 || TRIKKE_WIRE_MAX_RECORDS + 1) != 0 ||
trikke_encode_sample_packet(packet, TRIKKE_WIRE_MAX_PACKET_SIZE - 1, trikke_encode_sample_packet(packet, TRIKKE_WIRE_MAX_PACKET_SIZE - 1,
0, 0, 0, full_packet, 0, 0, 0, full_packet,
TRIKKE_WIRE_MAX_RECORDS) != 0) { TRIKKE_WIRE_MAX_RECORDS) != 0 ||
return fail(6, "invalid argument rejection contract"); trikke_encode_status_packet(
packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE - 1,
0, 0, 0, 0, &status) != 0) {
return fail(7, "invalid argument rejection contract");
} }
return 0; return 0;
} }
+73 -6
View File
@@ -14,9 +14,11 @@ from trikke_protocol import ( # noqa: E402
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED, PACKET_FLAG_TIMESTAMP_DELTA_SATURATED,
PACKET_TYPE_METADATA, PACKET_TYPE_METADATA,
PACKET_TYPE_SAMPLES, PACKET_TYPE_SAMPLES,
PACKET_TYPE_STATUS,
StreamParser, StreamParser,
sample_to_csv_row, sample_to_csv_row,
) )
from trikke_ble import BleFrameReassembler, encode_ack # noqa: E402
class ProtocolContractTest(unittest.TestCase): class ProtocolContractTest(unittest.TestCase):
@@ -79,6 +81,34 @@ class ProtocolContractTest(unittest.TestCase):
) )
cls.transport_fixture_passed = True cls.transport_fixture_passed = True
ble_protocol_executable = Path(cls.tempdir.name) / "ble_protocol_fixture"
subprocess.run(
[
compiler,
"-std=c11",
"-Wall",
"-Wextra",
"-Werror",
"-I",
str(ROOT / "main"),
str(ROOT / "main" / "trikke_ble_protocol.c"),
str(ROOT / "tests" / "ble_protocol_fixture.c"),
"-o",
str(ble_protocol_executable),
],
check=True,
)
ble_protocol_fixture = subprocess.run(
[str(ble_protocol_executable)], capture_output=True
)
if ble_protocol_fixture.returncode != 0:
stderr = ble_protocol_fixture.stderr.decode(errors="replace").strip()
raise AssertionError(
"BLE protocol fixture exited "
f"{ble_protocol_fixture.returncode}: {stderr}"
)
cls.ble_protocol_fixture_passed = True
@classmethod @classmethod
def tearDownClass(cls) -> None: def tearDownClass(cls) -> None:
cls.tempdir.cleanup() cls.tempdir.cleanup()
@@ -90,8 +120,8 @@ class ProtocolContractTest(unittest.TestCase):
for offset in range(0, len(stream), 7): for offset in range(0, len(stream), 7):
frames.extend(parser.feed(stream[offset : offset + 7])) frames.extend(parser.feed(stream[offset : offset + 7]))
self.assertEqual(4, len(frames)) self.assertEqual(5, len(frames))
metadata_frame, sample_frame, full_frame, saturated_frame = frames metadata_frame, sample_frame, full_frame, saturated_frame, status_frame = frames
self.assertEqual(PACKET_TYPE_METADATA, metadata_frame.packet_type) self.assertEqual(PACKET_TYPE_METADATA, metadata_frame.packet_type)
self.assertEqual(41, metadata_frame.packet_sequence) self.assertEqual(41, metadata_frame.packet_sequence)
self.assertEqual(2, metadata_frame.dropped_sample_count) self.assertEqual(2, metadata_frame.dropped_sample_count)
@@ -125,6 +155,16 @@ class ProtocolContractTest(unittest.TestCase):
) )
self.assertEqual(4_655_350, saturated_frame.samples[-1].timestamp_us) self.assertEqual(4_655_350, saturated_frame.samples[-1].timestamp_us)
self.assertEqual(PACKET_TYPE_STATUS, status_frame.packet_type)
self.assertEqual(45, status_frame.packet_sequence)
self.assertEqual(5, status_frame.status.sensor_read_failure_count)
self.assertEqual(6, status_frame.status.queue_overflow_count)
self.assertEqual(7, status_frame.status.transport_begin_retry_count)
self.assertEqual(8, status_frame.status.transport_disconnect_count)
self.assertEqual(9, status_frame.status.transport_send_failure_count)
self.assertEqual(10, status_frame.status.transport_replay_count)
self.assertEqual(11, status_frame.status.transport_invalid_ack_count)
row = sample_to_csv_row( row = sample_to_csv_row(
sample_frame.samples[0], metadata_frame.metadata, 3 sample_frame.samples[0], metadata_frame.metadata, 3
) )
@@ -135,6 +175,33 @@ class ProtocolContractTest(unittest.TestCase):
def test_transport_state_machine_contract(self) -> None: def test_transport_state_machine_contract(self) -> None:
self.assertTrue(self.transport_fixture_passed) self.assertTrue(self.transport_fixture_passed)
def test_ble_fragment_and_ack_contract(self) -> None:
self.assertTrue(self.ble_protocol_fixture_passed)
def test_ble_reassembly_and_replay_contract(self) -> None:
frame = self.encoded[: 36 + 48]
sequence = int.from_bytes(frame[12:16], "little")
def fragment(offset: int, size: int) -> bytes:
data = frame[offset : offset + size]
return (
sequence.to_bytes(4, "little")
+ offset.to_bytes(2, "little")
+ len(frame).to_bytes(2, "little")
+ data
)
reassembler = BleFrameReassembler()
self.assertIsNone(reassembler.feed(fragment(0, 20)))
# A replay from offset zero discards the partial attempt cleanly.
self.assertIsNone(reassembler.feed(fragment(0, 40)))
self.assertEqual(frame, reassembler.feed(fragment(40, len(frame) - 40)))
self.assertEqual(b"ACK1" + sequence.to_bytes(4, "little"), encode_ack(sequence))
self.assertEqual(0, reassembler.rejected_fragment_count)
self.assertIsNone(reassembler.feed(fragment(20, 20)))
self.assertEqual(1, reassembler.rejected_fragment_count)
def test_integrity_sequence_wrap_classification(self) -> None: def test_integrity_sequence_wrap_classification(self) -> None:
self.assertEqual( self.assertEqual(
(0, 0), IntegrityTracker._classify_sequence(0xFFFFFFFF, 0) (0, 0), IntegrityTracker._classify_sequence(0xFFFFFFFF, 0)
@@ -152,7 +219,7 @@ class ProtocolContractTest(unittest.TestCase):
frames = parser.feed(bytes(damaged) + self.encoded[first_size:]) frames = parser.feed(bytes(damaged) + self.encoded[first_size:])
self.assertEqual(1, parser.startup_crc_errors) self.assertEqual(1, parser.startup_crc_errors)
self.assertEqual(0, parser.crc_errors) self.assertEqual(0, parser.crc_errors)
self.assertEqual(3, len(frames)) self.assertEqual(4, len(frames))
self.assertEqual(PACKET_TYPE_SAMPLES, frames[0].packet_type) self.assertEqual(PACKET_TYPE_SAMPLES, frames[0].packet_type)
def test_crc_failure_after_sync_is_stream_error(self) -> None: def test_crc_failure_after_sync_is_stream_error(self) -> None:
@@ -168,14 +235,14 @@ class ProtocolContractTest(unittest.TestCase):
) )
self.assertEqual(0, parser.startup_crc_errors) self.assertEqual(0, parser.startup_crc_errors)
self.assertEqual(1, parser.crc_errors) self.assertEqual(1, parser.crc_errors)
self.assertEqual(3, len(frames)) self.assertEqual(4, len(frames))
self.assertEqual(PACKET_TYPE_METADATA, frames[0].packet_type) self.assertEqual(PACKET_TYPE_METADATA, frames[0].packet_type)
def test_trailing_partial_frame_is_observable(self) -> None: def test_trailing_partial_frame_is_observable(self) -> None:
parser = StreamParser() parser = StreamParser()
frames = parser.feed(self.encoded[:-5]) frames = parser.feed(self.encoded[:-5])
self.assertEqual(3, len(frames)) self.assertEqual(4, len(frames))
self.assertEqual(36 + 2 * 20 - 5, parser.buffered_bytes) self.assertEqual(36 + 32 - 5, parser.buffered_bytes)
def test_hardware_outage_validation_artifacts(self) -> None: def test_hardware_outage_validation_artifacts(self) -> None:
expected = { expected = {
+12
View File
@@ -175,6 +175,18 @@ def main() -> int:
f"dropped={integrity.final_dropped_sample_count}, " f"dropped={integrity.final_dropped_sample_count}, "
f"acquisition_loop_overruns={integrity.final_loop_overrun_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}" saved = f"Saved {output} and {csv_output}"
if args.wire is not None: if args.wire is not None:
saved += f"; raw wire saved to {args.wire}" saved += f"; raw wire saved to {args.wire}"
+220
View File
@@ -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())
+12
View File
@@ -76,6 +76,18 @@ def main() -> int:
f"acquisition_loop_overruns={integrity.final_loop_overrun_count}, " f"acquisition_loop_overruns={integrity.final_loop_overrun_count}, "
f"trailing_partial_bytes={stream.buffered_bytes}; saved {args.output}" 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 return 0
+66
View File
@@ -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)
+45 -3
View File
@@ -12,15 +12,18 @@ VERSION = 1
HEADER_SIZE = 36 HEADER_SIZE = 36
SAMPLE_RECORD_SIZE = 20 SAMPLE_RECORD_SIZE = 20
METADATA_SIZE = 48 METADATA_SIZE = 48
STATUS_SIZE = 32
MAX_RECORDS = 8 MAX_RECORDS = 8
PACKET_TYPE_METADATA = 1 PACKET_TYPE_METADATA = 1
PACKET_TYPE_SAMPLES = 2 PACKET_TYPE_SAMPLES = 2
PACKET_TYPE_STATUS = 3
PACKET_FLAG_TIMESTAMP_DELTA_SATURATED = 0x01 PACKET_FLAG_TIMESTAMP_DELTA_SATURATED = 0x01
HEADER = struct.Struct("<4sBBBBBBHIQIII") HEADER = struct.Struct("<4sBBBBBBHIQIII")
METADATA = struct.Struct("<HHHH10f") METADATA = struct.Struct("<HHHH10f")
SAMPLE = struct.Struct("<IHhhhhhhBB") SAMPLE = struct.Struct("<IHhhhhhhBB")
STATUS = struct.Struct("<HH7I")
CSV_COLUMNS = [ CSV_COLUMNS = [
"sequence", "sequence",
@@ -71,6 +74,17 @@ class Sample:
gyro_status: int 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) @dataclass(frozen=True)
class Frame: class Frame:
packet_type: int packet_type: int
@@ -80,6 +94,7 @@ class Frame:
dropped_sample_count: int dropped_sample_count: int
loop_overrun_count: int loop_overrun_count: int
metadata: Metadata | None metadata: Metadata | None
status: Status | None
samples: tuple[Sample, ...] samples: tuple[Sample, ...]
raw: bytes raw: bytes
@@ -98,6 +113,7 @@ class IntegrityTracker:
gyro_overrun_count: int = 0 gyro_overrun_count: int = 0
final_dropped_sample_count: int = 0 final_dropped_sample_count: int = 0
final_loop_overrun_count: int = 0 final_loop_overrun_count: int = 0
final_status: Status | None = None
_previous_packet_sequence: int | None = None _previous_packet_sequence: int | None = None
_previous_sample_sequence: int | None = None _previous_sample_sequence: int | None = None
_previous_timestamp_us: int | None = None _previous_timestamp_us: int | None = None
@@ -128,6 +144,8 @@ class IntegrityTracker:
self.timestamp_saturation_frame_count += 1 self.timestamp_saturation_frame_count += 1
self.final_dropped_sample_count = frame.dropped_sample_count self.final_dropped_sample_count = frame.dropped_sample_count
self.final_loop_overrun_count = frame.loop_overrun_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: for sample in frame.samples:
if self._previous_sample_sequence is not None: if self._previous_sample_sequence is not None:
@@ -241,8 +259,16 @@ class StreamParser:
valid_shape = ( valid_shape = (
version == VERSION version == VERSION
and header_size == HEADER_SIZE and header_size == HEADER_SIZE
and packet_type in (PACKET_TYPE_METADATA, PACKET_TYPE_SAMPLES) and packet_type in (
and payload_size <= max(METADATA_SIZE, SAMPLE_RECORD_SIZE * MAX_RECORDS) 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: if packet_type == PACKET_TYPE_METADATA:
valid_shape = valid_shape and ( valid_shape = valid_shape and (
@@ -254,6 +280,12 @@ class StreamParser:
and 1 <= record_count <= MAX_RECORDS and 1 <= record_count <= MAX_RECORDS
and payload_size == record_size * record_count 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: if not valid_shape:
self.header_errors += 1 self.header_errors += 1
self.skipped_bytes += 1 self.skipped_bytes += 1
@@ -276,6 +308,7 @@ class StreamParser:
continue continue
metadata = None metadata = None
status = None
samples: tuple[Sample, ...] = () samples: tuple[Sample, ...] = ()
payload = raw[HEADER_SIZE:] payload = raw[HEADER_SIZE:]
if packet_type == PACKET_TYPE_METADATA: if packet_type == PACKET_TYPE_METADATA:
@@ -290,7 +323,7 @@ class StreamParser:
gyro_bias_counts=values[10:13], gyro_bias_counts=values[10:13],
gyro_mdps_per_lsb=values[13], gyro_mdps_per_lsb=values[13],
) )
else: elif packet_type == PACKET_TYPE_SAMPLES:
decoded: list[Sample] = [] decoded: list[Sample] = []
timestamp_us = base_timestamp_us timestamp_us = base_timestamp_us
for index in range(record_count): for index in range(record_count):
@@ -308,6 +341,14 @@ class StreamParser:
) )
) )
samples = tuple(decoded) 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( frames.append(
Frame( Frame(
@@ -318,6 +359,7 @@ class StreamParser:
dropped_sample_count=dropped_sample_count, dropped_sample_count=dropped_sample_count,
loop_overrun_count=loop_overrun_count, loop_overrun_count=loop_overrun_count,
metadata=metadata, metadata=metadata,
status=status,
samples=samples, samples=samples,
raw=raw, raw=raw,
) )