thejayman77/trikkeSensors
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add framed binary telemetry transport

Browse Source
This commit is contained in:
Jay
2026-08-17 11:13:53 -04:00
parent a5c3087ee4
commit aceaa2b270
13 changed files with 1320 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/CMakeLists.txt
+2 -1
View File
@@ -1,5 +1,6 @@
idf_component_register(
SRCS "trikke_sensor_main.c"
SRCS "trikke_sensor_main.c" "trikke_protocol.c"
INCLUDE_DIRS "."
REQUIRES adxl345 l3g4200d esp_timer esp_driver_gpio esp_driver_i2c
esp_driver_usb_serial_jtag vfs
)
main/trikke_protocol.c
+187
View File
@@ -0,0 +1,187 @@
#include "trikke_protocol.h"
#include <limits.h>
#include <string.h>
_Static_assert(sizeof(float) == 4, "TRK1 metadata requires 32-bit float");
static const uint8_t TRIKKE_MAGIC[4] = {'T', 'R', 'K', '1'};
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 void put_u64_le(uint8_t *output, uint64_t value)
{
put_u32_le(output, (uint32_t)value);
put_u32_le(output + 4, (uint32_t)(value >> 32));
}
static void put_i16_le(uint8_t *output, int16_t value)
{
put_u16_le(output, (uint16_t)value);
}
static void put_float_le(uint8_t *output, float value)
{
uint32_t bits = 0;
memcpy(&bits, &value, sizeof(bits));
put_u32_le(output, bits);
}
static uint32_t crc32_update(uint32_t crc, const uint8_t *data, size_t size)
{
for (size_t i = 0; i < size; ++i) {
crc ^= data[i];
for (unsigned bit = 0; bit < 8; ++bit) {
const uint32_t mask = -(crc & 1U);
crc = (crc >> 1) ^ (0xEDB88320U & mask);
}
}
return crc;
}
static uint32_t packet_crc32(const uint8_t *packet, size_t payload_size)
{
uint32_t crc = UINT32_MAX;
crc = crc32_update(crc, packet + 4, 28);
crc = crc32_update(crc, packet + TRIKKE_WIRE_HEADER_SIZE, payload_size);
return ~crc;
}
static void encode_header(uint8_t *output,
uint8_t packet_type,
uint8_t record_size,
uint8_t record_count,
uint8_t flags,
uint16_t payload_size,
uint32_t packet_sequence,
int64_t base_timestamp_us,
uint32_t dropped_sample_count,
uint32_t loop_overrun_count)
{
memcpy(output, TRIKKE_MAGIC, sizeof(TRIKKE_MAGIC));
output[4] = TRIKKE_WIRE_VERSION;
output[5] = packet_type;
output[6] = TRIKKE_WIRE_HEADER_SIZE;
output[7] = record_size;
output[8] = record_count;
output[9] = flags;
put_u16_le(output + 10, payload_size);
put_u32_le(output + 12, packet_sequence);
put_u64_le(output + 16, (uint64_t)base_timestamp_us);
put_u32_le(output + 24, dropped_sample_count);
put_u32_le(output + 28, loop_overrun_count);
put_u32_le(output + 32, 0);
}
size_t trikke_encode_metadata_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_metadata_t *metadata)
{
const size_t packet_size =
TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE;
if (output == NULL || metadata == NULL || output_size < packet_size) {
return 0;
}
encode_header(output, TRIKKE_PACKET_TYPE_METADATA, 0, 0, 0,
TRIKKE_WIRE_METADATA_SIZE, packet_sequence, timestamp_us,
dropped_sample_count, loop_overrun_count);
uint8_t *payload = output + TRIKKE_WIRE_HEADER_SIZE;
put_u16_le(payload, metadata->sample_rate_hz);
put_u16_le(payload + 2, metadata->accel_range_g);
put_u16_le(payload + 4, metadata->gyro_range_dps);
put_u16_le(payload + 6, metadata->flags);
size_t offset = 8;
for (size_t axis = 0; axis < 3; ++axis, offset += sizeof(float)) {
put_float_le(payload + offset, metadata->accel_offset_counts[axis]);
}
for (size_t axis = 0; axis < 3; ++axis, offset += sizeof(float)) {
put_float_le(payload + offset, metadata->accel_counts_per_g[axis]);
}
for (size_t axis = 0; axis < 3; ++axis, offset += sizeof(float)) {
put_float_le(payload + offset, metadata->gyro_bias_counts[axis]);
}
put_float_le(payload + offset, metadata->gyro_mdps_per_lsb);
put_u32_le(output + 32, packet_crc32(output, TRIKKE_WIRE_METADATA_SIZE));
return packet_size;
}
size_t trikke_encode_sample_packet(
uint8_t *output,
size_t output_size,
uint32_t packet_sequence,
uint32_t dropped_sample_count,
uint32_t loop_overrun_count,
const trikke_wire_sample_t *samples,
size_t sample_count)
{
if (output == NULL || samples == NULL || sample_count == 0 ||
sample_count > TRIKKE_WIRE_MAX_RECORDS) {
return 0;
}
const size_t payload_size = sample_count * TRIKKE_WIRE_SAMPLE_RECORD_SIZE;
const size_t packet_size = TRIKKE_WIRE_HEADER_SIZE + payload_size;
if (output_size < packet_size) {
return 0;
}
uint8_t packet_flags = 0;
encode_header(output, TRIKKE_PACKET_TYPE_SAMPLES,
TRIKKE_WIRE_SAMPLE_RECORD_SIZE, (uint8_t)sample_count, 0,
(uint16_t)payload_size, packet_sequence,
samples[0].timestamp_us, dropped_sample_count,
loop_overrun_count);
int64_t previous_timestamp_us = samples[0].timestamp_us;
for (size_t i = 0; i < sample_count; ++i) {
uint8_t *record = output + TRIKKE_WIRE_HEADER_SIZE +
i * TRIKKE_WIRE_SAMPLE_RECORD_SIZE;
uint16_t timestamp_delta_10us = 0;
if (i > 0) {
const int64_t delta_us = samples[i].timestamp_us - previous_timestamp_us;
if (delta_us < 0 || delta_us > (int64_t)UINT16_MAX * 10) {
timestamp_delta_10us = UINT16_MAX;
packet_flags |= TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED;
} else {
timestamp_delta_10us = (uint16_t)((delta_us + 5) / 10);
}
}
put_u32_le(record, samples[i].sequence);
put_u16_le(record + 4, timestamp_delta_10us);
put_i16_le(record + 6, samples[i].accel_x);
put_i16_le(record + 8, samples[i].accel_y);
put_i16_le(record + 10, samples[i].accel_z);
put_i16_le(record + 12, samples[i].gyro_x);
put_i16_le(record + 14, samples[i].gyro_y);
put_i16_le(record + 16, samples[i].gyro_z);
record[18] = samples[i].accel_status;
record[19] = samples[i].gyro_status;
previous_timestamp_us = samples[i].timestamp_us;
}
output[9] = packet_flags;
put_u32_le(output + 32, packet_crc32(output, payload_size));
return packet_size;
}
main/trikke_protocol.h
+65
View File
@@ -0,0 +1,65 @@
#pragma once
#include <stddef.h>
#include <stdint.h>
#define TRIKKE_WIRE_VERSION 1
#define TRIKKE_WIRE_HEADER_SIZE 36
#define TRIKKE_WIRE_SAMPLE_RECORD_SIZE 20
#define TRIKKE_WIRE_METADATA_SIZE 48
#define TRIKKE_WIRE_MAX_RECORDS 8
#define TRIKKE_WIRE_MAX_PACKET_SIZE \
(TRIKKE_WIRE_HEADER_SIZE + \
TRIKKE_WIRE_SAMPLE_RECORD_SIZE * TRIKKE_WIRE_MAX_RECORDS)
#define TRIKKE_PACKET_TYPE_METADATA 1
#define TRIKKE_PACKET_TYPE_SAMPLES 2
#define TRIKKE_PACKET_FLAG_TIMESTAMP_DELTA_SATURATED 0x01
#define TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z 0x0001
#define TRIKKE_METADATA_FLAG_GYRO_IDENTITY 0x0002
#define TRIKKE_METADATA_FLAG_GYRO_POLARITY 0x0004
#define TRIKKE_METADATA_FLAG_GYRO_SCALE_NOMINAL 0x0008
typedef struct {
uint32_t sequence;
int64_t timestamp_us;
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
uint8_t accel_status;
uint8_t gyro_status;
} trikke_wire_sample_t;
typedef struct {
uint16_t sample_rate_hz;
uint16_t accel_range_g;
uint16_t gyro_range_dps;
uint16_t flags;
float accel_offset_counts[3];
float accel_counts_per_g[3];
float gyro_bias_counts[3];
float gyro_mdps_per_lsb;
} trikke_wire_metadata_t;
size_t trikke_encode_metadata_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_metadata_t *metadata);
size_t trikke_encode_sample_packet(
uint8_t *output,
size_t output_size,
uint32_t packet_sequence,
uint32_t dropped_sample_count,
uint32_t loop_overrun_count,
const trikke_wire_sample_t *samples,
size_t sample_count);
main/trikke_sensor_main.c
+196 -87
View File
@@ -1,16 +1,19 @@
#include <inttypes.h>
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdatomic.h>
#include "adxl345.h"
#include "driver/gpio.h"
#include "driver/i2c_master.h"
#include "driver/usb_serial_jtag_vfs.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "l3g4200d.h"
#include "trikke_protocol.h"
// Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7.
#define TRIKKE_I2C_PORT I2C_NUM_0
@@ -19,6 +22,8 @@
#define TRIKKE_I2C_FREQ_HZ 400000
#define TRIKKE_SAMPLE_RATE_HZ 100
#define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ)
#define TRIKKE_SAMPLE_QUEUE_DEPTH 128
#define TRIKKE_METADATA_INTERVAL_PACKETS 64
// Software calibration from the 2026-08-17 enclosure six-face capture.
// Accelerometer coefficients are measured. Gyroscope scale is nominal; its
@@ -42,6 +47,42 @@ typedef struct {
int32_t z;
} trikke_axes_sample_t;
typedef struct {
adxl345_t accelerometer;
l3g4200d_t gyroscope;
QueueHandle_t sample_queue;
atomic_uint_least32_t dropped_sample_count;
atomic_uint_least32_t loop_overrun_count;
} trikke_context_t;
static trikke_context_t s_context;
static const trikke_wire_metadata_t TRIKKE_METADATA = {
.sample_rate_hz = TRIKKE_SAMPLE_RATE_HZ,
.accel_range_g = 8,
.gyro_range_dps = 500,
.flags = TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z |
TRIKKE_METADATA_FLAG_GYRO_IDENTITY |
TRIKKE_METADATA_FLAG_GYRO_POLARITY |
TRIKKE_METADATA_FLAG_GYRO_SCALE_NOMINAL,
.accel_offset_counts = {
TRIKKE_ACCEL_X_OFFSET_COUNTS,
TRIKKE_ACCEL_Y_OFFSET_COUNTS,
TRIKKE_ACCEL_Z_OFFSET_COUNTS,
},
.accel_counts_per_g = {
TRIKKE_ACCEL_X_COUNTS_PER_G,
TRIKKE_ACCEL_Y_COUNTS_PER_G,
TRIKKE_ACCEL_Z_COUNTS_PER_G,
},
.gyro_bias_counts = {
TRIKKE_GYRO_X_BIAS_COUNTS,
TRIKKE_GYRO_Y_BIAS_COUNTS,
TRIKKE_GYRO_Z_BIAS_COUNTS,
},
.gyro_mdps_per_lsb = TRIKKE_GYRO_MDPS_PER_LSB,
};
static trikke_axes_sample_t map_accel_to_enclosure(const adxl345_sample_t *native)
{
// Enclosure frame: +X right, +Y toward the top, +Z toward the cover.
@@ -63,28 +104,116 @@ static trikke_axes_sample_t map_gyro_to_enclosure(const l3g4200d_sample_t *nativ
};
}
static trikke_axes_sample_t calibrate_accel_mg(const trikke_axes_sample_t *raw)
static void acquisition_task(void *argument)
{
return (trikke_axes_sample_t) {
.x = lroundf(((float)raw->x - TRIKKE_ACCEL_X_OFFSET_COUNTS) *
1000.0f / TRIKKE_ACCEL_X_COUNTS_PER_G),
.y = lroundf(((float)raw->y - TRIKKE_ACCEL_Y_OFFSET_COUNTS) *
1000.0f / TRIKKE_ACCEL_Y_COUNTS_PER_G),
.z = lroundf(((float)raw->z - TRIKKE_ACCEL_Z_OFFSET_COUNTS) *
1000.0f / TRIKKE_ACCEL_Z_COUNTS_PER_G),
};
trikke_context_t *context = argument;
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
uint32_t sequence = 0;
TickType_t last_wake = xTaskGetTickCount();
while (true) {
adxl345_sample_t accel = {0};
l3g4200d_sample_t gyro = {0};
uint8_t accel_status = 0;
uint8_t gyro_status = 0;
const int64_t timestamp_us = esp_timer_get_time();
const esp_err_t accel_err =
adxl345_read_raw(&context->accelerometer, &accel, &accel_status);
const esp_err_t gyro_err =
l3g4200d_read_raw(&context->gyroscope, &gyro, &gyro_status);
if (accel_err == ESP_OK && gyro_err == ESP_OK) {
const trikke_axes_sample_t enclosure_accel =
map_accel_to_enclosure(&accel);
const trikke_axes_sample_t enclosure_gyro =
map_gyro_to_enclosure(&gyro);
const trikke_wire_sample_t sample = {
.sequence = sequence,
.timestamp_us = timestamp_us,
.accel_x = (int16_t)enclosure_accel.x,
.accel_y = (int16_t)enclosure_accel.y,
.accel_z = (int16_t)enclosure_accel.z,
.gyro_x = (int16_t)enclosure_gyro.x,
.gyro_y = (int16_t)enclosure_gyro.y,
.gyro_z = (int16_t)enclosure_gyro.z,
.accel_status = accel_status,
.gyro_status = gyro_status,
};
if (xQueueSend(context->sample_queue, &sample, 0) != pdPASS) {
atomic_fetch_add(&context->dropped_sample_count, 1);
}
} else {
atomic_fetch_add(&context->dropped_sample_count, 1);
}
++sequence;
if (xTaskDelayUntil(&last_wake, TRIKKE_SAMPLE_TICKS) == pdFALSE) {
atomic_fetch_add(&context->loop_overrun_count, 1);
last_wake = xTaskGetTickCount();
vTaskDelay(1);
}
}
}
static trikke_axes_sample_t calibrate_gyro_mdps(const trikke_axes_sample_t *raw)
static bool write_binary_packet(const uint8_t *packet, size_t packet_size)
{
return (trikke_axes_sample_t) {
.x = lroundf(((float)raw->x - TRIKKE_GYRO_X_BIAS_COUNTS) *
TRIKKE_GYRO_MDPS_PER_LSB),
.y = lroundf(((float)raw->y - TRIKKE_GYRO_Y_BIAS_COUNTS) *
TRIKKE_GYRO_MDPS_PER_LSB),
.z = lroundf(((float)raw->z - TRIKKE_GYRO_Z_BIAS_COUNTS) *
TRIKKE_GYRO_MDPS_PER_LSB),
};
const bool complete = fwrite(packet, 1, packet_size, stdout) == packet_size;
fflush(stdout);
if (!complete) {
clearerr(stdout);
}
return complete;
}
static void output_task(void *argument)
{
trikke_context_t *context = argument;
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
uint8_t packet[TRIKKE_WIRE_MAX_PACKET_SIZE] = {0};
uint32_t packet_sequence = 0;
uint32_t sample_packet_count = 0;
size_t packet_size = trikke_encode_metadata_packet(
packet, sizeof(packet), packet_sequence++, esp_timer_get_time(),
atomic_load(&context->dropped_sample_count),
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
write_binary_packet(packet, packet_size);
while (true) {
trikke_wire_sample_t samples[TRIKKE_WIRE_MAX_RECORDS] = {0};
size_t sample_count = 0;
if (xQueueReceive(context->sample_queue, &samples[sample_count],
portMAX_DELAY) != pdPASS) {
continue;
}
++sample_count;
while (sample_count < TRIKKE_WIRE_MAX_RECORDS &&
xQueueReceive(context->sample_queue, &samples[sample_count],
pdMS_TO_TICKS(15)) == pdPASS) {
++sample_count;
}
if (sample_packet_count > 0 &&
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),
atomic_load(&context->loop_overrun_count), &TRIKKE_METADATA);
write_binary_packet(packet, packet_size);
}
packet_size = trikke_encode_sample_packet(
packet, sizeof(packet), packet_sequence++,
atomic_load(&context->dropped_sample_count),
atomic_load(&context->loop_overrun_count), samples, sample_count);
if (!write_binary_packet(packet, packet_size)) {
atomic_fetch_add(&context->dropped_sample_count, sample_count);
}
++sample_packet_count;
}
}
static esp_err_t init_i2c(i2c_master_bus_handle_t *bus)
@@ -103,7 +232,7 @@ static esp_err_t init_i2c(i2c_master_bus_handle_t *bus)
void app_main(void)
{
// Emit each CSV record immediately while testing over USB.
// Flush startup text by line; binary frames are flushed explicitly.
setvbuf(stdout, NULL, _IOLBF, 0);
ESP_LOGI(TAG, "Trikke motion telemetry prototype v0");
@@ -117,8 +246,7 @@ void app_main(void)
return;
}
adxl345_t accelerometer = {0};
err = adxl345_init(&accelerometer, bus, TRIKKE_I2C_FREQ_HZ);
err = adxl345_init(&s_context.accelerometer, bus, TRIKKE_I2C_FREQ_HZ);
if (err != ESP_OK) {
ESP_LOGE(TAG,
"ADXL345 not found at 0x53 or 0x1D (expected DEVID 0xE5): %s",
@@ -127,20 +255,19 @@ void app_main(void)
return;
}
ESP_LOGI(TAG, "ADXL345 detected at 0x%02X; 100 Hz, +/-8 g, full resolution",
adxl345_address(&accelerometer));
adxl345_address(&s_context.accelerometer));
l3g4200d_t gyroscope = {0};
err = l3g4200d_init(&gyroscope, bus, TRIKKE_I2C_FREQ_HZ);
err = l3g4200d_init(&s_context.gyroscope, bus, TRIKKE_I2C_FREQ_HZ);
if (err != ESP_OK) {
ESP_LOGE(TAG,
"L3G4200D not found at 0x69 or 0x68 (expected WHO_AM_I 0xD3): %s",
esp_err_to_name(err));
adxl345_deinit(&accelerometer);
adxl345_deinit(&s_context.accelerometer);
i2c_del_master_bus(bus);
return;
}
ESP_LOGI(TAG, "L3G4200D detected at 0x%02X; 100 Hz, 25 Hz BW, +/-500 dps",
l3g4200d_address(&gyroscope));
l3g4200d_address(&s_context.gyroscope));
// Discard the gyroscope's visible startup transient before beginning the stream.
vTaskDelay(pdMS_TO_TICKS(500));
@@ -160,66 +287,48 @@ void app_main(void)
"gyro(x,y,z)=(native_x,native_y,native_z)\n");
printf("# status_masks=accel_data_ready:0x80,accel_overrun:0x01,"
"gyro_data_ready:0x08,gyro_overrun:0x80\n");
printf("sequence,poll_timestamp_us,accel_x_raw,accel_y_raw,accel_z_raw,"
"gyro_x_raw,gyro_y_raw,gyro_z_raw,"
"accel_x_mg,accel_y_mg,accel_z_mg,"
"gyro_x_mdps,gyro_y_mdps,gyro_z_mdps,"
"accel_native_x_raw,accel_native_y_raw,accel_native_z_raw,"
"gyro_native_x_raw,gyro_native_y_raw,gyro_native_z_raw,"
"accel_int_source,gyro_status,loop_overrun_count\n");
printf("# binary_stream=TRK1,wire_version=%d,record_size=%d,"
"records_per_packet=%d\n",
TRIKKE_WIRE_VERSION, TRIKKE_WIRE_SAMPLE_RECORD_SIZE,
TRIKKE_WIRE_MAX_RECORDS);
fflush(stdout);
uint32_t sequence = 0;
uint32_t read_error_count = 0;
uint32_t loop_overrun_count = 0;
TickType_t last_wake = xTaskGetTickCount();
// 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);
while (true) {
adxl345_sample_t accel = {0};
l3g4200d_sample_t gyro = {0};
uint8_t accel_int_source = 0;
uint8_t gyro_status = 0;
const int64_t timestamp_us = esp_timer_get_time();
const esp_err_t accel_err = adxl345_read_raw(&accelerometer, &accel,
&accel_int_source);
const esp_err_t gyro_err = l3g4200d_read_raw(&gyroscope, &gyro, &gyro_status);
if (accel_err == ESP_OK && gyro_err == ESP_OK) {
const trikke_axes_sample_t enclosure_accel = map_accel_to_enclosure(&accel);
const trikke_axes_sample_t enclosure_gyro = map_gyro_to_enclosure(&gyro);
const trikke_axes_sample_t calibrated_accel =
calibrate_accel_mg(&enclosure_accel);
const trikke_axes_sample_t calibrated_gyro =
calibrate_gyro_mdps(&enclosure_gyro);
printf("%" PRIu32 ",%" PRId64 ",%" PRId32 ",%" PRId32 ",%" PRId32
",%" PRId32 ",%" PRId32 ",%" PRId32
",%" PRId32 ",%" PRId32 ",%" PRId32
",%" PRId32 ",%" PRId32 ",%" PRId32
",%" PRId16 ",%" PRId16 ",%" PRId16
",%" PRId16 ",%" PRId16 ",%" PRId16
",%" PRIu8 ",%" PRIu8 ",%" PRIu32 "\n",
sequence, timestamp_us,
enclosure_accel.x, enclosure_accel.y, enclosure_accel.z,
enclosure_gyro.x, enclosure_gyro.y, enclosure_gyro.z,
calibrated_accel.x, calibrated_accel.y, calibrated_accel.z,
calibrated_gyro.x, calibrated_gyro.y, calibrated_gyro.z,
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z,
accel_int_source, gyro_status, loop_overrun_count);
} else {
++read_error_count;
ESP_LOGE(TAG,
"sample %" PRIu32 " read failed (accel=%s, gyro=%s, total_errors=%" PRIu32 ")",
sequence, esp_err_to_name(accel_err), esp_err_to_name(gyro_err),
read_error_count);
}
++sequence;
if (xTaskDelayUntil(&last_wake, TRIKKE_SAMPLE_TICKS) == pdFALSE) {
// Do not issue a burst of back-to-back samples after a stalled output path.
++loop_overrun_count;
last_wake = xTaskGetTickCount();
}
s_context.sample_queue =
xQueueCreate(TRIKKE_SAMPLE_QUEUE_DEPTH, sizeof(trikke_wire_sample_t));
if (s_context.sample_queue == NULL) {
ESP_LOGE(TAG, "sample queue allocation failed");
l3g4200d_deinit(&s_context.gyroscope);
adxl345_deinit(&s_context.accelerometer);
i2c_del_master_bus(bus);
return;
}
TaskHandle_t output_task_handle = NULL;
TaskHandle_t acquisition_task_handle = NULL;
if (xTaskCreate(output_task, "trikke_output", 4096, &s_context, 5,
&output_task_handle) != pdPASS ||
xTaskCreate(acquisition_task, "trikke_acquire", 4096, &s_context, 10,
&acquisition_task_handle) != pdPASS) {
if (output_task_handle != NULL) {
vTaskDelete(output_task_handle);
}
if (acquisition_task_handle != NULL) {
vTaskDelete(acquisition_task_handle);
}
vQueueDelete(s_context.sample_queue);
ESP_LOGE(TAG, "telemetry task creation failed");
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);
xTaskNotifyGive(acquisition_task_handle);
}