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

simply skateboard

Browse Source
This commit is contained in:
sfja 2026-04-07 12:49:16 +02:00
parent f3d1e59942
commit 636195c7d1
1 changed files with 8 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
skateboard/main/skateboard.c
View File

@ -13,45 +13,12 @@
const char* TAG = "skateboard"; const char* TAG = "skateboard";
typedef struct {
float3 rotation;
float3 uncertainty;
} KalmanState;
static void kalman_update_axis(
float* angle, float* uncertainty, float rotation_rate, float rotation)
{
const float time_delta = 0.04f;
const float std_div0 = 4.0f;
const float std_div1 = 3.0f;
*angle += time_delta * rotation_rate;
*uncertainty += time_delta * time_delta * std_div0 * std_div0;
float gain = *uncertainty * 1 / (*uncertainty + std_div1 * std_div1);
*angle += gain * (rotation - *angle);
*uncertainty *= (1 - gain);
}
void kalman_update(KalmanState* state, float3 rotation_rate, float3 rotation)
{
kalman_update_axis(
&state->rotation.x, &state->uncertainty.x, rotation_rate.x, rotation.x);
kalman_update_axis(
&state->rotation.y, &state->uncertainty.y, rotation_rate.y, rotation.y);
kalman_update_axis(
&state->rotation.z, &state->uncertainty.z, rotation_rate.z, rotation.z);
}
typedef struct App { typedef struct App {
AppWifi wifi; AppWifi wifi;
AppMqtt mqtt; AppMqtt mqtt;
Mpu6050 mpu; Mpu6050 mpu;
float last_temp; float rotation;
float3 gyro_rotation;
float3 accel_rotation;
KalmanState kalman;
} App; } App;
#define msg_buffer_capacity 1024 #define msg_buffer_capacity 1024
@ -75,23 +42,11 @@ static void publish_message(App* app)
int msg_size = snprintf(msg_buffer, int msg_size = snprintf(msg_buffer,
msg_buffer_capacity - 1, msg_buffer_capacity - 1,
"{ gyro: [% 9.4f, % 9.4f, % 9.4f], accel: [% 9.4f, % 9.4f, % 9.4f], " "{\"rotation\":% 9.4f}",
"kalman: [% 9.4f, % 9.4f, % 9.4f] }", app->rotation);
app->gyro_rotation.x,
app->gyro_rotation.y,
app->gyro_rotation.z,
app->accel_rotation.x,
app->accel_rotation.y,
app->accel_rotation.z,
app->kalman.rotation.x,
app->kalman.rotation.y,
app->kalman.rotation.z);
#if false
app_mqtt_publish(&app->mqtt, "/skateboard/update", msg_buffer, msg_size); app_mqtt_publish(&app->mqtt, "/skateboard/update", msg_buffer, msg_size);
#else
ESP_LOGI(TAG, "%.*s", (int)msg_size, msg_buffer); ESP_LOGI(TAG, "%.*s", (int)msg_size, msg_buffer);
#endif
} }
static float calculate_accel_axis_angle( static float calculate_accel_axis_angle(
@ -109,26 +64,7 @@ static void mpu_timer_cb(void* arg)
float3 accel = { 0 }; float3 accel = { 0 };
ESP_ERROR_CHECK(mpu6050_get_acceleration(&app->mpu, &accel)); ESP_ERROR_CHECK(mpu6050_get_acceleration(&app->mpu, &accel));
float3 rotation = { 0 }; app->rotation = calculate_accel_axis_angle(accel.x, accel.y, accel.z);
ESP_ERROR_CHECK(mpu6050_get_rotation(&app->mpu, &rotation));
float temp = 0;
ESP_ERROR_CHECK(mpu6050_read_temperature(&app->mpu, &temp));
app->gyro_rotation.x += rotation.x * 0.04;
app->gyro_rotation.y += rotation.y * 0.04;
app->gyro_rotation.z += rotation.z * 0.04;
app->last_temp = temp;
app->accel_rotation.x
= calculate_accel_axis_angle(accel.x, accel.y, accel.z);
app->accel_rotation.y
= calculate_accel_axis_angle(accel.y, accel.x, accel.z);
app->accel_rotation.z = 0.0f;
// = calculate_accel_axis_angle(accel.z, accel.x, accel.y);
kalman_update(&app->kalman, rotation, app->accel_rotation);
} }
void app_main(void) void app_main(void)
@ -149,8 +85,8 @@ void app_main(void)
}; };
ESP_ERROR_CHECK(esp_timer_create(&mpu_timer_config, &mpu_timer)); ESP_ERROR_CHECK(esp_timer_create(&mpu_timer_config, &mpu_timer));
// app_wifi_init(&app.wifi); app_wifi_init(&app.wifi);
// app_mqtt_init(&app.mqtt); app_mqtt_init(&app.mqtt);
ESP_ERROR_CHECK(mpu6050_init(&app.mpu)); ESP_ERROR_CHECK(mpu6050_init(&app.mpu));
@ -162,8 +98,7 @@ void app_main(void)
ESP_ERROR_CHECK(esp_timer_start_periodic(mpu_timer, 40000)); ESP_ERROR_CHECK(esp_timer_start_periodic(mpu_timer, 40000));
vTaskDelay(pdMS_TO_TICKS(100)); vTaskDelay(pdMS_TO_TICKS(100));
// app_mqtt_subscribe(&app.mqtt, "/skateboard/configure", configure_cb, app_mqtt_subscribe(&app.mqtt, "/skateboard/configure", configure_cb, &app);
// &app);
ESP_LOGI(TAG, "Initialized"); ESP_LOGI(TAG, "Initialized");
ESP_LOGI(TAG, "Free memory: %" PRIu32 " bytes", esp_get_free_heap_size()); ESP_LOGI(TAG, "Free memory: %" PRIu32 " bytes", esp_get_free_heap_size());
@ -171,6 +106,6 @@ void app_main(void)
while (true) { while (true) {
publish_message(&app); publish_message(&app);
vTaskDelay(pdMS_TO_TICKS(200)); vTaskDelay(pdMS_TO_TICKS(100));
} }
} }