From 3f4c7e66cd6e7fbe75fd41d7c97c0784be808b07 Mon Sep 17 00:00:00 2001 From: franchioping Date: Fri, 3 Apr 2026 19:13:39 +0100 Subject: [PATCH] make imu less blocking by implementing double buffering for data. --- main/env_sens.cpp | 11 +++++------ main/gps.cpp | 38 +++++++++++++++++++++++------------ main/imu.cpp | 50 +++++++++++++++++++++++++++++++++++------------ main/imu.h | 19 +++++++++++++++++- main/nav.h | 2 +- main/sens_fus.h | 13 ++++++++++-- 6 files changed, 98 insertions(+), 35 deletions(-) diff --git a/main/env_sens.cpp b/main/env_sens.cpp index c12532f..dd6cdd3 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -90,15 +90,14 @@ void baro_poll_task(void *_) { float v_z = (filtered_alt - last_alt) / dt; + Eigen::Vector3f baro_pos = sens_fus.position; + baro_pos.z() = filtered_alt; + + Eigen::Vector3f baro_vel = sens_fus.velocity; + baro_vel.z() = v_z; if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, (TickType_t)20) == pdTRUE) { - Eigen::Vector3f baro_pos = sens_fus.position; - baro_pos.z() = filtered_alt; - - Eigen::Vector3f baro_vel = sens_fus.velocity; - baro_vel.z() = v_z; - // Update the filter with Baro data sens_fus.measure_baro(dt, baro_pos, baro_vel); diff --git a/main/gps.cpp b/main/gps.cpp index cec8c0d..71c3941 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -1,11 +1,11 @@ #include "gps.h" +#include "Eigen/Core" #include "esp_log.h" #include "sens_fus.h" static const char *TAG = "GPS_TASK"; void gps_poll_task(void *_) { - gps = new GPS(); gps->begin(); gps_mutex = xSemaphoreCreateMutex(); @@ -13,24 +13,36 @@ void gps_poll_task(void *_) { ESP_LOGI(TAG, "GPS TASK INIT."); while (true) { - if (xSemaphoreTake(gps_mutex, (TickType_t)1) == pdTRUE) { + bool has_new_data = false; + Eigen::Vector2f local_vel = Eigen::Vector2f::Zero(); + std::optional local_coords; - // ESP_LOGI(TAG, "Polling gps."); + if (xSemaphoreTake(gps_mutex, pdMS_TO_TICKS(50)) == pdTRUE) { gps->poll(); - if (gps->gps_avaliable() && sens_fus_mutex && - xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) { - auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero()); - auto coords = gps->get_coordinates(); - if (coords.has_value()) { - sens_fus.measure_gps(1.0, gps->get_coordinates().value(), - Eigen::Vector3f(vel.x(), vel.y(), 0.0)); - } - xSemaphoreGive(sens_fus_mutex); + + if (gps->gps_avaliable()) { + local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero()); + local_coords = gps->get_coordinates(); } xSemaphoreGive(gps_mutex); + + has_new_data = local_coords.has_value(); } else { ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX"); } - vTaskDelay(pdMS_TO_TICKS(100)); + + if (has_new_data && sens_fus_mutex) { + if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) { + sens_fus.measure_gps( + 1.0f, local_coords.value(), + Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f)); + xSemaphoreGive(sens_fus_mutex); + } else { + + ESP_LOGD(TAG, "Sens_fus busy, skipping GPS update."); + } + } + + vTaskDelay(pdMS_TO_TICKS(50)); // 10Hz polling } } diff --git a/main/imu.cpp b/main/imu.cpp index 04d210e..fc89abd 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -17,14 +17,15 @@ static const char *TAG = "IMU"; -BNO08x *setup_imu(imu_state *state) { +void setup_imu() { + imu_state *local_state = new imu_state; + BNO08x *imu = new BNO08x( bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25, GPIO_NUM_33, GPIO_NUM_36, GPIO_NUM_32, 2000000, false)); if (!imu->initialize()) { ESP_LOGE(TAG, "BNO08x Init failure."); - return nullptr; } imu->dynamic_calibration_autosave_enable(); @@ -32,36 +33,61 @@ BNO08x *setup_imu(imu_state *state) { imu->rpt.rv_game.enable(2500UL); imu->rpt.linear_accelerometer.enable(2500UL); + imu->rpt.cal_gyro.enable(2500UL); + + imu->register_cb([imu, local_state]() { + bool needs_updating = false; - imu->register_cb([imu, state]() { if (imu->rpt.rv_game.has_new_data()) { + + needs_updating = true; + auto sens_rot = imu->rpt.rv_game.get_quat(); - state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real}; + auto sens_euler = imu->rpt.rv_game.get_euler(); + local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real}; + local_state->rot_euler = {sens_euler.x, sens_euler.y, sens_euler.z}; + } + + if (imu->rpt.cal_gyro.has_new_data()) { + + needs_updating = true; + + auto cal_gyro = imu->rpt.cal_gyro.get(); + local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z}; } if (imu->rpt.linear_accelerometer.has_new_data()) { - auto sens_accel = imu->rpt.linear_accelerometer.get(); - state->accel = {sens_accel.x, sens_accel.y, sens_accel.z}; + needs_updating = true; + auto sens_accel = imu->rpt.linear_accelerometer.get(); + local_state->accel = {sens_accel.x, sens_accel.y, sens_accel.z}; int64_t current_time = esp_timer_get_time(); - if (state->last_time != -1) { - float dt = ((float)(current_time - state->last_time)) / 1000000.0f; - Vec3C accel_global = apply_rot(&state->accel, &state->rot); + if (local_state->last_time != -1) { + + float dt = + ((float)(current_time - local_state->last_time)) / 1000000.0f; + Vec3C accel_global = apply_rot(&local_state->accel, &local_state->rot); if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) { sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y, accel_global.z)); xSemaphoreGive(sens_fus_mutex); } else { - ESP_LOGE(TAG, "Failed to get mutex."); + ESP_LOGE(TAG, "Failed to get sens_fus mutex."); } } - state->last_time = current_time; + local_state->last_time = current_time; + } + + if (needs_updating) { + if (xSemaphoreTake(imu_state_mutex, 2)) { + imu_state_var = *local_state; + xSemaphoreGive(imu_state_mutex); + } } }); ESP_LOGI(TAG, "IMU Setup Success."); - return imu; } diff --git a/main/imu.h b/main/imu.h index 0287c45..2c83540 100644 --- a/main/imu.h +++ b/main/imu.h @@ -1,6 +1,8 @@ #pragma once #include "drone_controller.h" + +#include "freertos/idf_additions.h" #include #ifdef LOW @@ -12,10 +14,25 @@ #include "BNO08x.hpp" +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include + struct imu_state { Vec3C accel = {0, 0, 0}; QuatC rot = {0, 0, 0, 1}; int64_t last_time = -1; + Eigen::Vector3f angvel; + Eigen::Vector3f rot_euler; }; -BNO08x *setup_imu(imu_state *state); +void setup_imu(); + +inline SemaphoreHandle_t imu_state_mutex = NULL; +inline imu_state imu_state_var; diff --git a/main/nav.h b/main/nav.h index f69aad2..1c3f49d 100644 --- a/main/nav.h +++ b/main/nav.h @@ -17,7 +17,7 @@ #define WAYPOINT_COUNT 8 struct waypoint { - Eigen::Vector3f coords; // long, lat, alt + Eigen::Vector3f coords; // lon, lat, alt bool active; // active or to be skipped }; diff --git a/main/sens_fus.h b/main/sens_fus.h index 94c5844..9660d12 100644 --- a/main/sens_fus.h +++ b/main/sens_fus.h @@ -41,9 +41,17 @@ struct sens_fus_compl { float tau_yaw = 10.0f; // Yaw remains a scalar + Eigen::Matrix3f + yaw_rotation_matrix; // Pre-compute this when yaw_offset changes + + void update_yaw_matrix() { + yaw_rotation_matrix = + Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) + .toRotationMatrix(); + } + void predict(float dt, Eigen::Vector3f accel) { - Eigen::Vector3f accel_rotated = - Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel; + Eigen::Vector3f accel_rotated = yaw_rotation_matrix * accel; Eigen::Vector3f next_velocity = this->velocity + (this->last_accel_world + accel_rotated) * 0.5f * dt; @@ -72,6 +80,7 @@ struct sens_fus_compl { this->yaw_offset = std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset)); + this->update_yaw_matrix(); } // res = (1 - alpha) * state + alpha * measurement