make imu less blocking by implementing double buffering for data.

main/env_sens.cpp

@@ -90,14 +90,13 @@ void baro_poll_task(void *_) {
 
       float v_z = (filtered_alt - last_alt) / dt;
 
-      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;
+      if (sens_fus_mutex &&
+          xSemaphoreTake(sens_fus_mutex, (TickType_t)20) == pdTRUE) {
 
         // Update the filter with Baro data
         sens_fus.measure_baro(dt, baro_pos, baro_vel);

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<Eigen::Vector3f> 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) {
+      if (gps->gps_avaliable()) {
-        auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
+        local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
-        auto coords = gps->get_coordinates();
+        local_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);
       }
       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
   }
 }

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();
+      needs_updating = true;
-      state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
 
+      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) {
+      if (local_state->last_time != -1) {
-        float dt = ((float)(current_time - state->last_time)) / 1000000.0f;
+
-        Vec3C accel_global = apply_rot(&state->accel, &state->rot);
+        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;
 }

main/imu.h

@@ -1,6 +1,8 @@
 #pragma once
 
 #include "drone_controller.h"
+
+#include "freertos/idf_additions.h"
 #include <cstdint>
 
 #ifdef LOW
@@ -12,10 +14,25 @@
 
 #include "BNO08x.hpp"
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 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;

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
 };
 

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::Vector3f accel_rotated = yaw_rotation_matrix * accel;
-        Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * 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