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

calibrate enclosure motion outputs

Browse Source
This commit is contained in:
Jay
2026-08-17 10:44:28 -04:00
parent 07d47f99a4
commit a5c3087ee4
5 changed files with 167 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/trikke_sensor_main.c
+59 -3
View File
@@ -1,4 +1,5 @@
#include <inttypes.h>
#include <math.h>
#include <stdio.h>
#include "adxl345.h"
@@ -19,6 +20,20 @@
#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 {
@@ -48,6 +63,30 @@ 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)
{
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 = {
@@ -106,9 +145,16 @@ void app_main(void)
// 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("# 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");
@@ -116,6 +162,8 @@ void app_main(void)
"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");
@@ -139,8 +187,14 @@ void app_main(void)
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
@@ -148,6 +202,8 @@ void app_main(void)
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);