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/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);
}