sfja/h5-projekt-mst
1
0
Fork 0
mirror of https://github.com/Mercantec-GHC/h5-projekt-mst.git synced 2026-08-26 21:27:38 +02:00
Code Issues Packages Projects Releases Wiki Activity

fix driver 2

Browse Source
This commit is contained in:
sfja 2026-04-01 00:19:07 +02:00
parent c7d85ce151
commit 632e7b73fd
4 changed files with 175 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
skateboard/main/CMakeLists.txt
View File

@ -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 ".")

132
skateboard/main/new_mpu6050.c
View File

@ -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 <assert.h>
#include <stdbool.h>
#include <stddef.h>
@ -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;
}

5
skateboard/main/new_mpu6050.h
View File

@ -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);

101
skateboard/main/skateboard.c
View File

@ -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 <stdbool.h>
#include <stdio.h>
#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);