compiling with shit includes, should fix

main/drone.cpp

@@ -94,11 +94,11 @@ void drone_controller_task(void *params) {
   Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero();
 
   while (true) {
-    if (xSemaphoreTake(imu_state_mutex, 1)) {
+    if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) {
       imu_state_local = imu_state_var;
       xSemaphoreGive(imu_state_mutex);
     }
-    if (xSemaphoreTake(sens_fus_mutex, 1)) {
+    if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) {
       position_local = sens_fus.position;
       velocity_local = sens_fus.velocity;
       xSemaphoreGive(sens_fus_mutex);
@@ -165,6 +165,7 @@ void motor_throttles_task(void *params) {
     }
   }
 
+  uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
   while (true) {
     for (int i = 0; i < 4; i++) {
       if (!killswitch_active) {

main/env_sens.cpp

@@ -4,6 +4,16 @@
 #include <SPI.h>
 #include <Wire.h>
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 #include "freertos/idf_additions.h"
 #include "sens_fus.h"
 

main/gps.cpp

@@ -14,6 +14,7 @@ void gps_poll_task(void *_) {
 
   while (true) {
     bool has_new_data = false;
+    bool available = false;
     Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
     std::optional<Eigen::Vector3f> local_coords;
 
@@ -21,6 +22,7 @@ void gps_poll_task(void *_) {
       gps->poll();
 
       if (gps->gps_avaliable()) {
+        available = true;
         local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
         local_coords = gps->get_coordinates();
       }
@@ -31,18 +33,20 @@ void gps_poll_task(void *_) {
       ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
     }
 
-    if (has_new_data && sens_fus_mutex) {
+    if (has_new_data && available && 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));
+
+        ESP_LOGI(TAG, "applied gps measure to sens_fus");
         xSemaphoreGive(sens_fus_mutex);
       } else {
 
-        ESP_LOGD(TAG, "Sens_fus busy, skipping GPS update.");
+        ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
       }
     }
 
-    vTaskDelay(pdMS_TO_TICKS(50)); // 10Hz polling
+    vTaskDelay(pdMS_TO_TICKS(100)); // 10Hz polling
   }
 }

main/gps.h

@@ -83,15 +83,12 @@ struct GPS {
   }
 
   void poll() {
-    while (this->gps->read()) {
+    // char c = this->gps->read();
-      if (this->gps->newNMEAreceived()) {
+    // if (this->gps->newNMEAreceived()) {
-        char *line = this->gps->lastNMEA();
+    //   char *line = this->gps->lastNMEA();
-        // ESP_LOGI("GPS", "NMEA LINE: %s", line);
+    //   ESP_LOGI("GPS", "NMEA LINE: %s", line);
-        if (!this->gps->parse(line)) {
+    //   this->gps->parse(line);
-          continue;
+    // }
-        }
-      }
-    }
   }
 };
 

main/imu.cpp

@@ -1,7 +1,6 @@
 #include "imu.h"
 #include "esp_log.h"
 #include "esp_timer.h"
-#include "freertos/idf_additions.h"
 #include "hal/spi_types.h"
 #include "sens_fus.h"
 
@@ -15,10 +14,13 @@
 
 #include <Eigen/Dense>
 
+#include "freertos/idf_additions.h"
+
 static const char *TAG = "IMU";
 
 void setup_imu() {
   imu_state *local_state = new imu_state;
+  imu_state_mutex = xSemaphoreCreateMutex();
 
   BNO08x *imu = new BNO08x(
       bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25,
@@ -33,10 +35,12 @@ void setup_imu() {
 
   imu->rpt.rv_game.enable(2500UL);
   imu->rpt.linear_accelerometer.enable(2500UL);
-  imu->rpt.cal_gyro.enable(1000UL);
+  imu->rpt.cal_gyro.enable(2500UL);
 
   imu->register_cb([imu, local_state]() {
     bool needs_updating = false;
+    if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
+      return;
 
     if (imu->rpt.rv_game.has_new_data()) {
 
@@ -71,9 +75,16 @@ void setup_imu() {
         dcont::Vec3C accel_global =
             dcont::apply_rot(&local_state->accel, &local_state->rot);
 
-        if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
+        if (xSemaphoreTake(sens_fus_mutex, (TickType_t)0) == pdTRUE) {
+
+          ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x,
+                   accel_global.y, accel_global.z, dt);
+
           sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
                                                accel_global.z));
+
+          ESP_LOGE(TAG, "vel: (%f, %f, %f)", sens_fus.velocity.x(),
+                   sens_fus.velocity.z(), sens_fus.velocity.y());
           xSemaphoreGive(sens_fus_mutex);
         } else {
           ESP_LOGE(TAG, "Failed to get sens_fus mutex.");
@@ -83,7 +94,7 @@ void setup_imu() {
     }
 
     if (needs_updating) {
-      if (xSemaphoreTake(imu_state_mutex, 2)) {
+      if (xSemaphoreTake(imu_state_mutex, 0)) {
         imu_state_var = *local_state;
         xSemaphoreGive(imu_state_mutex);
       }

main/main.cpp

@@ -1,5 +1,14 @@
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
 #include "Eigen/Core"
+
 #include "driver/gpio.h"
 #include "drone.h"
 #include "drone_comms.h"
@@ -9,7 +18,6 @@
 #include "freertos/idf_additions.h"
 #include "freertos/projdefs.h"
 #include "freertos/task.h"
-#include <Arduino.h>
 #include <cstddef>
 #include <cstdint>
 #include <optional>
@@ -33,8 +41,6 @@ extern "C" void app_main(void) {
   gpio_install_isr_service(0);
   Serial.begin(115200);
 
-  setup_imu();
-
   xTaskCreatePinnedToCore(radio_task,   // Function name
                           "radio_rxtx", // Name for debugging
                           4096,         // Stack size in bytes
@@ -43,11 +49,11 @@ extern "C" void app_main(void) {
                           NULL,         // Task handle
                           0             // Core ID
   );
-
+  //
   xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
-
+  //
   xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
-
+  //
   xTaskCreatePinnedToCore(drone_controller_task,   // Function name
                           "drone_controller_task", // Name for debugging
                           1024 * 32,               // Stack size in bytes
@@ -57,28 +63,32 @@ extern "C" void app_main(void) {
                           1     // Core ID
   );
 
-  xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
+  // xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
-                          "motor_throttles_task", // Name for debugging
+  //                         "motor_throttles_task", // Name for debugging
-                          1024 * 4,               // Stack size in bytes
+  //                         1024 * 4,               // Stack size in bytes
-                          NULL,                   // Parameters
+  //                         NULL,                   // Parameters
-                          20,   // Priority (higher = more urgent)
+  //                         30,   // Priority (higher = more urgent)
-                          NULL, // Task handle
+  //                         NULL, // Task handle
-                          1     // Core ID
+  //                         1     // Core ID
-  );
+  // );
 
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
 
+  setup_imu();
+
   Eigen::Vector3f local_pos = {0, 0, 0};
   Eigen::Vector3f local_vel = {0, 0, 0};
   bool nav_data_ready = false;
 
   uint64_t last_print_time = 0;
   while (true) {
-    while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
+    while (packet_tx_queue &&
+           xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
       handle_packet(&packet_data[0]);
     }
 
     if (millis() > last_print_time + 1000) {
+      last_print_time = millis();
 
       std::optional<Eigen::Vector3f> coords;
       float lat, lon, alt;

main/nav.h

@@ -1,12 +1,5 @@
 #pragma once
 
-#include "Eigen/Core"
-#include "freertos/idf_additions.h"
-#include "gps.h"
-#include <cmath>
-#include <cstdint>
-#include <optional>
-
 #ifdef PS
 #undef PS
 #endif
@@ -17,6 +10,12 @@
 
 #include <Eigen/Dense>
 
+#include "freertos/idf_additions.h"
+#include "gps.h"
+#include <cmath>
+#include <cstdint>
+#include <optional>
+
 #define WAYPOINT_COUNT 8
 
 struct waypoint {

main/packet_handler.cpp

@@ -1,6 +1,14 @@
-#include "packet_handler.h"
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
 
 #include "Eigen/Core"
+
 #include "drone.h"
 #include "drone_comms.h"
 #include "drone_controller.h"
@@ -8,6 +16,7 @@
 #include "freertos/idf_additions.h"
 #include "gps.h"
 #include "nav.h"
+#include "packet_handler.h"
 #include "portmacro.h"
 #include "radio.h"
 #include "sens_fus.h"

main/packet_handler.h

@@ -2,8 +2,17 @@
 
 #include <cstdint>
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
 #include "drone_comms.h"
 #include "freertos/idf_additions.h"
+#include <Eigen/Dense>
 
 void handle_packet(uint8_t *packet_addr);
 

main/radio.cpp

@@ -1,9 +1,20 @@
 #include "radio.h"
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 #include "Esp.h"
 #include "esp32-hal-gpio.h"
 #include "esp_log.h"
 #include "freertos/idf_additions.h"
-#include <Arduino.h>
+#include "packet_handler.h"
 #include <RFM69.h>
 #include <SPI.h>
 #include <cstdint>

main/sens_fus.h

@@ -1,5 +1,4 @@
 #pragma once
-#include "freertos/idf_additions.h"
 #include <cmath>
 
 #ifdef PS
@@ -12,6 +11,8 @@
 
 #include <Eigen/Dense>
 
+#include "freertos/idf_additions.h"
+
 inline float getYawDifference(const Eigen::Vector3f &v_gps,
                               const Eigen::Vector3f &v_imu) {
   float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
@@ -41,8 +42,7 @@ struct sens_fus_compl {
 
   float tau_yaw = 10.0f; // Yaw remains a scalar
 
-  Eigen::Matrix3f
+  Eigen::Matrix3f yaw_rotation_matrix;
-      yaw_rotation_matrix; // Pre-compute this when yaw_offset changes
 
   void update_yaw_matrix() {
     yaw_rotation_matrix =