170 lines
6.1 KiB
C
170 lines
6.1 KiB
C
#include <inttypes.h>
|
|
#include <stdio.h>
|
|
|
|
#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)
|
|
|
|
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 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_freshness_validation_v2\n");
|
|
printf("# nominal_accel_scale_g_per_lsb=0.0039,"
|
|
"nominal_gyro_scale_dps_per_lsb=0.0175\n");
|
|
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_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);
|
|
|
|
printf("%" PRIu32 ",%" PRId64 ",%" 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,
|
|
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();
|
|
}
|
|
}
|
|
}
|