diff --git a/.gitignore b/.gitignore index 96c6735..e01875c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ build/ +build-usb/ sdkconfig captures/ sdkconfig.old diff --git a/README.md b/README.md index f9e115d..aced1af 100644 --- a/README.md +++ b/README.md @@ -7,11 +7,14 @@ This milestone does four things: 1. Detects and verifies both sensors by their identification registers. 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 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 @@ -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`. - 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 @@ -73,7 +76,14 @@ records into versioned `TRK1` frames, isolating acquisition from brief transport stalls. CRC, packet and sample sequences, timestamps, and cumulative 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 the driver ring and remains pending across bounded drain timeouts; firmware does 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 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 -transport milestone. +the fact. BLE `COMPLETE` instead means the receiver acknowledged the exact frame +after persistence. Measured end-to-end framing overhead is about 2.47 kB/s at 100 Hz, or 8.47 MiB/hour before BLE link overhead. @@ -103,7 +113,27 @@ idf.py build 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 20-byte record containing mapped raw sensor counts, timing, sequence, and the two diff --git a/docs/binary-record-v1.md b/docs/binary-record-v1.md index c9d2980..cf95b78 100644 --- a/docs/binary-record-v1.md +++ b/docs/binary-record-v1.md @@ -11,7 +11,7 @@ combined by an underlying byte transport. | ---: | ---: | --- | | 0 | 4 | ASCII magic `TRK1` | | 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`) | | 7 | 1 | Record size (`0` or `20`) | | 8 | 1 | Record count (`0` or 1–8) | @@ -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 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 -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 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 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 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 the endpoint accepts them. CRC and sequence checks make resulting loss visible, but an application acknowledgement and replay window are still required to -guarantee receipt. Accordingly, USB `COMPLETE` means endpoint drain, while the -planned reliable BLE backend will reserve `COMPLETE` for an application ACK of -the exact frame. +guarantee receipt. Accordingly, USB `COMPLETE` means endpoint drain, while +reliable BLE reserves `COMPLETE` for an application ACK of the exact frame. See +`ble-transport-v1.md` for fragmentation, replay, and UUIDs. 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. diff --git a/docs/ble-transport-v1.md b/docs/ble-transport-v1.md new file mode 100644 index 0000000..f3751fb --- /dev/null +++ b/docs/ble-transport-v1.md @@ -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. diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index b07d56c..39411da 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -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( - SRCS "trikke_sensor_main.c" "trikke_protocol.c" "trikke_transport.c" - "trikke_usb_transport.c" + SRCS ${trikke_sources} INCLUDE_DIRS "." - REQUIRES adxl345 l3g4200d esp_timer esp_driver_gpio esp_driver_i2c - esp_driver_usb_serial_jtag vfs + REQUIRES ${trikke_requires} ) diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild new file mode 100644 index 0000000..8943f50 --- /dev/null +++ b/main/Kconfig.projbuild @@ -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 diff --git a/main/trikke_ble_protocol.c b/main/trikke_ble_protocol.c new file mode 100644 index 0000000..72a66a9 --- /dev/null +++ b/main/trikke_ble_protocol.c @@ -0,0 +1,91 @@ +#include "trikke_ble_protocol.h" + +#include + +#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; +} diff --git a/main/trikke_ble_protocol.h b/main/trikke_ble_protocol.h new file mode 100644 index 0000000..ae01823 --- /dev/null +++ b/main/trikke_ble_protocol.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include + +#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 diff --git a/main/trikke_ble_transport.c b/main/trikke_ble_transport.c new file mode 100644 index 0000000..ddf1e33 --- /dev/null +++ b/main/trikke_ble_transport.c @@ -0,0 +1,479 @@ +#include "trikke_ble_transport.h" + +#include + +#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, + ¶meters, 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); +} diff --git a/main/trikke_ble_transport.h b/main/trikke_ble_transport.h new file mode 100644 index 0000000..7b49ce0 --- /dev/null +++ b/main/trikke_ble_transport.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include + +#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 diff --git a/main/trikke_protocol.c b/main/trikke_protocol.c index 87ad08c..d376a6d 100644 --- a/main/trikke_protocol.c +++ b/main/trikke_protocol.c @@ -202,3 +202,37 @@ size_t trikke_encode_sample_packet( put_u32_le(output + 32, packet_crc32(output, payload_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; +} diff --git a/main/trikke_protocol.h b/main/trikke_protocol.h index 78d8d95..43677eb 100644 --- a/main/trikke_protocol.h +++ b/main/trikke_protocol.h @@ -8,6 +8,7 @@ #define TRIKKE_WIRE_HEADER_SIZE 36 #define TRIKKE_WIRE_SAMPLE_RECORD_SIZE 20 #define TRIKKE_WIRE_METADATA_SIZE 48 +#define TRIKKE_WIRE_STATUS_SIZE 32 #define TRIKKE_WIRE_MAX_RECORDS 8 #define TRIKKE_WIRE_MAX_PACKET_SIZE \ (TRIKKE_WIRE_HEADER_SIZE + \ @@ -15,6 +16,7 @@ #define TRIKKE_PACKET_TYPE_METADATA 1 #define TRIKKE_PACKET_TYPE_SAMPLES 2 +#define TRIKKE_PACKET_TYPE_STATUS 3 #define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01 #define TRIKKE_WIRE_TIMESTAMP_DELTA_UNIT_US 10 @@ -50,6 +52,16 @@ typedef struct { float gyro_mdps_per_lsb; } 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( int64_t previous_timestamp_us, int64_t timestamp_us); @@ -71,3 +83,12 @@ size_t trikke_encode_sample_packet( uint32_t loop_overrun_count, const trikke_wire_sample_t *samples, 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); diff --git a/main/trikke_sensor_main.c b/main/trikke_sensor_main.c index 5c0b709..97b2811 100644 --- a/main/trikke_sensor_main.c +++ b/main/trikke_sensor_main.c @@ -15,7 +15,11 @@ #include "l3g4200d.h" #include "trikke_protocol.h" #include "trikke_transport.h" +#if CONFIG_TRIKKE_TRANSPORT_BLE +#include "trikke_ble_transport.h" +#else #include "trikke_usb_transport.h" +#endif // Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7. #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_QUEUE_DEPTH 512 #define TRIKKE_METADATA_INTERVAL_PACKETS 64 +#define TRIKKE_STATUS_INTERVAL_PACKETS 64 #define TRIKKE_TRANSPORT_RETRY_DELAY_MS 10 // Software calibration from the 2026-08-17 enclosure six-face capture. @@ -54,10 +59,15 @@ typedef struct { adxl345_t accelerometer; l3g4200d_t gyroscope; 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; trikke_transport_t transport; +#if CONFIG_TRIKKE_TRANSPORT_BLE + trikke_ble_transport_t ble_transport; +#else trikke_usb_transport_t usb_transport; +#endif } trikke_context_t; 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) { trikke_context_t *context = argument; @@ -146,10 +184,10 @@ static void acquisition_task(void *argument) .gyro_status = gyro_status, }; 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 { - atomic_fetch_add(&context->dropped_sample_count, 1); + atomic_fetch_add(&context->sensor_read_failure_count, 1); } ++sequence; @@ -207,10 +245,17 @@ static void output_task(void *argument) size_t packet_size = trikke_encode_metadata_packet( 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); 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) { trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0}; size_t sample_count = 0; @@ -244,15 +289,26 @@ static void output_task(void *argument) sample_packet_count % TRIKKE_METADATA_INTERVAL_PACKETS == 0) { packet_size = trikke_encode_metadata_packet( 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); write_binary_packet_until_sent( 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, sizeof(packet), packet_sequence++, - atomic_load(&context->dropped_sample_count), + dropped_sample_count(context), atomic_load(&context->loop_overrun_count), samples, sample_count); write_binary_packet_until_sent(context, &sender, packet, packet_size); ++sample_packet_count; @@ -338,27 +394,22 @@ void app_main(void) "records_per_packet=%d\n", TRIKKE_WIRE_VERSION, TRIKKE_WIRE_SAMPLE_RECORD_SIZE, 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); +#if !CONFIG_TRIKKE_TRANSPORT_BLE // The console defaults to CRLF conversion, which would insert bytes into // binary frames whenever a payload byte equals LF. usb_serial_jtag_vfs_set_tx_line_endings(ESP_LINE_ENDINGS_LF); - - 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; - } +#endif s_context.sample_queue = xQueueCreate(TRIKKE_SAMPLE_QUEUE_DEPTH, sizeof(trikke_wire_sample_t)); if (s_context.sample_queue == NULL) { - trikke_usb_transport_deinit(&s_context.usb_transport); ESP_LOGE(TAG, "sample queue allocation failed"); l3g4200d_deinit(&s_context.gyroscope); adxl345_deinit(&s_context.accelerometer); @@ -379,7 +430,6 @@ void app_main(void) vTaskDelete(acquisition_task_handle); } vQueueDelete(s_context.sample_queue); - trikke_usb_transport_deinit(&s_context.usb_transport); ESP_LOGE(TAG, "telemetry task creation failed"); l3g4200d_deinit(&s_context.gyroscope); adxl345_deinit(&s_context.accelerometer); @@ -387,6 +437,30 @@ void app_main(void) 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. esp_log_level_set("*", ESP_LOG_NONE); xTaskNotifyGive(output_task_handle); diff --git a/main/trikke_transport.c b/main/trikke_transport.c index 7d81889..2d9469f 100644 --- a/main/trikke_transport.c +++ b/main/trikke_transport.c @@ -10,6 +10,7 @@ void trikke_transport_sender_init(trikke_transport_sender_t *sender) { if (sender != NULL) { sender->pending = false; + sender->begin_retry_count = 0; } } @@ -44,6 +45,7 @@ trikke_transport_status_t trikke_transport_sender_step( sender->pending = true; return TRIKKE_TRANSPORT_FATAL; } + ++sender->begin_retry_count; sender->pending = false; } else { sender->pending = was_pending; diff --git a/main/trikke_transport.h b/main/trikke_transport.h index 2e295ba..0bca5d4 100644 --- a/main/trikke_transport.h +++ b/main/trikke_transport.h @@ -30,6 +30,7 @@ typedef struct { typedef struct { bool pending; + uint32_t begin_retry_count; } trikke_transport_sender_t; void trikke_transport_sender_init(trikke_transport_sender_t *sender); diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..4935c30 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,2 @@ +bleak>=3.0,<4 +pyserial>=3.5,<4 diff --git a/sdkconfig.defaults b/sdkconfig.defaults index 74cd29c..536748e 100644 --- a/sdkconfig.defaults +++ b/sdkconfig.defaults @@ -10,3 +10,13 @@ CONFIG_FREERTOS_HZ=1000 # The XIAO ESP32-C3 carries 4 MB of flash. 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 diff --git a/sdkconfig.usb.defaults b/sdkconfig.usb.defaults new file mode 100644 index 0000000..4f3b42a --- /dev/null +++ b/sdkconfig.usb.defaults @@ -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 diff --git a/tests/ble_protocol_fixture.c b/tests/ble_protocol_fixture.c new file mode 100644 index 0000000..45e8e54 --- /dev/null +++ b/tests/ble_protocol_fixture.c @@ -0,0 +1,70 @@ +#include +#include +#include + +#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; +} diff --git a/tests/protocol_fixture.c b/tests/protocol_fixture.c index 9ebe68c..49d5e46 100644 --- a/tests/protocol_fixture.c +++ b/tests/protocol_fixture.c @@ -88,10 +88,26 @@ int main(void) 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) || trikke_wire_timestamp_delta_fits(0, 655351) || 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( packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE - 1, @@ -103,8 +119,11 @@ int main(void) TRIKKE_WIRE_MAX_RECORDS + 1) != 0 || trikke_encode_sample_packet(packet, TRIKKE_WIRE_MAX_PACKET_SIZE - 1, 0, 0, 0, full_packet, - TRIKKE_WIRE_MAX_RECORDS) != 0) { - return fail(6, "invalid argument rejection contract"); + TRIKKE_WIRE_MAX_RECORDS) != 0 || + 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; } diff --git a/tests/test_trikke_protocol.py b/tests/test_trikke_protocol.py index 1b066eb..cc07c30 100644 --- a/tests/test_trikke_protocol.py +++ b/tests/test_trikke_protocol.py @@ -14,9 +14,11 @@ from trikke_protocol import ( # noqa: E402 PACKET_FLAG_TIMESTAMP_DELTA_SATURATED, PACKET_TYPE_METADATA, PACKET_TYPE_SAMPLES, + PACKET_TYPE_STATUS, StreamParser, sample_to_csv_row, ) +from trikke_ble import BleFrameReassembler, encode_ack # noqa: E402 class ProtocolContractTest(unittest.TestCase): @@ -79,6 +81,34 @@ class ProtocolContractTest(unittest.TestCase): ) 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 def tearDownClass(cls) -> None: cls.tempdir.cleanup() @@ -90,8 +120,8 @@ class ProtocolContractTest(unittest.TestCase): for offset in range(0, len(stream), 7): frames.extend(parser.feed(stream[offset : offset + 7])) - self.assertEqual(4, len(frames)) - metadata_frame, sample_frame, full_frame, saturated_frame = frames + self.assertEqual(5, len(frames)) + metadata_frame, sample_frame, full_frame, saturated_frame, status_frame = frames self.assertEqual(PACKET_TYPE_METADATA, metadata_frame.packet_type) self.assertEqual(41, metadata_frame.packet_sequence) 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(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( sample_frame.samples[0], metadata_frame.metadata, 3 ) @@ -135,6 +175,33 @@ class ProtocolContractTest(unittest.TestCase): def test_transport_state_machine_contract(self) -> None: 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: self.assertEqual( (0, 0), IntegrityTracker._classify_sequence(0xFFFFFFFF, 0) @@ -152,7 +219,7 @@ class ProtocolContractTest(unittest.TestCase): frames = parser.feed(bytes(damaged) + self.encoded[first_size:]) self.assertEqual(1, parser.startup_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) 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(1, parser.crc_errors) - self.assertEqual(3, len(frames)) + self.assertEqual(4, len(frames)) self.assertEqual(PACKET_TYPE_METADATA, frames[0].packet_type) def test_trailing_partial_frame_is_observable(self) -> None: parser = StreamParser() frames = parser.feed(self.encoded[:-5]) - self.assertEqual(3, len(frames)) - self.assertEqual(36 + 2 * 20 - 5, parser.buffered_bytes) + self.assertEqual(4, len(frames)) + self.assertEqual(36 + 32 - 5, parser.buffered_bytes) def test_hardware_outage_validation_artifacts(self) -> None: expected = { diff --git a/tools/capture_binary.py b/tools/capture_binary.py index a8f1dcb..298eaf4 100644 --- a/tools/capture_binary.py +++ b/tools/capture_binary.py @@ -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}" diff --git a/tools/capture_ble.py b/tools/capture_ble.py new file mode 100644 index 0000000..b896e86 --- /dev/null +++ b/tools/capture_ble.py @@ -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()) diff --git a/tools/decode_binary.py b/tools/decode_binary.py index f86a204..54acea5 100644 --- a/tools/decode_binary.py +++ b/tools/decode_binary.py @@ -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 diff --git a/tools/trikke_ble.py b/tools/trikke_ble.py new file mode 100644 index 0000000..de7c114 --- /dev/null +++ b/tools/trikke_ble.py @@ -0,0 +1,66 @@ +"""BLE fragment reassembly and acknowledgement helpers for TRK1 frames.""" + +from __future__ import annotations + +import struct + +BLE_FRAGMENT_HEADER = struct.Struct(" 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) diff --git a/tools/trikke_protocol.py b/tools/trikke_protocol.py index a3dad5f..3688bc4 100644 --- a/tools/trikke_protocol.py +++ b/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("