From 632e7b73fddb7a24471f835ca25750aa415c3476 Mon Sep 17 00:00:00 2001 From: sfja Date: Wed, 1 Apr 2026 00:19:07 +0200 Subject: [PATCH] fix driver 2 --- skateboard/main/CMakeLists.txt | 2 +- skateboard/main/new_mpu6050.c | 132 +++++++++++++++++++++++++++++---- skateboard/main/new_mpu6050.h | 5 +- skateboard/main/skateboard.c | 101 +++++++++++++------------ 4 files changed, 175 insertions(+), 65 deletions(-) diff --git a/skateboard/main/CMakeLists.txt b/skateboard/main/CMakeLists.txt index da7bf65..98dc0de 100644 --- a/skateboard/main/CMakeLists.txt +++ b/skateboard/main/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS skateboard.c app_wifi.c app_mpu.c app_mqtt.c new_mpu6050.c - PRIV_REQUIRES nvs_flash esp_netif esp_wifi mqtt esp_driver_i2c + PRIV_REQUIRES nvs_flash esp_netif esp_wifi mqtt esp_driver_i2c esp_timer INCLUDE_DIRS ".") diff --git a/skateboard/main/new_mpu6050.c b/skateboard/main/new_mpu6050.c index a4da3d7..f125261 100644 --- a/skateboard/main/new_mpu6050.c +++ b/skateboard/main/new_mpu6050.c @@ -3,7 +3,9 @@ #include "driver/i2c_types.h" #include "esp_err.h" #include "esp_log.h" +#include "esp_timer.h" #include "freertos/idf_additions.h" +#include "freertos/projdefs.h" #include #include #include @@ -108,6 +110,8 @@ static esp_err_t write_bits( esp_err_t new_mpu6050_init(Mpu6050* dev) { + *dev = (Mpu6050) { 0 }; + i2c_master_bus_config_t bus_config = { .clk_source = I2C_CLK_SRC_DEFAULT, .i2c_port = I2C_NUM_0, @@ -143,11 +147,13 @@ esp_err_t new_mpu6050_init(Mpu6050* dev) dev->gyro_range = MPU6050_GYRO_RANGE_250; dev->accel_range = MPU6050_ACCEL_RANGE_2; + vTaskDelay(pdMS_TO_TICKS(250)); + CHECK(new_mpu6050_set_clock_source(dev, MPU6050_CLKSEL_PLL_GYRO_X_REF)); CHECK(new_mpu6050_set_sleep_enabled(dev, false)); - CHECK(new_mpu6050_set_sample_rate_div(dev, 7)); + CHECK(new_mpu6050_set_sample_rate_div(dev, 3)); return ESP_OK; } @@ -155,21 +161,24 @@ esp_err_t new_mpu6050_init(Mpu6050* dev) esp_err_t new_mpu6050_get_rotation(Mpu6050* dev, float3* rotation) { static const float resolution[] = { - [MPU6050_GYRO_RANGE_250] = 250.0f / 32768.0f, - [MPU6050_GYRO_RANGE_500] = 500.0f / 32768.0f, - [MPU6050_GYRO_RANGE_1000] = 1000.0f / 32768.0f, - [MPU6050_GYRO_RANGE_2000] = 2000.0f / 32768.0f, + [MPU6050_GYRO_RANGE_250] = 1.0f / 131.0f, + [MPU6050_GYRO_RANGE_500] = 1.0f / 65.5f, + [MPU6050_GYRO_RANGE_1000] = 1.0f / 32.8f, + [MPU6050_GYRO_RANGE_2000] = 1.0f / 16.4f, }; uint8_t buffer[6]; CHECK(read_regs(dev, buffer, 6, REG_GYRO_XOUT_H)); rotation->x - = (int16_t)(buffer[0] << 8 | buffer[1]) * resolution[dev->gyro_range]; + = (int16_t)(buffer[0] << 8 | buffer[1]) * resolution[dev->gyro_range] + - dev->rotation_bias.x; rotation->y - = (int16_t)(buffer[2] << 8 | buffer[3]) * resolution[dev->gyro_range]; + = (int16_t)(buffer[2] << 8 | buffer[3]) * resolution[dev->gyro_range] + - dev->rotation_bias.y; rotation->z - = (int16_t)(buffer[4] << 8 | buffer[5]) * resolution[dev->gyro_range]; + = (int16_t)(buffer[4] << 8 | buffer[5]) * resolution[dev->gyro_range] + - dev->rotation_bias.z; return ESP_OK; } @@ -177,21 +186,24 @@ esp_err_t new_mpu6050_get_rotation(Mpu6050* dev, float3* rotation) esp_err_t new_mpu6050_get_acceleration(Mpu6050* dev, float3* accel) { static const float resolution[] = { - [MPU6050_ACCEL_RANGE_2] = 2.0f / 32768.0f, - [MPU6050_ACCEL_RANGE_4] = 4.0f / 32768.0f, - [MPU6050_ACCEL_RANGE_8] = 8.0f / 32768.0f, - [MPU6050_ACCEL_RANGE_16] = 16.0f / 32768.0f, + [MPU6050_ACCEL_RANGE_2] = 1.0f / 16384.0f, + [MPU6050_ACCEL_RANGE_4] = 1.0f / 1892.0f, + [MPU6050_ACCEL_RANGE_8] = 1.0f / 4096.0f, + [MPU6050_ACCEL_RANGE_16] = 1.0f / 2048.0f, }; uint8_t buffer[6]; CHECK(read_regs(dev, buffer, 6, REG_ACCEL_XOUT_H)); accel->x - = (int16_t)(buffer[0] << 8 | buffer[1]) * resolution[dev->accel_range]; + = (int16_t)(buffer[0] << 8 | buffer[1]) * resolution[dev->accel_range] + - dev->accel_bias.x; accel->y - = (int16_t)(buffer[2] << 8 | buffer[3]) * resolution[dev->accel_range]; + = (int16_t)(buffer[2] << 8 | buffer[3]) * resolution[dev->accel_range] + - dev->accel_bias.y; accel->z - = (int16_t)(buffer[4] << 8 | buffer[5]) * resolution[dev->accel_range]; + = (int16_t)(buffer[4] << 8 | buffer[5]) * resolution[dev->accel_range] + - dev->accel_bias.z; return ESP_OK; } @@ -260,3 +272,93 @@ esp_err_t new_mpu6050_set_dlpf(Mpu6050* dev, Mpu6050_DLPF selector) return write_bits( dev, REG_CONFIG, BIT_CONFIG_DLPF_CFG, MASK_CONFIG_DLPF_CFG, selector); } + +typedef struct { + Mpu6050* dev; + int count; + esp_err_t status; + float3 accel_acc; + float3 rotation_acc; + EventGroupHandle_t event_group; +} Calibrator; + +#define CALIBRATION_COUNT_TOTAL 25 + +static void calibrate_measure_cb(void* arg) +{ + Calibrator* calib = arg; + + if (calib->status != ESP_OK || calib->count >= CALIBRATION_COUNT_TOTAL) + goto loop_break; + + float3 accel; + calib->status = new_mpu6050_get_acceleration(calib->dev, &accel); + + if (calib->status != ESP_OK) + goto loop_break; + + float3 rotation; + calib->status = new_mpu6050_get_rotation(calib->dev, &rotation); + + if (calib->status != ESP_OK) + goto loop_break; + + calib->accel_acc.x += accel.x; + calib->accel_acc.y += accel.y; + calib->accel_acc.z += accel.z; + + calib->rotation_acc.x += rotation.x; + calib->rotation_acc.y += rotation.y; + calib->rotation_acc.z += rotation.z; + + calib->count += 1; + return; + +loop_break: + xEventGroupSetBits(calib->event_group, 1); +} + +esp_err_t new_mpu6050_calibrate(Mpu6050* dev, const float3* initial_rotation) +{ + + Calibrator calib = { + .dev = dev, + .event_group = xEventGroupCreate(), + }; + + esp_timer_handle_t timer; + esp_timer_create_args_t timer_config = { + .callback = calibrate_measure_cb, + .arg = &calib, + }; + CHECK(esp_timer_create(&timer_config, &timer)); + + CHECK(esp_timer_start_periodic(timer, 40000)); + + xEventGroupWaitBits(calib.event_group, + 1, + pdFALSE, + pdFALSE, + pdMS_TO_TICKS(CALIBRATION_COUNT_TOTAL * 40 * 2)); + + CHECK(calib.status); + + CHECK(esp_timer_stop(timer)); + CHECK(esp_timer_delete(timer)); + + dev->accel_bias.x = calib.accel_acc.x / (float)CALIBRATION_COUNT_TOTAL + - initial_rotation->x; + dev->accel_bias.y = calib.accel_acc.y / (float)CALIBRATION_COUNT_TOTAL + - initial_rotation->y; + dev->accel_bias.z = calib.accel_acc.z / (float)CALIBRATION_COUNT_TOTAL + - initial_rotation->z; + + dev->rotation_bias.x + = calib.rotation_acc.x / (float)CALIBRATION_COUNT_TOTAL; + dev->rotation_bias.y + = calib.rotation_acc.y / (float)CALIBRATION_COUNT_TOTAL; + dev->rotation_bias.z + = calib.rotation_acc.z / (float)CALIBRATION_COUNT_TOTAL; + + return ESP_OK; +} diff --git a/skateboard/main/new_mpu6050.h b/skateboard/main/new_mpu6050.h index 8cb175f..a542776 100644 --- a/skateboard/main/new_mpu6050.h +++ b/skateboard/main/new_mpu6050.h @@ -56,7 +56,8 @@ typedef struct { i2c_master_dev_handle_t i2c_dev; Mpu6050_GyroRange gyro_range; Mpu6050_AccelRange accel_range; - + float3 rotation_bias; + float3 accel_bias; } Mpu6050; esp_err_t new_mpu6050_init(Mpu6050* dev); @@ -117,3 +118,5 @@ typedef enum : uint8_t { // Digital low pass filter (DLPF) // Note: Sampling rate is dependent on whether DLPF is enabled. esp_err_t new_mpu6050_set_dlpf(Mpu6050* dev, Mpu6050_DLPF selector); + +esp_err_t new_mpu6050_calibrate(Mpu6050* dev, const float3* initial_rotation); diff --git a/skateboard/main/skateboard.c b/skateboard/main/skateboard.c index 4e712fa..236a857 100644 --- a/skateboard/main/skateboard.c +++ b/skateboard/main/skateboard.c @@ -3,29 +3,25 @@ #include "esp_event.h" #include "esp_log.h" #include "esp_netif.h" +#include "esp_timer.h" #include "freertos/idf_additions.h" #include "nvs_flash.h" #include #include -#define NEW_DRIVER - -#ifndef NEW_DRIVER -#include "app_mpu.h" -#else #include "new_mpu6050.h" -#endif const char* TAG = "skateboard"; typedef struct App { AppWifi wifi; AppMqtt mqtt; -#ifndef NEW_DRIVER - AppMpu mpu; -#else Mpu6050 mpu; -#endif + + float3 rotation_acc; + float3 accel_acc; + float time_acc; + float last_temp; } App; #define msg_buffer_capacity 1024 @@ -46,39 +42,19 @@ static void configure_cb(const char* topic, static void publish_message(App* app) { - float3 accel = { 0 }; -#ifndef NEW_DRIVER - app_mpu_read_acceleration(&app->mpu, &accel); -#else - ESP_ERROR_CHECK(new_mpu6050_get_acceleration(&app->mpu, &accel)); -#endif - - float3 rotation = { 0 }; -#ifndef NEW_DRIVER - app_mpu_read_rotation(&app->mpu, &rotation); -#else - ESP_ERROR_CHECK(new_mpu6050_get_rotation(&app->mpu, &rotation)); -#endif - - float temp = 0; -#ifndef NEW_DRIVER - app_mpu_read_temperature(&app->mpu, &temp); -#else - ESP_ERROR_CHECK(new_mpu6050_read_temperature(&app->mpu, &temp)); -#endif int msg_size = snprintf(msg_buffer, msg_buffer_capacity - 1, "{ \"acceleration\": [% 9.4f, % 9.4f, % 9.4f], " "\"rotation\": [% 9.4f, % 9.4f, % 9.4f], " "\"temperature\": % 5.2f }", - accel.x, - accel.y, - accel.z, - rotation.x, - rotation.y, - rotation.z, - temp); + app->accel_acc.x, + app->accel_acc.y, + app->accel_acc.z, + app->rotation_acc.x, + app->rotation_acc.y, + app->rotation_acc.z, + app->last_temp); #if false app_mqtt_publish(&app->mqtt, "/skateboard/update", msg_buffer, msg_size); @@ -87,6 +63,31 @@ static void publish_message(App* app) #endif } +static void mpu_timer_cb(void* arg) +{ + App* app = arg; + (void)app; + + float3 accel = { 0 }; + ESP_ERROR_CHECK(new_mpu6050_get_acceleration(&app->mpu, &accel)); + + float3 rotation = { 0 }; + ESP_ERROR_CHECK(new_mpu6050_get_rotation(&app->mpu, &rotation)); + + float temp = 0; + ESP_ERROR_CHECK(new_mpu6050_read_temperature(&app->mpu, &temp)); + + app->accel_acc.x = accel.x; + app->accel_acc.y = accel.y; + app->accel_acc.z = accel.z; + + app->rotation_acc.x = rotation.x; + app->rotation_acc.y = rotation.y; + app->rotation_acc.z = rotation.z; + + app->last_temp = temp; +} + void app_main(void) { ESP_LOGI(TAG, "Initializing"); @@ -96,23 +97,27 @@ void app_main(void) ESP_ERROR_CHECK(esp_netif_init()); ESP_ERROR_CHECK(esp_event_loop_create_default()); - App app = { - .wifi = { - .event_group = 0, - .wifi_retries = 0, - }, + App app = { }; + + esp_timer_handle_t mpu_timer; + esp_timer_create_args_t mpu_timer_config = { + .callback = mpu_timer_cb, + .arg = &app, }; + ESP_ERROR_CHECK(esp_timer_create(&mpu_timer_config, &mpu_timer)); // app_wifi_init(&app.wifi); // app_mqtt_init(&app.mqtt); - ESP_LOGI(TAG, "=== Initializing MPU6050 ==="); -#ifndef NEW_DRIVER - app_mpu_init(&app.mpu); -#else ESP_ERROR_CHECK(new_mpu6050_init(&app.mpu)); -#endif - ESP_LOGI(TAG, "=== MPU6050 initialized ==="); + + ESP_LOGI(TAG, "Calibrating MPU6050"); + ESP_ERROR_CHECK( + new_mpu6050_calibrate(&app.mpu, &(float3) { 0.0f, 0.0f, -1.0f })); + ESP_LOGI(TAG, "MPU6050 calibrated"); + + ESP_ERROR_CHECK(esp_timer_start_periodic(mpu_timer, 40000)); + vTaskDelay(pdMS_TO_TICKS(100)); // app_mqtt_subscribe(&app.mqtt, "/skateboard/configure", configure_cb, // &app);