idk if working since azores

components/drone_comms

@@ -1 +1 @@
-Subproject commit e93d423065d1f100fbd9da05383ab555261aa6d4
+Subproject commit 1a1fccca83a4a24670715efa4b532d8a7c4e51ca

main/CMakeLists.txt

@@ -1,7 +1,7 @@
 file(GLOB_RECURSE SOURCES "*.cpp")
 
 idf_component_register(SRCS "${SOURCES}"
-                    INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab)
+                    INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab drone_comms)
 
 target_compile_options(${COMPONENT_LIB} PRIVATE 
                        "-Wno-gnu-array-member-paren-init" 

main/env_sens.cpp

@@ -1,30 +1,35 @@
+#include "env_sens.h"
 #include "esp_log.h"
 #include <Adafruit_BME280.h>
 #include <SPI.h>
 #include <Wire.h>
 
-#define SEALEVELPRESSURE_HPA (1013.25)
+#include "freertos/idf_additions.h"
+#include "nav.h"
+
+#define SEALEVELPRESSURE_HPA (1030)
 
 Adafruit_BME280 bme; // use I2C interface
 Adafruit_Sensor *bme_temp = bme.getTemperatureSensor();
 Adafruit_Sensor *bme_pressure = bme.getPressureSensor();
 Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
 
-static const constexpr char *TAG = "IMU";
+static const constexpr char *TAG = "BARO";
 
 namespace env_sens {
-
 void setup() {
 
+  baro_mutex = xSemaphoreCreateMutex();
+
   if (!bme.begin()) {
 
     ESP_LOGE(TAG, "Couldn't find a valid sensor");
     return;
   }
-  bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X2,
+  ESP_LOGI(TAG, "BARO SETUP COMPLETE.");
-                  Adafruit_BME280::SAMPLING_X2, Adafruit_BME280::SAMPLING_NONE,
+  bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X8,
-                  Adafruit_BME280::FILTER_OFF,
+                  Adafruit_BME280::SAMPLING_X8, Adafruit_BME280::SAMPLING_NONE,
-                  Adafruit_BME280::STANDBY_MS_62_5);
+                  Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_10);
 
   bme_temp->printSensorDetails();
   bme_pressure->printSensorDetails();
@@ -33,20 +38,23 @@ void setup() {
 
 float get_temperature() {
   sensors_event_t temp_event;
+
   bme_temp->getEvent(&temp_event);
   return temp_event.temperature;
 }
 
 float get_pressure() {
   sensors_event_t e;
+
   bme_pressure->getEvent(&e);
+
   return e.pressure;
 }
 
 float calculateAltitude(float pressure, float seaLevelPressure,
                         float tempCelsius) {
   float altitude =
-      (((pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) *
+      (((std::pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) *
        (tempCelsius + 273.15)) /
       0.0065;
   return altitude;
@@ -62,4 +70,47 @@ void dbg_sens() {
            get_pressure(), get_altitude());
 }
 
+void baro_poll_task(void *_) {
+  env_sens::setup();
+
+  float last_alt = env_sens::get_altitude();
+  uint32_t last_time = xTaskGetTickCount();
+
+  float filtered_alt = last_alt;
+  const float alt_lpf = 0.1f;
+
+  while (true) {
+    uint32_t now = xTaskGetTickCount();
+    float dt = (now - last_time) * portTICK_PERIOD_MS / 1000.0f;
+
+    if (dt > 0.001f) { // Prevent division by zero
+      float current_alt = env_sens::get_altitude();
+
+      filtered_alt = (alt_lpf * current_alt) + (1.0f - alt_lpf) * filtered_alt;
+
+      float v_z = (filtered_alt - last_alt) / dt;
+
+      if (nav_mutex && xSemaphoreTake(nav_mutex, (TickType_t)20) == pdTRUE) {
+
+        Eigen::Vector3f baro_pos = nav_filter.position;
+        baro_pos.z() = filtered_alt;
+
+        Eigen::Vector3f baro_vel = nav_filter.velocity;
+        baro_vel.z() = v_z;
+
+        // Update the filter with Baro data
+        nav_filter.measure_baro(dt, baro_pos, baro_vel);
+
+        xSemaphoreGive(nav_mutex);
+      }
+
+      last_alt = filtered_alt;
+      last_time = now;
+    }
+
+    // BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal
+    vTaskDelay(pdMS_TO_TICKS(20));
+  }
+}
+
 } // namespace env_sens

main/env_sens.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "freertos/idf_additions.h"
 namespace env_sens {
 
 void setup();
@@ -11,5 +12,8 @@ float get_pressure();
 void dbg_sens();
 
 float get_altitude();
+void baro_poll_task(void *_);
 
 } // namespace env_sens
+
+inline SemaphoreHandle_t baro_mutex = NULL;

main/gps.cpp

@@ -0,0 +1,36 @@
+#include "gps.h"
+#include "esp_log.h"
+#include "nav.h"
+
+static const char *TAG = "GPS_TASK";
+
+void gps_poll_task(void *_) {
+
+  gps = new GPS();
+  gps->begin();
+  gps_mutex = xSemaphoreCreateMutex();
+
+  ESP_LOGI(TAG, "GPS TASK INIT.");
+
+  while (true) {
+    if (xSemaphoreTake(gps_mutex, (TickType_t)1) == pdTRUE) {
+
+      // ESP_LOGI(TAG, "Polling gps.");
+      gps->poll();
+      if (gps->gps_avaliable() && nav_mutex &&
+          xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
+        auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
+        auto coords = gps->get_coordinates();
+        if (coords.has_value()) {
+          nav_filter.measure_gps(1.0, gps->get_coordinates().value(),
+                                 Eigen::Vector3f(vel.x(), vel.y(), 0.0));
+        }
+        xSemaphoreGive(nav_mutex);
+      }
+      xSemaphoreGive(gps_mutex);
+    } else {
+      ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
+    }
+    vTaskDelay(pdMS_TO_TICKS(100));
+  }
+}

main/gps.h

@@ -11,6 +11,7 @@
 #include <Eigen/Dense>
 
 #include "HardwareSerial.h"
+
 #include "esp_log.h"
 #include <Adafruit_GPS.h>
 #include <cmath>
@@ -22,7 +23,7 @@ const float KNOTS_TO_M_SEC = 0.5144444f;
 const float earth_radius = 6371000.0f;
 
 struct GPS {
-  Adafruit_GPS gps;
+  Adafruit_GPS *gps;
   float origin_lat;
   float origin_long;
 
@@ -33,22 +34,22 @@ struct GPS {
   }
 
   void begin() {
-    this->gps = Adafruit_GPS(&Serial2);
+    this->gps = new Adafruit_GPS(&Serial2);
-    this->gps.begin(9600);
+    this->gps->begin(9600);
-    this->gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
+    this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
 
-    this->gps.sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
+    this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
-    this->gps.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
+    this->gps->sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
   }
 
-  bool gps_avaliable() { return this->gps.fix; }
+  bool gps_avaliable() { return this->gps->fix; }
 
   std::optional<Eigen::Vector2f> velocity() {
-    if (!this->gps.fix) {
+    if (!this->gps->fix) {
       return std::nullopt;
     }
-    float speed = this->gps.speed * KNOTS_TO_M_SEC;
+    float speed = this->gps->speed * KNOTS_TO_M_SEC;
-    float angle = this->gps.angle * TO_RAD;
+    float angle = this->gps->angle * TO_RAD;
 
     return Eigen::Vector2f(cos(M_PI_2 - angle), sin(M_PI_2 - angle)) * speed;
   }
@@ -66,28 +67,36 @@ struct GPS {
 
     float y = dLat * earth_radius;
     float x = dLon * earth_radius * std::cos(meanLat);
-    float z = this->gps.altitude;
+    float z = this->gps->altitude;
 
     return Eigen::Vector3f(x, y, z);
   }
 
   std::optional<Eigen::Vector3f> get_coordinates() {
-    if (this->gps.fix == false && this->gps.latitudeDegrees != 0.0 &&
+    if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 ||
-        this->gps.longitudeDegrees != 0.0) {
+        this->gps->longitudeDegrees != 0.0) {
       return std::nullopt;
     }
-    float latitude = this->gps.latitudeDegrees;
+    float latitude = this->gps->latitudeDegrees;
-    float longitude = this->gps.longitudeDegrees;
+    float longitude = this->gps->longitudeDegrees;
 
-    return this->waypoint_to_xyz(latitude, longitude, this->gps.altitude);
+    return this->waypoint_to_xyz(latitude, longitude, this->gps->altitude);
   }
 
   void poll() {
-    this->gps.read();
+    while (this->gps->read()) {
-    if (this->gps.newNMEAreceived()) {
+      if (this->gps->newNMEAreceived()) {
-      ESP_LOGD("GPS", "NMEA LINE: %s", this->gps.lastNMEA());
+        char *line = this->gps->lastNMEA();
-      if (!this->gps.parse(this->gps.lastNMEA()))
+        // ESP_LOGI("GPS", "NMEA LINE: %s", line);
-        return;
+        if (!this->gps->parse(line)) {
+          continue;
+        }
+      }
     }
   }
 };
+
+inline SemaphoreHandle_t gps_mutex;
+inline GPS *gps = nullptr;
+
+void gps_poll_task(void *_);

main/imu.cpp

@@ -2,68 +2,63 @@
 #include "esp_log.h"
 #include "esp_timer.h"
 #include "freertos/idf_additions.h"
+#include "hal/spi_types.h"
+#include "nav.h"
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
 
 static const char *TAG = "IMU";
 
-SemaphoreHandle_t imuStateMutex = NULL;
-
 BNO08x *setup_imu(imu_state *state) {
-  BNO08x *imu = new BNO08x();
+  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;
   }
 
-  if (imuStateMutex == NULL) {
-    imuStateMutex = xSemaphoreCreateMutex();
-  }
-
   imu->dynamic_calibration_autosave_enable();
   imu->dynamic_calibration_enable(BNO08xCalSel::all);
 
   imu->rpt.rv_game.enable(2500UL);
   imu->rpt.linear_accelerometer.enable(2500UL);
-  // imu->rpt.rv_game.tare();
 
   imu->register_cb([imu, state]() {
     if (imu->rpt.rv_game.has_new_data()) {
       auto sens_rot = imu->rpt.rv_game.get_quat();
-      auto euler = imu->rpt.rv_game.get_euler();
+      state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
-      if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
-        state->euler_ang = {euler.x, euler.y, euler.z};
-        state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
-        xSemaphoreGive(imuStateMutex);
-      }
     }
 
     if (imu->rpt.linear_accelerometer.has_new_data()) {
 
-      if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
+      auto sens_accel = imu->rpt.linear_accelerometer.get();
-        auto sens_accel = imu->rpt.linear_accelerometer.get();
+      state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
-        state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
 
-        int64_t current_time = esp_timer_get_time();
+      int64_t current_time = esp_timer_get_time();
 
-        if (state->last_time != -1) {
+      if (state->last_time != -1) {
-          float dt = (current_time - state->last_time) / 1000000.0f;
+        float dt = ((float)(current_time - state->last_time)) / 1000000.0f;
+        Vec3C accel_global = apply_rot(&state->accel, &state->rot);
 
-          Vec3C delta = apply_rot(&state->accel, &state->rot);
+        if (xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
-
+          nav_filter.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
-          // Trapezoidal Integration for Velocity: v = v0 + 0.5 * (a + a0) * dt
+                                                 accel_global.z));
-          delta.x += state->last_accel.x;
+          xSemaphoreGive(nav_mutex);
-          delta.y += state->last_accel.y;
+        } else {
-          delta.z += state->last_accel.z;
+          ESP_LOGE(TAG, "Failed to get mutex.");
-
-          multiply_vec_by(&delta, dt * 0.5f);
-          add_to_vec(&state->vel, &delta);
-
-          state->last_accel = state->accel;
         }
-        state->last_time = current_time;
-
-        xSemaphoreGive(imuStateMutex);
       }
+      state->last_time = current_time;
     }
   });
 

main/imu.h

@@ -4,19 +4,12 @@
 #include <cstdint>
 
 #include "BNO08x.hpp"
-#include "BNO08xGlobalTypes.hpp"
 #include "drone_controller.h"
-#include "freertos/idf_additions.h"
 
 struct imu_state {
   Vec3C accel = {0, 0, 0};
-  Vec3C last_accel = {0, 0, 0};
   QuatC rot = {0, 0, 0, 1};
-  Vec3C vel = {0, 0, 0};
   int64_t last_time = -1;
-  Vec3C euler_ang = {0, 0, 0};
 };
 
 BNO08x *setup_imu(imu_state *state);
-
-extern SemaphoreHandle_t imuStateMutex;

main/main.cpp

@@ -1,53 +1,92 @@
 
-#include "esp32-hal.h"
+#include "driver/gpio.h"
+#include "drone_comms.h"
 #include "esp_log.h"
-#include <cmath>
+#include "freertos/FreeRTOS.h"
-#include <cstdio>
-
 #include "freertos/idf_additions.h"
 #include "freertos/projdefs.h"
+#include "freertos/task.h"
+#include "imu.h"
+#include <Arduino.h>
+
+#include "env_sens.h"
 #include "gps.h"
+#include "nav.h"
+#include "radio.h"
 
-static const constexpr char *TAG = "Main";
+static const char *TAG = "MAIN";
-
-SemaphoreHandle_t gps_mutex;
-GPS *gps = nullptr;
-
-void gps_poll_task(void *_) {
-  while (true) {
-    if (xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
-      gps->poll();
-      xSemaphoreGive(gps_mutex);
-    }
-    vTaskDelay(pdMS_TO_TICKS(50));
-  }
-}
-
-void gps_poll_init() {
-  gps_mutex = xSemaphoreCreateMutex();
-  if (gps_mutex != NULL) {
-    xTaskCreate(gps_poll_task, "Writer", 2048, NULL, 1, NULL);
-  }
-}
 
 extern "C" void app_main(void) {
+
+  nav_mutex = xSemaphoreCreateMutex();
   initArduino();
-  gps = new GPS();
+  gpio_install_isr_service(0);
-  gps_poll_init();
+  Serial.begin(115200);
 
+  static imu_state imu_state;
+  auto _ = setup_imu(&imu_state);
+
+  // xTaskCreatePinnedToCore(radio_rx_task, // Function name
+  //                         "radio_rx",    // Name for debugging
+  //                         4096,          // Stack size in bytes
+  //                         NULL,          // Parameters
+  //                         1,             // Priority (higher = more urgent)
+  //                         NULL,          // Task handle
+  //                         1              // Core ID
+  // );
+  //
+  xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
+
+  xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 1, NULL);
+
+  ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
+
+  Eigen::Vector3f local_pos = {0, 0, 0};
+  Eigen::Vector3f local_vel = {0, 0, 0};
+  bool nav_data_ready = false;
   while (true) {
-    if (xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
+    // if (controller_input_semaphore &&
-      ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
+    //     xSemaphoreTake(controller_input_semaphore, (TickType_t)30) == pdTRUE)
-               gps->gps.latitudeDegrees, gps->gps.longitudeDegrees,
+    //     {
-               gps->gps.altitude);
+    //   inp = current_controller_input;
-      if (gps->gps_avaliable()) {
+    //   xSemaphoreGive(controller_input_semaphore);
+    //
+    //   ESP_LOGI(TAG, "CONT INPUT: (%f, %f), (%f,%f)", inp.lx, inp.ly, inp.rx,
+    //            inp.ry);
+    // }
 
-        auto D_pos = gps->get_coordinates().value();
+    if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
-        ESP_LOGI(TAG, "    -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
+      if (gps->gps_avaliable()) {
-                 D_pos[2]);
+        ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
+                 gps->gps->latitudeDegrees, gps->gps->longitudeDegrees,
+                 gps->gps->altitude);
+
+        auto D_pos_cond = gps->get_coordinates();
+        if (D_pos_cond.has_value()) {
+          auto D_pos = D_pos_cond.value();
+
+          ESP_LOGI(TAG, "    -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
+                   D_pos[2]);
+        }
       }
       xSemaphoreGive(gps_mutex);
     }
-    vTaskDelay(pdMS_TO_TICKS(250));
+    env_sens::dbg_sens();
+
+    if (nav_mutex && xSemaphoreTake(nav_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
+      local_pos = nav_filter.position;
+      local_vel = nav_filter.velocity;
+      nav_data_ready = true;
+      xSemaphoreGive(nav_mutex); // RELEASE IMMEDIATELY
+    }
+
+    if (nav_data_ready) {
+      ESP_LOGI(TAG, "nav(pos): (%f, %f, %f)", local_pos[0], local_pos[1],
+               local_pos[2]);
+      ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
+               local_vel[2]);
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(1000));
   }
 }

main/nav.h

@@ -0,0 +1,98 @@
+#pragma once
+#include "freertos/idf_additions.h"
+#include <cmath>
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+inline float getYawDifference(const Eigen::Vector3f &v_gps,
+                              const Eigen::Vector3f &v_imu) {
+  float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
+  float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
+
+  float delta_yaw = yaw_gps - yaw_imu;
+
+  return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
+}
+
+struct nav_compl {
+  Eigen::Vector3f position = Eigen::Vector3f::Zero();
+  Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
+  float yaw_offset = 0.0f;
+
+  // Time Constants per axis (X, Y, Z)
+  // Lower = faster tracking of GPS; Higher = smoother/more IMU trust
+  Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
+  Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
+
+  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
+  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
+
+  float tau_yaw = 2.0f; // Yaw remains a scalar
+
+  void predict(float dt, Eigen::Vector3f accel) {
+    // Rotate body-frame accel to world-frame
+    Eigen::Vector3f accel_rotated =
+        Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
+
+    Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
+
+    // Trapezoidal integration for position
+    this->position += (this->velocity + next_velocity) * 0.5f * dt;
+    this->velocity = next_velocity;
+  }
+
+  void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
+    // Calculate Alpha vectors using element-wise operations
+    // Formula: alpha = dt / (tau + dt)
+    Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
+    Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
+    float alpha_yaw = dt / (tau_yaw + dt);
+
+    // 1. Position Update (Element-wise LPF)
+    // res = (1 - alpha) * state + alpha * measurement
+    this->position =
+        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
+        alpha_pos.array() * gps_pos.array();
+
+    // 2. Yaw Correction (only if moving > 1.0 m/s)
+    if (gps_vel.norm() > 1.0f) {
+      float yaw_delta = getYawDifference(gps_vel, this->velocity);
+      this->yaw_offset += yaw_delta * alpha_yaw;
+
+      this->yaw_offset =
+          std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
+    }
+
+    // 3. Velocity Update (Element-wise LPF)
+    this->velocity =
+        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
+        alpha_vel.array() * gps_vel.array();
+  }
+
+  void measure_baro(float dt, Eigen::Vector3f baro_pos,
+                    Eigen::Vector3f baro_vel) {
+    // Calculate Alpha vectors using element-wise operations
+    // Formula: alpha = dt / (tau + dt)
+    Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
+    Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);
+
+    this->position =
+        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
+        alpha_pos.array() * baro_pos.array();
+
+    this->velocity =
+        (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
+        alpha_vel.array() * baro_vel.array();
+  }
+};
+
+inline SemaphoreHandle_t nav_mutex = NULL;
+inline nav_compl nav_filter;

main/pos.h

@@ -1,65 +0,0 @@
-
-#ifdef PS
-#undef PS
-#endif
-
-#ifdef F
-#undef F
-#endif
-
-#include <Eigen/Dense>
-
-inline float getYawDifference(const Eigen::Vector3f &v_gps,
-                              const Eigen::Vector3f &v_imu) {
-  // 1. Extract the 2D components (Project to XY plane)
-  float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
-  float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
-
-  // 2. Calculate the raw difference
-  float delta_yaw = yaw_gps - yaw_imu;
-
-  // 3. Normalize the angle to [-PI, PI]
-  while (delta_yaw > M_PI)
-    delta_yaw -= 2.0 * M_PI;
-  while (delta_yaw < -M_PI)
-    delta_yaw += 2.0 * M_PI;
-
-  return delta_yaw;
-}
-
-struct nav_compl {
-  Eigen::Vector3f position = Eigen::Vector3f::Zero();
-  Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
-  float yaw_offset = 0.0;
-  float yaw_delta_weight = 0.04;
-
-  Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.03f);
-  Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.03f);
-
-  void predict(float dt, Eigen::Vector3f accel) {
-    Eigen::Vector3f accel_rotated =
-        Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3d::UnitZ()) * accel;
-
-    Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
-
-    this->position += (this->velocity + next_velocity) * 0.5f * dt;
-
-    this->velocity = next_velocity;
-  }
-
-  void measure(Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
-    this->position = (Eigen::Vector3f::Ones() - gps_weight_pos)
-                         .cwiseProduct(this->position) +
-                     gps_weight_pos.cwiseProduct(gps_pos);
-
-    if (gps_vel.norm() > 1) {
-      float yaw_delta = getYawDifference(gps_vel, this->velocity);
-      yaw_offset =
-          yaw_offset * (1 - yaw_delta_weight) + yaw_delta * yaw_delta_weight;
-    }
-
-    this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel)
-                         .cwiseProduct(this->velocity) +
-                     gps_weight_vel.cwiseProduct(gps_vel);
-  }
-};

main/radio.cpp

@@ -0,0 +1,92 @@
+#include "radio.h"
+#include "esp32-hal-gpio.h"
+#include "esp_log.h"
+#include "freertos/idf_additions.h"
+#include <Arduino.h>
+#include <RFM69.h>
+#include <SPI.h>
+#include <cstdint>
+#include <cstring>
+
+#include <drone_comms.h>
+
+#define RFM69_RST 14
+#define RFM69_CS 12
+#define RFM69_INT 39
+#define SPI_SCLK 18
+#define SPI_MISO 19
+#define SPI_MOSI 23
+
+#define FREQUENCY RF69_433MHZ
+#define NODEID 1
+#define NETWORKID 100
+
+static const char *TAG = "RADIO_TASK";
+
+#define PACKET_QUEUE_CAP 10
+
+void radio_rx_task(void *pvParameters) {
+  ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID());
+  packet_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
+  controller_input_semaphore = xSemaphoreCreateMutex();
+
+  pinMode(RFM69_CS, OUTPUT);
+  pinMode(RFM69_INT, INPUT);
+
+  // 1. Setup SPI for this task
+  SPIClass vspi(VSPI);
+  vspi.begin(SPI_SCLK, SPI_MISO, SPI_MOSI, 34);
+
+  // 3. Hardware Reset
+  pinMode(RFM69_RST, OUTPUT);
+  digitalWrite(RFM69_RST, HIGH);
+  vTaskDelay(pdMS_TO_TICKS(10));
+  digitalWrite(RFM69_RST, LOW);
+  vTaskDelay(pdMS_TO_TICKS(50));
+
+  RFM69 radio(RFM69_CS, RFM69_INT, true, &vspi);
+
+  if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) {
+    radio.setHighPower(true);
+    ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
+  } else {
+    ESP_LOGE(TAG, "Radio Init FAILED! Deleting Task.");
+    vTaskDelete(NULL);
+  }
+
+  // 4. Polling Loop
+  while (1) {
+    if (radio.receiveDone()) {
+      // ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID,
+      // radio.RSSI,
+      //          radio.DATALEN);
+
+      memset(packet_data, '\0', sizeof(packet_data));
+      memcpy(packet_data, radio.DATA, radio.DATALEN);
+
+      PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]);
+      if (packet_type == CONTROLLER_INPUT) {
+
+        controller_input *inp =
+            (controller_input *)(&packet_data[0] + sizeof(PACKET_TYPE));
+
+        if (xSemaphoreTake(controller_input_semaphore, (TickType_t)50) ==
+            pdTRUE) {
+          current_controller_input = *inp;
+          xSemaphoreGive(controller_input_semaphore);
+        }
+
+      } else {
+        xQueueSend(packet_queue, &packet_data, portMAX_DELAY);
+      }
+
+      if (radio.ACKRequested()) {
+        radio.sendACK();
+      }
+    }
+
+    // Small delay to prevent Watchdog trigger and allow other tasks to run
+    // 1ms is usually enough for high-speed polling
+    vTaskDelay(pdMS_TO_TICKS(1));
+  }
+}

main/radio.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include "drone_comms.h"
+#include "freertos/idf_additions.h"
+
+inline QueueHandle_t packet_queue = NULL;
+inline SemaphoreHandle_t controller_input_semaphore = NULL;
+inline controller_input current_controller_input;
+
+void radio_rx_task(void *pvParameters);