add reliable BLE telemetry transport
This commit is contained in:
+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(
|
||||
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}
|
||||
)
|
||||
|
||||
@@ -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));
|
||||
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_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);
|
||||
|
||||
+93
-19
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user