#include #include #include #include "adxl345.h" #include "driver/gpio.h" #include "driver/i2c_master.h" #include "esp_err.h" #include "esp_log.h" #include "esp_timer.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "l3g4200d.h" // Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7. #define TRIKKE_I2C_PORT I2C_NUM_0 #define TRIKKE_I2C_SDA_GPIO GPIO_NUM_6 #define TRIKKE_I2C_SCL_GPIO GPIO_NUM_7 #define TRIKKE_I2C_FREQ_HZ 400000 #define TRIKKE_SAMPLE_RATE_HZ 100 #define TRIKKE_SAMPLE_TICKS pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ) // Software calibration from the 2026-08-17 enclosure six-face capture. // Accelerometer coefficients are measured. Gyroscope scale is nominal; its // zero-rate biases and all three axis polarities were measured. #define TRIKKE_ACCEL_X_OFFSET_COUNTS (-1.417678f) #define TRIKKE_ACCEL_Y_OFFSET_COUNTS (-4.400892f) #define TRIKKE_ACCEL_Z_OFFSET_COUNTS (11.776132f) #define TRIKKE_ACCEL_X_COUNTS_PER_G (258.890661f) #define TRIKKE_ACCEL_Y_COUNTS_PER_G (259.825135f) #define TRIKKE_ACCEL_Z_COUNTS_PER_G (245.755573f) #define TRIKKE_GYRO_X_BIAS_COUNTS (9.0482f) #define TRIKKE_GYRO_Y_BIAS_COUNTS (-153.6198f) #define TRIKKE_GYRO_Z_BIAS_COUNTS (-7.0238f) #define TRIKKE_GYRO_MDPS_PER_LSB (17.5f) static const char *TAG = "trikke"; typedef struct { int32_t x; int32_t y; int32_t z; } trikke_axes_sample_t; 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. // Mounted ADXL345: native +Y right, native +X down, native +Z toward cover. return (trikke_axes_sample_t) { .x = native->y, .y = -(int32_t)native->x, .z = native->z, }; } static trikke_axes_sample_t map_gyro_to_enclosure(const l3g4200d_sample_t *native) { // The mounted L3G4200D axes already match the enclosure frame. return (trikke_axes_sample_t) { .x = native->x, .y = native->y, .z = native->z, }; } static trikke_axes_sample_t calibrate_accel_mg(const trikke_axes_sample_t *raw) { 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), }; } static trikke_axes_sample_t calibrate_gyro_mdps(const trikke_axes_sample_t *raw) { 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), }; } static esp_err_t init_i2c(i2c_master_bus_handle_t *bus) { const i2c_master_bus_config_t config = { .i2c_port = TRIKKE_I2C_PORT, .sda_io_num = TRIKKE_I2C_SDA_GPIO, .scl_io_num = TRIKKE_I2C_SCL_GPIO, .clk_source = I2C_CLK_SRC_DEFAULT, .glitch_ignore_cnt = 7, .flags.enable_internal_pullup = true, }; return i2c_new_master_bus(&config, bus); } void app_main(void) { // Emit each CSV record immediately while testing over USB. setvbuf(stdout, NULL, _IOLBF, 0); ESP_LOGI(TAG, "Trikke motion telemetry prototype v0"); ESP_LOGI(TAG, "I2C: SDA=GPIO%d, SCL=GPIO%d, clock=%d Hz", TRIKKE_I2C_SDA_GPIO, TRIKKE_I2C_SCL_GPIO, TRIKKE_I2C_FREQ_HZ); i2c_master_bus_handle_t bus = NULL; esp_err_t err = init_i2c(&bus); if (err != ESP_OK) { ESP_LOGE(TAG, "I2C initialization failed: %s", esp_err_to_name(err)); return; } adxl345_t accelerometer = {0}; err = adxl345_init(&accelerometer, bus, TRIKKE_I2C_FREQ_HZ); if (err != ESP_OK) { ESP_LOGE(TAG, "ADXL345 not found at 0x53 or 0x1D (expected DEVID 0xE5): %s", esp_err_to_name(err)); i2c_del_master_bus(bus); return; } ESP_LOGI(TAG, "ADXL345 detected at 0x%02X; 100 Hz, +/-8 g, full resolution", adxl345_address(&accelerometer)); l3g4200d_t gyroscope = {0}; err = l3g4200d_init(&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); i2c_del_master_bus(bus); return; } ESP_LOGI(TAG, "L3G4200D detected at 0x%02X; 100 Hz, 25 Hz BW, +/-500 dps", l3g4200d_address(&gyroscope)); // Discard the gyroscope's visible startup transient before beginning the stream. vTaskDelay(pdMS_TO_TICKS(500)); printf("# format=trikke_calibrated_v3\n"); printf("# accel_calibration=offset_counts:(%.6f,%.6f,%.6f)," "counts_per_g:(%.6f,%.6f,%.6f)\n", TRIKKE_ACCEL_X_OFFSET_COUNTS, TRIKKE_ACCEL_Y_OFFSET_COUNTS, TRIKKE_ACCEL_Z_OFFSET_COUNTS, TRIKKE_ACCEL_X_COUNTS_PER_G, TRIKKE_ACCEL_Y_COUNTS_PER_G, TRIKKE_ACCEL_Z_COUNTS_PER_G); printf("# gyro_calibration=bias_counts:(%.4f,%.4f,%.4f)," "nominal_mdps_per_lsb:%.1f,polarity:verified\n", TRIKKE_GYRO_X_BIAS_COUNTS, TRIKKE_GYRO_Y_BIAS_COUNTS, TRIKKE_GYRO_Z_BIAS_COUNTS, TRIKKE_GYRO_MDPS_PER_LSB); printf("# enclosure_axes=+x:right,+y:top,+z:toward_cover\n"); printf("# mapping=accel(x,y,z)=(native_y,-native_x,native_z);" "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"); uint32_t sequence = 0; uint32_t read_error_count = 0; uint32_t loop_overrun_count = 0; TickType_t last_wake = xTaskGetTickCount(); 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(); } } }