add reliable BLE telemetry transport
This commit is contained in:
@@ -1,4 +1,5 @@
|
|||||||
build/
|
build/
|
||||||
|
build-usb/
|
||||||
sdkconfig
|
sdkconfig
|
||||||
captures/
|
captures/
|
||||||
sdkconfig.old
|
sdkconfig.old
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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 1–8) |
|
| 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
|
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.
|
||||||
|
|||||||
@@ -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
@@ -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
|
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -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
|
||||||
@@ -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,
|
||||||
|
¶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);
|
||||||
|
}
|
||||||
@@ -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
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
|
|||||||
@@ -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
@@ -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);
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -0,0 +1,2 @@
|
|||||||
|
bleak>=3.0,<4
|
||||||
|
pyserial>=3.5,<4
|
||||||
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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 = {
|
||||||
|
|||||||
@@ -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}"
|
||||||
|
|||||||
@@ -0,0 +1,220 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Capture acknowledged TRK1 telemetry from the Trikke BLE service."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import asyncio
|
||||||
|
import csv
|
||||||
|
import os
|
||||||
|
import signal
|
||||||
|
from contextlib import ExitStack
|
||||||
|
from datetime import datetime
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from trikke_ble import BleFrameReassembler, encode_ack
|
||||||
|
from trikke_protocol import (
|
||||||
|
CSV_COLUMNS,
|
||||||
|
PACKET_TYPE_METADATA,
|
||||||
|
Frame,
|
||||||
|
IntegrityTracker,
|
||||||
|
Metadata,
|
||||||
|
StreamParser,
|
||||||
|
sample_to_csv_row,
|
||||||
|
)
|
||||||
|
|
||||||
|
DEVICE_NAME = "TrikkeSensor"
|
||||||
|
DATA_UUID = "7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c11"
|
||||||
|
ACK_UUID = "7d2ea000-f75b-4a9b-8fbe-3d4c2a1e9c12"
|
||||||
|
|
||||||
|
|
||||||
|
def parse_args() -> argparse.Namespace:
|
||||||
|
parser = argparse.ArgumentParser()
|
||||||
|
parser.add_argument("--address", help="BLE address/identifier; scan by name when omitted")
|
||||||
|
parser.add_argument("--name", default=DEVICE_NAME)
|
||||||
|
parser.add_argument("--output", type=Path, help="validated binary .trk output")
|
||||||
|
parser.add_argument("--csv", type=Path, help="decoded CSV output")
|
||||||
|
return parser.parse_args()
|
||||||
|
|
||||||
|
|
||||||
|
async def capture(args: argparse.Namespace) -> int:
|
||||||
|
try:
|
||||||
|
from bleak import BleakClient, BleakScanner
|
||||||
|
from bleak.exc import BleakError
|
||||||
|
except ImportError:
|
||||||
|
print("BLE capture requires bleak: python3 -m pip install -r requirements.txt")
|
||||||
|
return 2
|
||||||
|
|
||||||
|
stem = datetime.now().strftime("ble_%Y%m%d_%H%M%S")
|
||||||
|
output = args.output or Path("captures") / f"{stem}.trk"
|
||||||
|
csv_output = args.csv or output.with_suffix(".csv")
|
||||||
|
output.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
csv_output.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
|
||||||
|
device = args.address
|
||||||
|
|
||||||
|
stop = asyncio.Event()
|
||||||
|
loop = asyncio.get_running_loop()
|
||||||
|
for signum in (signal.SIGINT, signal.SIGTERM):
|
||||||
|
try:
|
||||||
|
loop.add_signal_handler(signum, stop.set)
|
||||||
|
except NotImplementedError:
|
||||||
|
pass
|
||||||
|
|
||||||
|
fragments: asyncio.Queue[bytes] = asyncio.Queue(maxsize=512)
|
||||||
|
callback_drop_count = 0
|
||||||
|
|
||||||
|
def on_fragment(_characteristic: object, data: bytearray) -> None:
|
||||||
|
payload = bytes(data)
|
||||||
|
|
||||||
|
def enqueue() -> None:
|
||||||
|
nonlocal callback_drop_count
|
||||||
|
try:
|
||||||
|
fragments.put_nowait(payload)
|
||||||
|
except asyncio.QueueFull:
|
||||||
|
callback_drop_count += 1
|
||||||
|
|
||||||
|
loop.call_soon_threadsafe(enqueue)
|
||||||
|
|
||||||
|
reassembler = BleFrameReassembler()
|
||||||
|
parser = StreamParser()
|
||||||
|
integrity = IntegrityTracker()
|
||||||
|
metadata: Metadata | None = None
|
||||||
|
pending_frames: list[Frame] = []
|
||||||
|
last_persisted_sequence: int | None = None
|
||||||
|
last_persisted_raw: bytes | None = None
|
||||||
|
sample_count = 0
|
||||||
|
frame_count = 0
|
||||||
|
|
||||||
|
with ExitStack() as stack:
|
||||||
|
raw_capture = stack.enter_context(output.open("wb"))
|
||||||
|
decoded = stack.enter_context(csv_output.open("w", encoding="utf-8", newline=""))
|
||||||
|
writer = csv.writer(decoded)
|
||||||
|
writer.writerow(CSV_COLUMNS)
|
||||||
|
print(f"Recording to {output} and {csv_output}; press Ctrl-C to stop")
|
||||||
|
while not stop.is_set():
|
||||||
|
try:
|
||||||
|
if device is None:
|
||||||
|
print(f"Scanning for {args.name}...")
|
||||||
|
device = await BleakScanner.find_device_by_name(
|
||||||
|
args.name, timeout=5.0
|
||||||
|
)
|
||||||
|
if device is None:
|
||||||
|
await asyncio.sleep(0.5)
|
||||||
|
continue
|
||||||
|
print(f"Connecting to {device}...")
|
||||||
|
async with BleakClient(device) as client:
|
||||||
|
reassembler.reset()
|
||||||
|
while not fragments.empty():
|
||||||
|
fragments.get_nowait()
|
||||||
|
await client.start_notify(DATA_UUID, on_fragment)
|
||||||
|
print("BLE connected and subscribed")
|
||||||
|
while not stop.is_set() and client.is_connected:
|
||||||
|
try:
|
||||||
|
fragment = await asyncio.wait_for(
|
||||||
|
fragments.get(), timeout=0.25
|
||||||
|
)
|
||||||
|
except TimeoutError:
|
||||||
|
continue
|
||||||
|
assembled = reassembler.feed(fragment)
|
||||||
|
if assembled is None:
|
||||||
|
continue
|
||||||
|
|
||||||
|
frames = parser.feed(assembled)
|
||||||
|
if len(frames) != 1 or frames[0].raw != assembled:
|
||||||
|
continue
|
||||||
|
frame = frames[0]
|
||||||
|
if (
|
||||||
|
frame.packet_sequence == last_persisted_sequence
|
||||||
|
and frame.raw == last_persisted_raw
|
||||||
|
):
|
||||||
|
await client.write_gatt_char(
|
||||||
|
ACK_UUID,
|
||||||
|
encode_ack(frame.packet_sequence),
|
||||||
|
response=True,
|
||||||
|
)
|
||||||
|
continue
|
||||||
|
|
||||||
|
raw_capture.write(frame.raw)
|
||||||
|
raw_capture.flush()
|
||||||
|
os.fsync(raw_capture.fileno())
|
||||||
|
integrity.observe(frame)
|
||||||
|
if frame.packet_type == PACKET_TYPE_METADATA:
|
||||||
|
metadata = frame.metadata
|
||||||
|
for pending in pending_frames:
|
||||||
|
for sample in pending.samples:
|
||||||
|
writer.writerow(sample_to_csv_row(
|
||||||
|
sample,
|
||||||
|
metadata,
|
||||||
|
pending.loop_overrun_count,
|
||||||
|
))
|
||||||
|
sample_count += 1
|
||||||
|
pending_frames.clear()
|
||||||
|
elif metadata is None:
|
||||||
|
pending_frames.append(frame)
|
||||||
|
else:
|
||||||
|
for sample in frame.samples:
|
||||||
|
writer.writerow(sample_to_csv_row(
|
||||||
|
sample, metadata, frame.loop_overrun_count
|
||||||
|
))
|
||||||
|
sample_count += 1
|
||||||
|
decoded.flush()
|
||||||
|
|
||||||
|
# The binary stream is authoritative and fsynced before
|
||||||
|
# ACK. A lost ACK is safe: replay is deduped above.
|
||||||
|
last_persisted_sequence = frame.packet_sequence
|
||||||
|
last_persisted_raw = frame.raw
|
||||||
|
await client.write_gatt_char(
|
||||||
|
ACK_UUID,
|
||||||
|
encode_ack(frame.packet_sequence),
|
||||||
|
response=True,
|
||||||
|
)
|
||||||
|
frame_count += 1
|
||||||
|
if client.is_connected:
|
||||||
|
await client.stop_notify(DATA_UUID)
|
||||||
|
except (BleakError, OSError) as error:
|
||||||
|
if not stop.is_set():
|
||||||
|
print(f"BLE interrupted ({error}); reconnecting")
|
||||||
|
if args.address is None:
|
||||||
|
device = None
|
||||||
|
if not stop.is_set():
|
||||||
|
await asyncio.sleep(0.5)
|
||||||
|
|
||||||
|
print(
|
||||||
|
f"Stopped after {frame_count} frames and {sample_count} samples; "
|
||||||
|
f"fragment_rejects={reassembler.rejected_fragment_count}, "
|
||||||
|
f"callback_drops={callback_drop_count}, "
|
||||||
|
f"packet_gaps={integrity.packet_gap_count}, "
|
||||||
|
f"sample_gaps={integrity.sample_gap_count}, "
|
||||||
|
f"crc_errors={parser.crc_errors}, "
|
||||||
|
f"trailing_partial_bytes={parser.buffered_bytes}"
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
f"Status totals: accel_stale={integrity.accel_stale_count}, "
|
||||||
|
f"accel_overrun={integrity.accel_overrun_count}, "
|
||||||
|
f"gyro_stale={integrity.gyro_stale_count}, "
|
||||||
|
f"gyro_overrun={integrity.gyro_overrun_count}, "
|
||||||
|
f"dropped={integrity.final_dropped_sample_count}, "
|
||||||
|
f"acquisition_loop_overruns={integrity.final_loop_overrun_count}"
|
||||||
|
)
|
||||||
|
if integrity.final_status is not None:
|
||||||
|
status = integrity.final_status
|
||||||
|
print(
|
||||||
|
"Cause totals: "
|
||||||
|
f"sensor_read_failures={status.sensor_read_failure_count}, "
|
||||||
|
f"queue_overflows={status.queue_overflow_count}, "
|
||||||
|
f"transport_begin_retries={status.transport_begin_retry_count}, "
|
||||||
|
f"transport_disconnects={status.transport_disconnect_count}, "
|
||||||
|
f"transport_send_failures={status.transport_send_failure_count}, "
|
||||||
|
f"transport_replays={status.transport_replay_count}, "
|
||||||
|
f"transport_invalid_acks={status.transport_invalid_ack_count}"
|
||||||
|
)
|
||||||
|
return 0 if metadata is not None else 4
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
return asyncio.run(capture(parse_args()))
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -76,6 +76,18 @@ def main() -> int:
|
|||||||
f"acquisition_loop_overruns={integrity.final_loop_overrun_count}, "
|
f"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
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,66 @@
|
|||||||
|
"""BLE fragment reassembly and acknowledgement helpers for TRK1 frames."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import struct
|
||||||
|
|
||||||
|
BLE_FRAGMENT_HEADER = struct.Struct("<IHH")
|
||||||
|
BLE_ACK = struct.Struct("<4sI")
|
||||||
|
BLE_MIN_FRAME_SIZE = 36
|
||||||
|
BLE_MAX_FRAME_SIZE = 196
|
||||||
|
|
||||||
|
|
||||||
|
class BleFrameReassembler:
|
||||||
|
def __init__(self) -> None:
|
||||||
|
self.rejected_fragment_count = 0
|
||||||
|
self._sequence: int | None = None
|
||||||
|
self._total_size = 0
|
||||||
|
self._frame = bytearray()
|
||||||
|
|
||||||
|
def reset(self) -> None:
|
||||||
|
self._sequence = None
|
||||||
|
self._total_size = 0
|
||||||
|
self._frame.clear()
|
||||||
|
|
||||||
|
def feed(self, fragment: bytes) -> bytes | None:
|
||||||
|
if len(fragment) <= BLE_FRAGMENT_HEADER.size:
|
||||||
|
self.rejected_fragment_count += 1
|
||||||
|
self.reset()
|
||||||
|
return None
|
||||||
|
sequence, offset, total_size = BLE_FRAGMENT_HEADER.unpack_from(fragment)
|
||||||
|
data = fragment[BLE_FRAGMENT_HEADER.size :]
|
||||||
|
if (
|
||||||
|
total_size < BLE_MIN_FRAME_SIZE
|
||||||
|
or total_size > BLE_MAX_FRAME_SIZE
|
||||||
|
or offset >= total_size
|
||||||
|
or offset + len(data) > total_size
|
||||||
|
):
|
||||||
|
self.rejected_fragment_count += 1
|
||||||
|
self.reset()
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Offset zero is an explicit replay boundary, including when the same
|
||||||
|
# packet restarts after an ACK timeout or reconnect.
|
||||||
|
if offset == 0:
|
||||||
|
self._sequence = sequence
|
||||||
|
self._total_size = total_size
|
||||||
|
self._frame = bytearray()
|
||||||
|
if (
|
||||||
|
self._sequence != sequence
|
||||||
|
or self._total_size != total_size
|
||||||
|
or offset != len(self._frame)
|
||||||
|
):
|
||||||
|
self.rejected_fragment_count += 1
|
||||||
|
self.reset()
|
||||||
|
return None
|
||||||
|
|
||||||
|
self._frame.extend(data)
|
||||||
|
if len(self._frame) != self._total_size:
|
||||||
|
return None
|
||||||
|
frame = bytes(self._frame)
|
||||||
|
self.reset()
|
||||||
|
return frame
|
||||||
|
|
||||||
|
|
||||||
|
def encode_ack(packet_sequence: int) -> bytes:
|
||||||
|
return BLE_ACK.pack(b"ACK1", packet_sequence & 0xFFFFFFFF)
|
||||||
@@ -12,15 +12,18 @@ VERSION = 1
|
|||||||
HEADER_SIZE = 36
|
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,
|
||||||
)
|
)
|
||||||
|
|||||||
Reference in New Issue
Block a user