test rollback after

Reviewed-on: #1

.gitmodules

@@ -17,3 +17,21 @@
 [submodule "components/esp32_Adafruit_Sensor"]
 	path = components/esp32_Adafruit_Sensor
 	url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_Sensor.git
+[submodule "components/DShotRMT"]
+	path = components/DShotRMT
+	url = ssh://git@forgejo.galard.uk:222/Cansat/DShotRMT.git
+
+
+[submodule "components/RFM69_LowPowerLab"]
+	path = components/RFM69_LowPowerLab
+	url = ssh://git@forgejo.galard.uk:222/Cansat/RFM69_LowPowerLab.git
+
+
+[submodule "components/SPIFlash_LowPowerLab"]
+	path = components/SPIFlash_LowPowerLab
+	url = ssh://git@forgejo.galard.uk:222/Cansat/SPIFlash_LowPowerLab.git
+
+
+[submodule "components/drone_comms"]
+	path = components/drone_comms
+	url = ssh://git@forgejo.galard.uk:222/Cansat/drone_comms.git

components/DShotRMT

@@ -0,0 +1 @@
+Subproject commit 86d682313234556720dc63ad53aace47ab3d190c

components/RFM69_LowPowerLab

@@ -0,0 +1 @@
+Subproject commit 9b083db90723c4911ac79edb6c1aa35c4c127cb8

components/SPIFlash_LowPowerLab

@@ -0,0 +1 @@
+Subproject commit 60ede5c9393ca7bbcf3ea62a55e26f6234b29f7c

components/drone_comms

@@ -0,0 +1 @@
+Subproject commit 24803f37ed9a5824682f7e83535c43392db21c20

components/drone_controller

@@ -1 +1 @@
-Subproject commit 3ad9eb42b9734d72c3f6fbc3d3e107d21a32b589
+Subproject commit 3504bc479075bbc363697edc4123978e4342c823

components/esp32_Adafruit_GPS

@@ -1 +1 @@
-Subproject commit 3df2e79d866d1a46fe79f9094a8647a3fd8c5c40
+Subproject commit 06ee1eea5a5073337591edc713a755306cea1331

dependencies.lock

@@ -154,8 +154,18 @@ dependencies:
       registry_url: https://components.espressif.com
       type: service
     version: 0.6.1~4
+  espressif/dl_fft:
+    component_hash: 7dadbd644c0d7ba4733cc3726ec4cff6edf27b043725e1115861dec1609a3d28
+    dependencies:
+    - name: idf
+      require: private
+      version: '>=5.0'
+    source:
+      registry_url: https://components.espressif.com
+      type: service
+    version: 0.3.1
   espressif/esp-dsp:
-    component_hash: 939e9c053487d6e7b7320a5cb761b2200e4b331730d6721668755ef76ab9f067
+    component_hash: 42dce32d46ac93dc11f60d368e29a830e9661c7345d794b8a45c343479cae636
     dependencies:
     - name: idf
       require: private
@@ -163,7 +173,7 @@ dependencies:
     source:
       registry_url: https://components.espressif.com
       type: service
-    version: 1.7.1
+    version: 1.7.0
   espressif/esp-modbus:
     component_hash: 5d5e90b9e55721a8a194b301ad8102d4affb647f47b74cd413ff7d1ce2c1169c
     dependencies:
@@ -181,6 +191,24 @@ dependencies:
       registry_url: https://components.espressif.com
       type: service
     version: 0.0.11
+  espressif/esp-sr:
+    component_hash: c518b25995cf03021a09442feedb7e40acba9f15d6aa903ebdfe0f657b08197b
+    dependencies:
+    - name: espressif/dl_fft
+      registry_url: https://components.espressif.com
+      require: private
+      version: '>=0.2.0'
+    - name: espressif/esp-dsp
+      registry_url: https://components.espressif.com
+      require: private
+      version: 1.7.0
+    - name: idf
+      require: private
+      version: '>=5.0'
+    source:
+      registry_url: https://components.espressif.com
+      type: service
+    version: 2.4.0
   espressif/esp-zboss-lib:
     component_hash: 321883d142421f65009972408287441794250057668a11abbdfd8bec77c3309a
     dependencies:
@@ -254,7 +282,7 @@ dependencies:
       type: service
     version: 1.2.2
   espressif/esp_modem:
-    component_hash: 07b6ca85dc0c1edbcd76f650005c74067001db138db6bfa106bf05567adca4af
+    component_hash: f4fa6dab2496af2673a68881132bb7ce0f05bf62e30e5bd0d45b607066378a28
     dependencies:
     - name: idf
       require: private
@@ -262,7 +290,7 @@ dependencies:
     source:
       registry_url: https://components.espressif.com
       type: service
-    version: 2.0.0
+    version: 2.0.1
   espressif/esp_rainmaker:
     component_hash: f6fe458fc7a0102ee2879f0247da4b41419e6c07de12031f66e5e9454d25baaa
     dependencies:
@@ -375,35 +403,8 @@ dependencies:
       registry_url: https://components.espressif.com
       type: service
     version: 1.0.3
-  espressif/lan867x:
-    component_hash: 651c9d6e3d96acb08bd5066144b12a12e94df0a6241127a0538d8680689de2c4
-    dependencies:
-    - name: espressif/lan86xx_common
-      registry_url: https://components.espressif.com
-      require: private
-      version: '*'
-    - name: idf
-      require: private
-      version: '>=5.3'
-    source:
-      registry_url: https://components.espressif.com
-      type: service
-    targets:
-    - esp32
-    - esp32p4
-    version: 2.0.0
-  espressif/lan86xx_common:
-    component_hash: 6dcb1be9f43ae940979bd628fa5bee278949b66c545e1fe2321e486102e81026
-    dependencies:
-    - name: idf
-      require: private
-      version: '>=5.2'
-    source:
-      registry_url: https://components.espressif.com
-      type: service
-    version: 1.0.0
   espressif/libsodium:
-    component_hash: ec52dce5da4af52b1908a0e23e2cf8f492edea090450e51f0d1fbe1108b6d58e
+    component_hash: b51f5836f044d8b7fbb1784257605c47ff7356f701377b005912fe6a2f12db37
     dependencies:
     - name: idf
       require: private
@@ -411,9 +412,9 @@ dependencies:
     source:
       registry_url: https://components.espressif.com
       type: service
-    version: 1.0.20~4
+    version: 1.0.21
   espressif/mdns:
-    component_hash: 7c0fa01a1cd0e72a87ec1928c3b661c0a3a9034a6d3a69dcf4850db8c6f272db
+    component_hash: 1ebe3bd675bb9d1c58f52bc0b609b32f74e572b01c328f9e61282040c775495c
     dependencies:
     - name: idf
       require: private
@@ -421,7 +422,7 @@ dependencies:
     source:
       registry_url: https://components.espressif.com
       type: service
-    version: 1.10.1
+    version: 1.11.0
   espressif/network_provisioning:
     component_hash: ef2e10182fd1861e68b821491916327c25416ca7ae70e5a6e43313dbc71fe993
     dependencies:
@@ -464,5 +465,5 @@ direct_dependencies:
 - espressif/arduino-esp32
 - idf
 manifest_hash: bb586ec925aec51fa9a2f4da694bbf1dd0c5ef5f4f654f1ef56d33c48f29627b
-target: esp32
+target: esp32s3
 version: 2.0.0

main/CMakeLists.txt

@@ -1,5 +1,7 @@
-idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp" 
+file(GLOB_RECURSE SOURCES "*.cpp")
-                    INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen)
+
+idf_component_register(SRCS "${SOURCES}"
+                    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/drone.cpp

@@ -0,0 +1,146 @@
+#include "drone.h"
+
+#include "DShotRMT.h"
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "driver/rmt_tx.h"
+#include "drone_comms.h"
+#include "drone_controller.h"
+
+#include "dshot_definitions.h"
+#include "esp32-hal.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/idf_additions.h"
+#include "freertos/projdefs.h"
+#include "freertos/task.h"
+#include "imu.h"
+#include "nav.h"
+#include "packet_handler.h"
+#include "sens_fus.h"
+#include "soc/gpio_num.h"
+#include <algorithm>
+#include <cstdint>
+#include <cstring>
+#include <optional>
+#include <stdlib.h>
+
+#define CONTROLLER_TASK_FREQUENCY 1000.0;
+
+dcont::ControllerConfig default_config() {
+  dcont::ControllerConfig config;
+
+  // 1. Configure the PID Stack
+  // Note: kp, ki, kd are arrays of 3 [roll, pitch, yaw]
+
+  // Position Loop (Position -> Velocity)
+  config.stack.position_pid = {
+      .kp = {1.0f, 1.0f, 1.0f}, // kp
+      .ki = {0.0f, 0.0f, 0.0f}, // ki
+      .kd = {0.0f, 0.0f, 0.0f}, // kd
+      .frequency = 25.0f        // frequency (Hz)
+  };
+
+  // Velocity Loop (Velocity -> Acceleration/Rotation)
+  config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f},
+                             .ki = {0.01f, 0.01f, 0.01f},
+                             .kd = {0.0f, 0.0f, 0.0f},
+                             .frequency = 50.0f};
+  // Rotation Loop (Rotation/Accel -> Angular Rate)
+  config.stack.rotation_pid = {.kp = {4.0f, 4.0f, 4.0f},
+                               .ki = {1.0f, 1.0f, 1.0f},
+                               .kd = {0.0f, 0.0f, 0.0f},
+                               .frequency = 200.0f};
+
+  // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
+  config.stack.rate_pid = {.kp = {0.1f, 0.1f, 1.0f},
+                           .ki = {0.01f, 0.01f, 0.01f},
+                           .kd = {0.001f, 0.001f, 0.001f},
+                           .frequency = 1000.0f};
+  // 2. Set Constraints
+  config.stack.max_rate = 3.14f;  // ~180 degrees/s
+  config.stack.max_linvel = 3.0f; // 10 m/s
+
+  // 3. Physical Drone Properties
+  config.mass = 0.350f;     // kg
+  config.max_thrust = 2.6f; // Newtons
+  config.max_torque = 0.5f; // Nm
+
+  float mixer[4][3] = {
+      // x, y, z
+      //
+
+      {-1.0, -1.0, -1.0}, // Rear Right
+      {-1.0, 1.0, 1.0},   // Rear Left
+      {1.0, -1.0, 1.0},   // Front Right
+      {1.0, 1.0, -1.0},   // Front Left
+  };
+
+  for (int i = 0; i < 4; i++) {
+    for (int j = 0; j < 3; j++) {
+      config.motor_map[i][j] = mixer[i][j];
+    }
+  }
+
+  return config;
+}
+
+constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
+
+void drone_controller_task(void *params) {
+  drone_cont = new drone_cont_state;
+  drone_cont->init();
+
+  while (true) {
+    drone_cont->update();
+
+    vTaskDelay(pdMS_TO_TICKS(1));
+  }
+}
+
+const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
+                                  GPIO_NUM_15};
+
+// const gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_14, GPIO_NUM_46,
+//                                   GPIO_NUM_16};
+
+DShotRMT *motors[4];
+void motor_throttles_task(void *params) {
+  motor_throttles = (float *)malloc(sizeof(float) * 4);
+
+  for (int i = 0; i < 4; i++) {
+    motor_throttles[i] = 0.02;
+    motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
+    motors[i]->begin();
+  }
+
+  // ARM
+  unsigned long armTime = millis();
+  while (millis() - armTime < 5000) {
+    for (int i = 0; i < 4; i++) {
+      motors[i]->sendThrottlePercent(0);
+    }
+    vTaskDelay(2);
+  }
+
+  unsigned long armTime_sec = millis();
+  while (millis() - armTime_sec < 1000) {
+    for (int i = 0; i < 4; i++) {
+      motors[i]->sendThrottlePercent(20 * (millis() - armTime_sec) /
+                                     armTime_sec);
+    }
+    vTaskDelay(2);
+  }
+
+  while (true) {
+    for (int i = 0; i < 4; i++) {
+      float throttle =
+          std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.3f;
+      if (atomic_load(&killswitch_active)) {
+        throttle = 0.0;
+      }
+      motors[i]->sendThrottlePercent(throttle);
+    }
+    vTaskDelay(2);
+  }
+}

main/drone.h

@@ -0,0 +1,231 @@
+#pragma once
+
+#include "esp_log.h"
+#include "nav.h"
+
+#include "esp32-hal.h"
+#include <atomic>
+#include <cmath>
+#include <cstdint>
+#include <stdatomic.h>
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+#include "drone_comms.h"
+#include "drone_controller.h"
+
+#include "packet_handler.h"
+
+#include "imu.h"
+#include "packet_handler.h"
+#include "sens_fus.h"
+
+#define CONNECTION_LOST_THRESHOLD 200
+
+#define MAX_LANDING_LINVEL 1.0
+
+void setup_drone();
+
+inline dcont::Vec3C v3f_to_v3c(Eigen::Vector3f v) { return {v[0], v[1], v[2]}; }
+
+void drone_controller_task(void *params);
+
+void motor_throttles_task(void *params);
+
+// 3     4     2     1
+inline float *motor_throttles = nullptr;
+inline std::atomic_bool killswitch_active = false;
+
+dcont::ControllerConfig default_config();
+
+struct drone_cont_state {
+  bool angvel_stablilized;
+  bool fall_vel_stabilized;
+
+  Eigen::Vector3f pos;
+  Eigen::Vector3f vel;
+  Eigen::Quaternionf rot;
+  Eigen::Vector3f angvel;
+
+  dcont::StackedController *drone_controller;
+  atomic_uint_fast8_t current_input_mode = INPUT_TYPE::ACRO;
+
+  void init() { this->drone_controller = dcont::create(default_config()); }
+
+  void drone_cont_stabilize() {
+    // 1. ANGLE VELOCITY STABILIZATION
+    // Kill the spin first to ensure sensors/control loops can work accurately.
+    if (!this->angvel_stablilized) {
+      dcont::set_input(drone_controller,
+                       dcont::Input{.joystick = {.throttle_input = 0.0,
+                                                 .roll_input = 0.0,
+                                                 .yaw_input = 0.0,
+                                                 .pitch_input = 0.0},
+                                    .acceleration = {0.0, 0.0, 0.0},
+                                    .rotation = {0.0, 0.0, 0.0},
+                                    .velocity = {0.0, 0.0, 0.0},
+                                    .position = {0.0, 0.0, 0.0},
+                                    .mode = dcont::ModeInput::Acro});
+      if (this->angvel.norm() < 1.0) {
+        this->angvel_stablilized = true;
+      }
+    }
+
+    // 2. FALL VELOCITY STABILIZATION (WITH PROP WASH AVOIDANCE)
+    // Instead of climbing straight up, we move at an angle.
+    if (!this->fall_vel_stabilized) {
+      dcont::set_input(drone_controller,
+                       dcont::Input{.joystick = {.throttle_input = 1.0,
+                                                 .roll_input = 0.0,
+                                                 .yaw_input = 0.0,
+                                                 .pitch_input = 0.0},
+                                    .acceleration = {0.0, 0.0, 0.0},
+                                    .rotation = {0.0, 0.05 * M_PI, 0.0},
+                                    .velocity = {0.0, 0.0, 0.0},
+                                    .position = {0.0, 0.0, 0.0},
+                                    .mode = dcont::ModeInput::Rotation});
+
+      if (this->vel.z() - 1.0 >= 0) {
+        this->fall_vel_stabilized = true;
+      }
+    }
+
+    dcont::set_input(drone_controller,
+                     dcont::Input{.joystick = {.throttle_input = 0.0,
+                                               .roll_input = 0.0,
+                                               .yaw_input = 0.0,
+                                               .pitch_input = 0.0},
+                                  .acceleration = {0.0, 0.0, 0.0},
+                                  .rotation = {0.0, 0.0, 0.0},
+                                  .velocity = {0.0, 0.0, 0.0},
+                                  .position = {0.0, 0.0, 0.0},
+                                  .mode = dcont::ModeInput::Velocity});
+  }
+
+  inline bool stabilization_done() {
+    return this->fall_vel_stabilized && this->angvel_stablilized;
+  }
+
+  void fetch_sens() {
+    static imu_state imu_state_local;
+    if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) {
+      imu_state_local = imu_state_var;
+      xSemaphoreGive(imu_state_mutex);
+      this->angvel = imu_state_local.angvel;
+    }
+    if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) {
+      this->pos = sens_fus.position;
+      this->vel = sens_fus.velocity;
+      xSemaphoreGive(sens_fus_mutex);
+    }
+
+    this->rot = imu_state_var.rot;
+  }
+
+  void update_controller_sens() {
+
+    dcont::set_cur_time(drone_controller, millis() / 1000.0f);
+    dcont::set_cur_angvel(drone_controller, v3f_to_v3c(this->angvel));
+    dcont::set_cur_linvel(drone_controller, v3f_to_v3c(this->vel));
+    dcont::set_cur_pos(drone_controller, v3f_to_v3c(this->pos));
+
+    dcont::set_cur_rot(drone_controller, dcont::QuatC{.i = this->rot.x(),
+                                                      .j = this->rot.y(),
+                                                      .k = this->rot.z(),
+                                                      .w = this->rot.w()});
+  }
+
+  void update_throttles() {
+    auto thrt_var = dcont::get_throttles(drone_controller);
+    auto thrt = thrt_var.values;
+    memcpy(motor_throttles, thrt, sizeof(float) * 4);
+    // ESP_LOGI("CONT", "thr: %f, %f, %f, %f", thrt[0], thrt[1], thrt[2],
+    // thrt[3]); ESP_LOGE("CONT", "UPDATE THROTTLES");
+  }
+
+  void update_input() {
+
+    packet_controller_input c;
+
+    switch (atomic_load(&this->current_input_mode)) {
+    case INPUT_TYPE::ACRO: {
+      if (xSemaphoreTake(controller_input_semaphore, 10)) {
+        // if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) {
+        //   current_controller_input = {0, 0, 0, 0};
+        // }
+        c = current_controller_input;
+
+        xSemaphoreGive(controller_input_semaphore);
+
+        dcont::set_input(
+            drone_controller,
+            dcont::Input{.joystick = {.throttle_input = c.ly,
+                                      .roll_input = c.rx,
+                                      .yaw_input = c.lx,
+                                      .pitch_input = c.ry},
+                         .acceleration = {0.0, 0.0, 0.0},
+                         .rotation = {c.ry * 3, c.rx * 3, c.lx * 3},
+                         .velocity = {0.0, 0.0, 0.0},
+                         .position = {0.0, 0.0, 0.0},
+                         .mode = dcont::ModeInput::Rotation});
+      } else {
+        drone_cont_stabilize();
+      }
+    } break;
+
+    case INPUT_TYPE::AUTO_NAV: {
+      if (!stabilization_done()) {
+        drone_cont_stabilize();
+      } else if (xSemaphoreTake(nav_mutex, 10)) {
+        waypoint wayp = nav_man.get_current_waypoint();
+        if (nav_man.current_waypoint == 8) {
+          dcont::set_max_linvel(this->drone_controller, MAX_LANDING_LINVEL);
+        }
+
+        xSemaphoreGive(nav_mutex);
+        if (wayp.coords_in_axis == std::nullopt) {
+          drone_cont_stabilize();
+        } else {
+
+          auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero());
+
+          dcont::set_input(
+              drone_controller,
+              dcont::Input{.joystick = {.throttle_input = 0.0,
+                                        .roll_input = 0.0,
+                                        .yaw_input = 0.0,
+                                        .pitch_input = 0.0},
+                           .acceleration = {0.0, 0.0, 0.0},
+                           .rotation = {0.0, 0.0, 0.0},
+                           .velocity = {0.0, 0.0, 0.0},
+                           .position = {coords.x(), coords.y(), coords.z()},
+                           .mode = dcont::ModeInput::Position});
+        }
+      } else {
+        drone_cont_stabilize();
+      }
+      break;
+    }
+    default:
+    case INPUT_TYPE::STABILIZE_FALL: {
+      drone_cont_stabilize();
+      break;
+    }
+    }
+  }
+  void update() {
+    this->fetch_sens();
+    this->update_controller_sens();
+    this->update_input();
+    this->update_throttles();
+  }
+};
+
+inline drone_cont_state *drone_cont = nullptr;

main/env_sens.cpp

@@ -1,30 +1,48 @@
+#include "env_sens.h"
+
+#include "nav.h"
+#include "sens_fus.h"
+
 #include "esp_log.h"
 #include <Adafruit_BME280.h>
 #include <SPI.h>
 #include <Wire.h>
+#include <cmath>
 
-#define SEALEVELPRESSURE_HPA (1013.25)
+#include "freertos/idf_additions.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";
+
+#define BARO_SDA GPIO_NUM_47 // SDI
+#define BARO_SCL GPIO_NUM_48
 
 namespace env_sens {
-
 void setup() {
 
-  if (!bme.begin()) {
+  baro_mutex = xSemaphoreCreateMutex();
+
+  TwoWire *wire = new TwoWire(0);
+
+  wire->begin(BARO_SDA, BARO_SCL);
+
+  if (!bme.begin(119, wire)) {
 
     ESP_LOGE(TAG, "Couldn't find a valid sensor");
+
+    ESP.restart();
     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_X1,
-                  Adafruit_BME280::FILTER_OFF,
+                  Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_NONE,
-                  Adafruit_BME280::STANDBY_MS_62_5);
+                  Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_500);
 
   bme_temp->printSensorDetails();
   bme_pressure->printSensorDetails();
@@ -32,21 +50,41 @@ void setup() {
 }
 
 float get_temperature() {
+
+  if (millis() - env_sens::time_term_read > 20) {
+
     sensors_event_t temp_event;
+    if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
+
       bme_temp->getEvent(&temp_event);
+      xSemaphoreGive(baro_mutex);
+    }
+
     return temp_event.temperature;
   }
+  return env_sens::term_read;
+}
 
 float get_pressure() {
+
+  if (millis() - env_sens::time_baro_read > 20) {
+
     sensors_event_t e;
+    if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
+
       bme_pressure->getEvent(&e);
+      xSemaphoreGive(baro_mutex);
+    }
+
     return e.pressure;
   }
+  return env_sens::baro_read;
+}
 
 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 +100,50 @@ 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();
+      if (current_alt == INFINITY) {
+        continue;
+      }
+
+      filtered_alt = (alt_lpf * current_alt) + (1.0f - alt_lpf) * filtered_alt;
+
+      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) {
+
+        // Update the filter with Baro data
+        sens_fus.measure_baro(dt, baro_pos, baro_vel);
+
+        xSemaphoreGive(sens_fus_mutex);
+      }
+
+      last_alt = filtered_alt;
+      last_time = now;
+    }
+
+    // BME280 config has a 20ms standby, so 20ms-50ms poll is ideal
+    vTaskDelay(pdMS_TO_TICKS(500));
+  }
+}
+
 } // namespace env_sens

main/env_sens.h

@@ -1,5 +1,17 @@
 #pragma once
 
+#include <cstdint>
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+#include "freertos/idf_additions.h"
 namespace env_sens {
 
 void setup();
@@ -11,5 +23,14 @@ float get_pressure();
 void dbg_sens();
 
 float get_altitude();
+void baro_poll_task(void *_);
+
+inline uint64_t time_term_read = 0;
+inline uint64_t time_baro_read = 0;
+
+inline float term_read = 0;
+inline float baro_read = 0;
 
 } // namespace env_sens
+
+inline SemaphoreHandle_t baro_mutex = NULL;

main/gps.cpp

@@ -0,0 +1,64 @@
+#include "gps.h"
+
+#include "esp32-hal.h"
+#include "esp_log.h"
+#include "freertos/idf_additions.h"
+#include "sens_fus.h"
+#include <cstdint>
+
+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.");
+
+  uint64_t last_time = millis();
+  while (true) {
+    bool available = false;
+    Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
+    std::optional<Eigen::Vector3f> local_coords;
+
+    if (xSemaphoreTake(gps_mutex, pdMS_TO_TICKS(50)) == pdTRUE) {
+      gps->poll();
+
+      if (gps->gps_avaliable()) {
+        local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
+        local_coords = gps->get_coordinates();
+
+        available = local_coords.has_value();
+      }
+      xSemaphoreGive(gps_mutex);
+
+    } else {
+      ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
+    }
+
+    if (sens_fus_mutex) {
+      if (available) {
+
+        uint64_t current_time = millis();
+        if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
+          sens_fus.measure_gps(
+              (current_time - last_time) / 1000.0f, local_coords.value(),
+              Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f));
+
+          xSemaphoreGive(sens_fus_mutex);
+          last_time = current_time;
+        } else {
+
+          ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update.");
+        }
+      } else {
+        if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) {
+          sens_fus.gps_lost();
+          xSemaphoreGive(sens_fus_mutex);
+        }
+      }
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(5)); // 10Hz polling
+  }
+}

main/gps.h

@@ -0,0 +1,136 @@
+#pragma once
+
+#include <cstdint>
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+#include "HardwareSerial.h"
+
+#include "esp_log.h"
+#include <Adafruit_GPS.h>
+#include <cmath>
+#include <ctime>
+#include <optional>
+
+const float TO_RAD = M_PI / 180.0f;
+const float KNOTS_TO_M_SEC = 0.5144444f;
+const float earth_radius = 6371000.0f;
+
+#define GPS_TX_PIN 18
+#define GPS_RX_PIN 17
+
+struct GPS {
+  Adafruit_GPS *gps;
+  float origin_lat;
+  float origin_long;
+
+  GPS() {
+
+    origin_lat = INFINITY;
+    origin_long = INFINITY;
+  }
+
+  uint64_t unix_timestamp_millis() {
+    struct tm t;
+
+    t.tm_year = (2000 + this->gps->year) - 1900;
+    t.tm_mon = this->gps->month - 1;
+    t.tm_mday = this->gps->day;
+    t.tm_hour = this->gps->hour;
+    t.tm_min = this->gps->minute;
+    t.tm_sec = this->gps->seconds;
+    t.tm_isdst = 0; // GPS is always UTC, so no Daylight Savings
+
+    // mktime converts struct tm to time_t (Unix timestamp)
+    time_t unixTime = mktime(&t);
+
+    // Calculate how many milliseconds have passed since the GPS updated its
+    // data
+    unsigned long msSinceUpdate = millis() - this->gps->lastDate;
+
+    // Return the base timestamp plus the elapsed seconds
+    return (uint64_t)unixTime * 1000 + msSinceUpdate + gps->milliseconds;
+  }
+
+  uint64_t unix_timestamp_seconds() {
+    return this->unix_timestamp_millis() / 1000;
+  }
+
+  void begin() {
+    Serial2.begin(9600, SERIAL_8N1, GPS_TX_PIN, GPS_RX_PIN);
+    this->gps = new Adafruit_GPS(&Serial2);
+    // this->gps->begin(9600);
+    this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
+
+    this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
+    this->gps->sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
+  }
+
+  bool gps_avaliable() { return this->gps->fix; }
+
+  std::optional<Eigen::Vector2f> velocity() {
+    if (!this->gps->fix) {
+      return std::nullopt;
+    }
+    float speed = this->gps->speed * KNOTS_TO_M_SEC;
+    float angle = this->gps->angle * TO_RAD;
+
+    return Eigen::Vector2f(cos(M_PI_2 - angle), sin(M_PI_2 - angle)) * speed;
+  }
+
+  std::optional<Eigen::Vector3f>
+  waypoint_to_xyz(float latitude, float longitude, float height) {
+    if (this->origin_lat == INFINITY || this->origin_long == INFINITY) {
+      if (this->gps_avaliable()) {
+        this->origin_lat = this->gps->latitudeDegrees;
+        this->origin_long = this->gps->longitudeDegrees;
+      } else {
+        return std::nullopt;
+      }
+    }
+
+    float dLat = (latitude - this->origin_lat) * TO_RAD;
+    float dLon = (longitude - this->origin_long) * TO_RAD;
+
+    float meanLat = (this->origin_lat) * TO_RAD;
+
+    float y = dLat * earth_radius;
+    float x = dLon * earth_radius * std::cos(meanLat);
+    float z = this->gps->altitude;
+
+    return Eigen::Vector3f(x, y, z);
+  }
+
+  std::optional<Eigen::Vector3f> get_coordinates() {
+    if (this->gps->fix == false) {
+      return std::nullopt;
+    }
+    float latitude = this->gps->latitudeDegrees;
+    float longitude = this->gps->longitudeDegrees;
+
+    return this->waypoint_to_xyz(latitude, longitude, this->gps->altitude);
+  }
+
+  void poll() {
+    for (int i = 0; i < 50; i++) {
+      char _ = this->gps->read();
+    }
+    if (this->gps->newNMEAreceived()) {
+      char *line = this->gps->lastNMEA();
+      // ESP_LOGI("GPS", "NMEA LINE: %s", line);
+      this->gps->parse(line);
+    }
+  }
+};
+
+inline SemaphoreHandle_t gps_mutex;
+inline GPS *gps = nullptr;
+
+void gps_poll_task(void *_);

main/imu.cpp

@@ -1,68 +1,152 @@
 #include "imu.h"
+#include "Eigen/Geometry"
+#include "Esp.h"
+#include "drone_controller.h"
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 #include "esp_log.h"
 #include "esp_timer.h"
+#include "hal/spi_types.h"
+#include "sens_fus.h"
+
 #include "freertos/idf_additions.h"
 
 static const char *TAG = "IMU";
 
-SemaphoreHandle_t imuStateMutex = NULL;
+#define IMU_CS GPIO_NUM_3   // Green
+#define IMU_MOSI GPIO_NUM_2 // DI - White
+#define IMU_RST GPIO_NUM_1  // Red
 
-BNO08x *setup_imu(imu_state *state) {
+#define IMU_INT GPIO_NUM_4  // Yellow
-  BNO08x *imu = new BNO08x();
+#define IMU_MISO GPIO_NUM_5 // SDA - White
+#define IMU_SCLK GPIO_NUM_6 // SCL - Green
 
+BNO08x *setup_imu() {
+  imu_state *local_state = new imu_state;
+  imu_state_mutex = xSemaphoreCreateMutex();
+
+  BNO08x *imu =
+      new BNO08x(bno08x_config_t(SPI2_HOST, IMU_MOSI, IMU_MISO, IMU_SCLK,
+                                 IMU_CS, IMU_INT, IMU_RST, 2000000, false));
+
+  ESP_LOGI(TAG, "INITIALIZING IMU");
   if (!imu->initialize()) {
     ESP_LOGE(TAG, "BNO08x Init failure.");
-    return nullptr;
+    ESP.restart();
-  }
-
-  if (imuStateMutex == NULL) {
-    imuStateMutex = xSemaphoreCreateMutex();
   }
+  imu->print_product_ids();
 
   imu->dynamic_calibration_autosave_enable();
   imu->dynamic_calibration_enable(BNO08xCalSel::all);
 
-  imu->rpt.rv_game.enable(2500UL);
+  // imu->rpt.rv_game.enable(2500UL);
+  imu->rpt.rv.enable(2500UL);
+  // imu->rpt.cal_magnetometer.enable(10000UL);
   imu->rpt.linear_accelerometer.enable(2500UL);
-  // imu->rpt.rv_game.tare();
+  // imu->rpt.accelerometer.enable(10000UL);
+  imu->rpt.cal_gyro.enable(2500UL);
 
-  imu->register_cb([imu, state]() {
+  imu->register_cb([imu, local_state]() {
-    if (imu->rpt.rv_game.has_new_data()) {
+    // ESP_LOGI("IMU", "CALLBACK");
-      auto sens_rot = imu->rpt.rv_game.get_quat();
+    bool needs_updating = false;
-      auto euler = imu->rpt.rv_game.get_euler();
+    if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
-      if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
+      return;
-        state->euler_ang = {euler.x, euler.y, euler.z};
+
-        state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
+    if (imu->rpt.rv.has_new_data()) {
-        xSemaphoreGive(imuStateMutex);
+
+      needs_updating = true;
+
+      auto sens_rot = imu->rpt.rv.get_quat();
+      auto sens_euler = imu->rpt.rv.get_euler();
+      local_state->rot =
+          Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k);
+
+      // ESP_LOGI("ROT", "(%f, %f, %f, %f)", sens_rot.real, sens_rot.i,
+      // sens_rot.j,
+      //          sens_rot.k);
+
+      // Eigen::Quaternionf q_global_yaw(
+      //     Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));
+      // local_state->rot = q_global_yaw * local_state->rot;
+
+      // local_state->rot.normalize();
+
+      // {.i = sens_rot.i, .j = sens_rot.j, .k = sens_rot.k, .w =
+      // sens_rot.real}; local_state->rot_euler = {sens_euler.x, sens_euler.y,
+      // sens_euler.z}; ESP_LOGI("IMU", "rot: roll %f, pitch %f, yaw %f",
+      //          local_state->rot_euler.x(), local_state->rot_euler.y(),
+      //          local_state->rot_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()) {
 
-      if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
+      needs_updating = true;
-        auto sens_accel = imu->rpt.linear_accelerometer.get();
+
-        state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
+      auto sens_accel_lin = imu->rpt.linear_accelerometer.get();
+      local_state->lin_accel = {sens_accel_lin.x, sens_accel_lin.y,
+                                sens_accel_lin.z};
+
+      // ESP_LOGI("IMU-ac", "lin_accel %f,%f,%f", sens_accel_lin.x,
+      //          sens_accel_lin.y, sens_accel_lin.z);
 
       int64_t current_time = esp_timer_get_time();
 
-        if (state->last_time != -1) {
+      if (local_state->last_time != -1) {
-          float dt = (current_time - state->last_time) / 1000000.0f;
 
-          Vec3C delta = apply_rot(&state->accel, &state->rot);
+        float dt =
+            ((float)(current_time - local_state->last_time)) / 1000000.0f;
 
-          // Trapezoidal Integration for Velocity: v = v0 + 0.5 * (a + a0) * dt
+        dcont::QuatC quatc{.i = local_state->rot.x(),
-          delta.x += state->last_accel.x;
+                           .j = local_state->rot.y(),
-          delta.y += state->last_accel.y;
+                           .k = local_state->rot.z(),
-          delta.z += state->last_accel.z;
+                           .w = local_state->rot.w()};
 
-          multiply_vec_by(&delta, dt * 0.5f);
+        dcont::Vec3C accel_global =
-          add_to_vec(&state->vel, &delta);
+            dcont::apply_rot(&local_state->lin_accel, &quatc);
 
-          state->last_accel = state->accel;
+        local_state->lin_accel_global = accel_global;
+
+        if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == 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.");
+        }
+      }
+      local_state->last_time = current_time;
     }
-        state->last_time = current_time;
 
-        xSemaphoreGive(imuStateMutex);
+    if (needs_updating) {
+      if (xSemaphoreTake(imu_state_mutex, 0)) {
+        imu_state_var = *local_state;
+        xSemaphoreGive(imu_state_mutex);
+      } else {
+        ESP_LOGE(TAG, "FAILED TO GET IMU STATE MUTEX");
       }
     }
   });

main/imu.h

@@ -1,20 +1,40 @@
 #pragma once
 
-#include "BNO08x.hpp"
-#include "BNO08xGlobalTypes.hpp"
 #include "drone_controller.h"
-#include <cmath>
+
+#include "freertos/idf_additions.h"
 #include <cstdint>
 
+#ifdef LOW
+#undef LOW
+#endif
+#ifdef HIGH
+#undef HIGH
+#endif
+
+#include "BNO08x.hpp"
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 struct imu_state {
-  Vec3C accel = {0, 0, 0};
+  dcont::Vec3C lin_accel = {0, 0, 0};
-  Vec3C last_accel = {0, 0, 0};
+  dcont::Vec3C lin_accel_global = {0, 0, 0};
-  QuatC rot = {0, 0, 0, 1};
+  // dcont::Vec3C accel = {0, 0, 0};
-  Vec3C vel = {0, 0, 0};
+  Eigen::Quaternionf rot = {0, 0, 0, 1};
   int64_t last_time = -1;
-  Vec3C euler_ang = {0, 0, 0};
+  Eigen::Vector3f angvel;
+  Eigen::Vector3f rot_euler;
 };
 
-BNO08x *setup_imu(imu_state *state);
+BNO08x *setup_imu();
 
-extern SemaphoreHandle_t imuStateMutex;
+inline SemaphoreHandle_t imu_state_mutex = NULL;
+inline imu_state imu_state_var;

main/main.cpp

@@ -1,36 +1,228 @@
-#include "esp_log.h"
+#include "Eigen/Core"
-#include "esp_timer.h"
+#include "driver/gpio.h"
-#include <cmath>
+#include "drone.h"
-#include <cstdint>
+#include "drone_comms.h"
-#include <stdio.h>
+#include "drone_controller.h"
+#include "esp32-hal.h" #include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/idf_additions.h"
+#include "freertos/projdefs.h"
+#include "freertos/task.h"
+#include <atomic>
+#include <cmath> #include <cstdint>
+#include <optional>
 
 #include "env_sens.h"
-#include "freertos/idf_additions.h"
+#include "gps.h"
 #include "imu.h"
+#include "nav.h"
+#include "packet_handler.h"
+#include "radio.h"
+#include "sens_fus.h"
+#include "servo.h"
 
-static const constexpr char *TAG = "Main";
+static const char *TAG = "MAIN";
+
+#define TIME_RELEASE_QUEUE_TO_ACTIVATION 500
 
 extern "C" void app_main(void) {
-  env_sens::setup();
 
-  imu_state imu_state;
+  sens_fus_mutex = xSemaphoreCreateMutex();
-  BNO08x *imu = setup_imu(&imu_state);
+  configASSERT(sens_fus_mutex);
-  if (imu == nullptr) {
+  nav_mutex = xSemaphoreCreateMutex();
-    ESP_LOGE(TAG, "IMU setup failed.");
+
-    return;
+  initArduino();
+  gpio_install_isr_service(0);
+  Serial.begin(115200);
+
+  xTaskCreatePinnedToCore(radio_task,   // Function name
+                          "radio_rxtx", // Name for debugging
+                          4096,         // Stack size in bytes
+                          NULL,         // Parameters
+                          5,            // Priority (higher = more urgent)
+                          NULL,         // Task handle
+                          0             // Core ID
+  );
+
+  xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5,
+                          NULL, 0);
+
+  xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0);
+
+  xTaskCreatePinnedToCore(drone_controller_task,   // Function name
+                          "drone_controller_task", // Name for debugging
+                          1024 * 32,               // Stack size in bytes
+                          NULL,                    // Parameters
+                          20,   // Priority (higher = more urgent)
+                          NULL, // Task handle
+                          1     // Core ID
+  );
+
+  xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
+                          "motor_throttles_task", // Name for debugging
+                          1024 * 4,               // Stack size in bytes
+                          NULL,                   // Parameters
+                          24,   // Priority (higher = more urgent)
+                          NULL, // Task handle
+                          1     // Core ID
+  );
+
+  // vTaskDelay(5000);
+  // for (int i = 0; i < 4; i++) {
+  //   motor_throttles[i] = 10.0;
+  //   vTaskDelay(2000);
+  //   motor_throttles[i] = 0.0;
+  // }
+
+  xTaskCreate(
+      [](void *pvParameters) {
+        while (true) {
+          while (packet_rx_queue &&
+                 xQueueReceive(packet_rx_queue, &packet_data[0], 20)) {
+            handle_packet(&packet_data[0]);
+          }
+          vTaskDelay(1);
+        }
+      },
+      "lambda_recv_task", 8192, NULL, 5, NULL);
+
+  setup_imu();
+  servo_init();
+  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;
+
+  uint64_t last_print_time = 0;
+  uint64_t last_position_broadcast_time = 0;
+  uint64_t last_status_broadcast_time = 0;
+  uint64_t time_activation_queue = 0;
+  bool released = false;
+
+  while (true) {
+
+    if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
+      send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
+      last_position_broadcast_time = millis();
+    }
+    // auto vel = sens_fus.velocity;
+    // ESP_LOGI("GPSvsIMU", "gps_vel: %f, imu_vel: %f",
+    //          gps->velocity().value().norm(),
+    //          Eigen::Vector3f(vel.x(), vel.y(), 0.0).norm());
+
+    if (millis() > last_status_broadcast_time + 500 && packet_tx_queue) {
+      send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS);
+      last_status_broadcast_time = millis();
     }
 
-  ESP_LOGE(TAG, "IMU setup sucess.");
+    if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) {
 
-  while (1) {
+      if (gps->gps_avaliable()) {
-    vTaskDelay(pdMS_TO_TICKS(300));
+        waypoint current_wayp = nav_man.get_current_waypoint();
-    if (xSemaphoreTake(imuStateMutex, pdMS_TO_TICKS(5)) == pdTRUE) {
+        auto pos = sens_fus.position;
-      ESP_LOGI(TAG, "accel - %f, %f, %f", imu_state.accel.x, imu_state.accel.y,
+
-               imu_state.accel.z);
+        if ((current_wayp.coords_in_axis.value_or(
-      ESP_LOGI(TAG, "; euler - %f, %f, %f\n", imu_state.euler_ang.x,
+                 Eigen::Vector3f(INFINITY, INFINITY, INFINITY)) -
-               imu_state.euler_ang.y, imu_state.euler_ang.z);
+             pos)
-      xSemaphoreGive(imuStateMutex);
+                .norm() < 1.0) {
+          nav_man.waypoint_reached();
+        }
+      }
+
+      xSemaphoreGive(nav_mutex);
+    }
+
+    // Release
+    if (imu_state_var.lin_accel_global.z < -8.0 && !released) {
+      released = true;
+      time_activation_queue = millis();
+    }
+
+    if (released && drone_cont &&
+        std::atomic_load(&drone_cont->current_input_mode) ==
+            INPUT_TYPE::AUTO_NAV &&
+        millis() - time_activation_queue > TIME_RELEASE_QUEUE_TO_ACTIVATION) {
+
+      dcont::reset_pid_states(drone_cont->drone_controller);
+      servo_set(SERVO_OPTIONS::UP);
+
+      xTaskCreate(
+          [](void *pvParameters) {
+            vTaskDelay(pdMS_TO_TICKS(700));
+            std::atomic_store(&killswitch_active, false);
+          },
+          "lambda_task", 2048, NULL, 5, NULL);
+    }
+
+    // Logging
+    if (millis() > last_print_time + 1000) {
+      last_print_time = millis();
+
+      std::optional<Eigen::Vector3f> coords;
+      float lat, lon, alt;
+      bool gps_values = false;
+      bool fix = false;
+      uint8_t sat_count = 0;
+      if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
+        if (gps->gps_avaliable()) {
+          coords = gps->get_coordinates();
+          lat = gps->gps->latitudeDegrees;
+          lon = gps->gps->longitudeDegrees;
+          alt = gps->gps->altitude;
+          gps_values = true;
+        }
+        sat_count = gps->gps->satellites;
+        fix = gps->gps->fix;
+        xSemaphoreGive(gps_mutex);
+
+        if (gps_values) {
+
+          ESP_LOGI(TAG,
+                   "loc -> lat: %f, long: %f, height: %f, sat_c: %d, fix: %b",
+                   lat, lon, alt, sat_count, fix);
+        }
+
+        if (coords.has_value()) {
+          auto D_pos = coords.value();
+
+          ESP_LOGI(TAG, "    -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
+                   D_pos[2]);
+        }
       }
       env_sens::dbg_sens();
+
+      if (sens_fus_mutex &&
+          xSemaphoreTake(sens_fus_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
+        local_pos = sens_fus.position;
+        local_vel = sens_fus.velocity;
+        nav_data_ready = true;
+        xSemaphoreGive(sens_fus_mutex);
+      }
+
+      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]);
+      }
+
+      if (motor_throttles != nullptr) {
+
+        ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0],
+                 motor_throttles[1], motor_throttles[2], motor_throttles[3]);
+      }
+
+      if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) {
+
+        ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)",
+                 current_controller_input.lx, current_controller_input.ly,
+                 current_controller_input.rx, current_controller_input.ry);
+      }
+      // ESP_LOGI(TAG, "ROT: (%f, %f, %f)", imu_state_var.rot_euler.x(),
+      //          imu_state_var.rot_euler.y(), imu_state_var.rot_euler.z());
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(5));
   }
 }

main/nav.h

@@ -0,0 +1,74 @@
+#pragma once
+
+#include "drone_controller.h"
+
+#include "freertos/idf_additions.h"
+#include "gps.h"
+#include <cmath>
+#include <cstdint>
+#include <optional>
+
+#define WAYPOINT_COUNT 8
+
+struct waypoint {
+  Eigen::Vector3f coords; // lat, lon, alt
+  std::optional<Eigen::Vector3f> coords_in_axis = std::nullopt;
+  bool active = false; // active or to be skipped
+  bool landing = false;
+};
+
+struct drone_nav {
+  waypoint waypoints[WAYPOINT_COUNT + 1];
+  uint8_t current_waypoint;
+
+  void set_active_mask(uint8_t mask) {
+    for (int i = 0; i < WAYPOINT_COUNT; i++) {
+      this->waypoints[i].active = (mask & (1 << i)) != 0;
+    }
+  }
+
+  uint8_t get_active_mask() {
+    uint8_t mask = 0;
+    for (int i = 0; i < WAYPOINT_COUNT; i++) {
+      if (waypoints[i].active) {
+        mask |= (1 << i);
+      }
+    }
+    return mask;
+  }
+
+  waypoint get_current_waypoint() {
+    waypoint wayp = this->waypoints[this->current_waypoint];
+    if (!wayp.coords_in_axis.has_value()) {
+      auto axis =
+          gps->waypoint_to_xyz(wayp.coords[0], wayp.coords[1], wayp.coords[2]);
+      wayp.coords_in_axis = axis;
+    }
+    return wayp;
+  }
+
+  void waypoint_reached() {
+    waypoint wayp = waypoints[this->current_waypoint];
+    if (wayp.landing) {
+      this->waypoints[8] = waypoint{
+          .coords = Eigen::Vector3f(wayp.coords.x(), wayp.coords.y(), 0.0f),
+          .coords_in_axis = std::nullopt};
+      current_waypoint = 8;
+
+      return;
+    }
+
+    int i = this->current_waypoint + 1;
+    while (i != this->current_waypoint) {
+      bool active = waypoints[i].active;
+      if (active) {
+        this->current_waypoint = i;
+      }
+      i++;
+      i = i % 8;
+    }
+  }
+};
+
+inline SemaphoreHandle_t nav_mutex = NULL;
+inline drone_nav nav_man;

main/packet_handler.cpp

@@ -0,0 +1,177 @@
+#include "packet_handler.h"
+
+#include "drone.h"
+#include "drone_comms.h"
+#include "drone_controller.h"
+#include "env_sens.h"
+#include "esp32-hal.h"
+#include "esp_log.h"
+#include "freertos/idf_additions.h"
+#include "gps.h"
+#include "imu.h"
+#include "nav.h"
+#include "portmacro.h"
+#include "radio.h"
+#include "sens_fus.h"
+#include <atomic>
+#include <cstdint>
+#include <sys/unistd.h>
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+void handle_waypoint_update(uint8_t *packet_addr, uint8_t index);
+void handle_nav_update(uint8_t *packet_addr);
+
+void handle_packet(uint8_t *packet_addr) {
+  PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr);
+  if (packet_type == PACKET_TYPE::COMMAND_REQUEST) {
+    packet_command_request *packet =
+        (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
+    send_packet_getter(packet->packet_requested);
+  }
+
+  // NAV SETTERS
+  else if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
+           packet_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
+    uint8_t index = packet_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
+    handle_waypoint_update(packet_addr, index);
+
+  } else if (packet_type == PACKET_TYPE::DRONE_NAV) {
+    handle_nav_update(packet_addr);
+
+  } else if (packet_type == PACKET_TYPE::KILLSWITCH_TOGGLE) {
+    packet_killswitch_toggle *packet =
+        (packet_killswitch_toggle *)(packet_addr + sizeof(PACKET_TYPE));
+    std::atomic_store(&killswitch_active, packet->killswitch_active);
+    ESP_LOGI("RADIO_RX", "KILLSWITCH PAACKET");
+    ESP_LOGE("switch_set", "killswitch set to: %d, packet: %d",
+             atomic_load(&killswitch_active), packet->killswitch_active);
+
+  } else if (packet_type == PACKET_TYPE::MODE_INPUT) {
+    packet_mode_input *packet =
+        (packet_mode_input *)(packet_addr + sizeof(PACKET_TYPE));
+    atomic_store(&drone_cont->current_input_mode, packet->input_type);
+
+  } else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) {
+    packet_controller_input *packet =
+        (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
+
+    if (xSemaphoreTake(controller_input_semaphore, 10)) {
+      current_controller_input = *packet;
+      time_last_controller = millis();
+      xSemaphoreGive(controller_input_semaphore);
+    }
+  }
+}
+
+void handle_waypoint_update(uint8_t *packet_addr, uint8_t index) {
+  packet_drone_nav_waypoint *packet =
+      (packet_drone_nav_waypoint *)(packet_addr + sizeof(PACKET_TYPE));
+  float lon, lat, alt;
+  lon = packet->coord[0];
+  lat = packet->coord[1];
+  alt = packet->coord[2];
+  Eigen::Vector3f coords(lon, lat, alt);
+
+  if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+    nav_man.waypoints[index].coords = coords;
+    xSemaphoreGive(nav_mutex);
+  }
+}
+
+void handle_nav_update(uint8_t *packet_addr) {
+  packet_drone_nav *packet =
+      (packet_drone_nav *)(packet_addr + sizeof(PACKET_TYPE));
+
+  if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+    nav_man.set_active_mask(packet->active_mask);
+    nav_man.current_waypoint = packet->current_waypoint;
+  }
+}
+
+void send_packet_getter(PACKET_TYPE requested_type) {
+
+  std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
+
+  if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
+
+    imu_state state;
+    if (xSemaphoreTake(imu_state_mutex, 20)) {
+      state = imu_state_var;
+      xSemaphoreGive(imu_state_mutex);
+    }
+    Eigen::Vector3f local_vel = state.rot.inverse() * sens_fus.velocity;
+
+    ESP_LOGE("Radio tx", "[f,%f,%f,%f]", state.rot.w(), state.rot.x(),
+             state.rot.y(), state.rot.z());
+    resp_packet = create_packet_pooled(
+        PACKET_TYPE::INFO_DRONE_POSITION,
+        packet_info_drone_position{
+            .trans = {sens_fus.position.x(), sens_fus.position.y(),
+                      sens_fus.position.z()},
+            .vel = {local_vel.x(), local_vel.y(), local_vel.z()},
+            .rot = {state.rot.w(), state.rot.x(), state.rot.y(), state.rot.z()},
+        });
+  }
+
+  if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
+    if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY)) {
+      resp_packet = create_packet_pooled(
+          PACKET_TYPE::INFO_DRONE_STATUS,
+          packet_info_drone_status{
+              .origin = {gps->origin_long, gps->origin_lat},
+              .time_since_boot = millis(),
+              .unix_timestamp_millis = gps->unix_timestamp_millis(),
+              .gps_fix = gps->gps_avaliable()});
+      xSemaphoreGive(gps_mutex);
+    }
+  }
+
+  // Navigation
+  if (requested_type == PACKET_TYPE::DRONE_NAV) {
+    uint8_t active_mask, current;
+    if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+      active_mask = nav_man.get_active_mask();
+      current = nav_man.current_waypoint;
+      xSemaphoreGive(nav_mutex);
+
+      resp_packet = create_packet_pooled(
+          PACKET_TYPE::DRONE_NAV, packet_drone_nav{active_mask, current});
+    }
+  }
+
+  if (requested_type == PACKET_TYPE::KILLSWITCH_TOGGLE) {
+    bool value = atomic_load(&killswitch_active);
+    resp_packet = create_packet_pooled(PACKET_TYPE::KILLSWITCH_TOGGLE,
+                                       packet_killswitch_toggle{value});
+  }
+
+  if (requested_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 &&
+      requested_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) {
+    uint8_t index = requested_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
+
+    Eigen::Vector3f coords;
+    bool land = false;
+    if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
+      coords = nav_man.waypoints[index].coords;
+      land = nav_man.waypoints[index].landing;
+      xSemaphoreGive(nav_mutex);
+    }
+
+    resp_packet = create_packet_pooled(
+        requested_type,
+        packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}, land});
+  }
+
+  if (resp_packet.first != nullptr) {
+    xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
+  }
+}

main/packet_handler.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <cstdint>
+
+#include "drone_comms.h"
+#include "esp32-hal.h"
+#include "freertos/idf_additions.h"
+
+void handle_packet(uint8_t *packet_addr);
+
+void send_packet_getter(PACKET_TYPE requested_type);
+
+inline SemaphoreHandle_t controller_input_semaphore = NULL;
+inline packet_controller_input current_controller_input;
+
+inline uint64_t time_last_controller = 0;

main/radio.cpp

@@ -0,0 +1,145 @@
+#include "radio.h"
+
+#include "Esp.h"
+#include "esp32-hal-gpio.h"
+#include "esp32-hal-spi.h"
+#include "esp_log.h"
+#include "freertos/idf_additions.h"
+#include "packet_handler.h"
+#include <RFM69.h>
+#include <SPI.h>
+#include <cstdint>
+#include <cstring>
+
+#include <drone_comms.h>
+
+// Right to left on hardware.
+
+#define RFM69_INT GPIO_NUM_8 // DI0
+#define RFM69_CS GPIO_NUM_9  // NSS
+#define RFM69_RST GPIO_NUM_10
+
+#define RFM69_SCLK GPIO_NUM_11 // SCK
+#define RFM69_MOSI GPIO_NUM_12
+#define RFM69_MISO GPIO_NUM_13
+
+#define FREQUENCY RF69_433MHZ
+#define NODEID 1
+#define GROUNDID 99
+#define NETWORKID 100
+
+static const char *TAG = "RADIO_TASK";
+
+#define PACKET_QUEUE_CAP 10
+
+static bool is_pending_switch = false;
+static uint32_t target_bitrate = 0;
+static uint32_t switch_at_ms = 0;
+static uint32_t rollback_at_ms = 0;
+static bool confirmed_at_new_rate = false;
+
+void radio_task(void *pvParameters) {
+  ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID());
+  packet_rx_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
+  packet_tx_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
+  controller_input_semaphore = xSemaphoreCreateMutex();
+
+  pinMode(RFM69_CS, OUTPUT);
+  pinMode(RFM69_INT, INPUT);
+
+  SPIClass hspi(HSPI);
+  hspi.begin(RFM69_SCLK, RFM69_MISO, RFM69_MOSI, RFM69_CS);
+
+  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, &hspi);
+
+  if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) {
+    radio.setPowerLevel(31);
+    radio.setHighPower(true);
+    // radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
+    ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
+    radio.readAllRegsCompact();
+
+  } else {
+    ESP_LOGE(TAG, "Radio Init FAILED! Restarting.");
+    ESP.restart();
+  }
+
+  while (1) {
+    uint32_t now = millis();
+
+    if (radio.receiveDone()) {
+
+      // If we receive ANY valid packet while in probation, confirm the switch
+      // (Bit-rate switching)
+      if (is_pending_switch && now > switch_at_ms) {
+        confirmed_at_new_rate = true;
+        ESP_LOGI(TAG, "New bitrate confirmed by valid packet.");
+      }
+
+      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 == COMMAND_CHANGE_DATARATE) {
+        packet_command_datarate *cmd =
+            (packet_command_datarate *)(&packet_data[0] + sizeof(PACKET_TYPE));
+        target_bitrate = cmd->target_bitrate;
+
+        switch_at_ms = now + cmd->ms_delay;
+        rollback_at_ms = now + cmd->ms_rollback;
+        is_pending_switch = true;
+        confirmed_at_new_rate = false;
+
+        ESP_LOGI(TAG, "Datarate change requested: %d. Switching in 100ms...",
+                 target_bitrate);
+      } else {
+        ESP_LOGI(TAG, "RECVD PACKET");
+        xQueueSend(packet_rx_queue, &packet_data[0], portMAX_DELAY);
+      }
+
+      if (radio.ACKRequested()) {
+        radio.sendACK();
+      }
+    }
+
+    // Send packets that were queued up for sending
+    if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
+      PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]);
+      radio.send(GROUNDID, &packet_data[0], get_packet_size(packet_type));
+    }
+
+    // --- STATE MACHINE FOR BITRATE SWITCHING ---
+
+    // 1. Execute the Switch
+    if (is_pending_switch && now >= switch_at_ms && !confirmed_at_new_rate) {
+      // We only want to trigger the register write once
+      radio.setCustomBitrate(target_bitrate);
+      switch_at_ms = 0xFFFFFFFF; // Prevent re-triggering
+    }
+
+    // 2. The Rollback (The Fail-safe)
+    if (is_pending_switch && !confirmed_at_new_rate && now > rollback_at_ms) {
+      ESP_LOGE(TAG,
+               "ROLLBACK: No confirmation at new rate. Reverting to default.");
+      radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
+      is_pending_switch = false;
+    }
+
+    // 3. Clear pending flag once confirmed
+    if (confirmed_at_new_rate) {
+      is_pending_switch = false;
+      confirmed_at_new_rate = false;
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(1));
+  }
+}

main/radio.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include "drone_comms.h"
+#include "freertos/idf_additions.h"
+
+inline QueueHandle_t packet_rx_queue = NULL;
+inline QueueHandle_t packet_tx_queue = NULL;
+
+void radio_task(void *pvParameters);

main/sens_fus.h

@@ -0,0 +1,110 @@
+#pragma once
+#include "Eigen/Core"
+#include <cmath>
+#include <cstdlib>
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#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());
+  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 sens_fus_compl {
+  Eigen::Vector3f position = Eigen::Vector3f::Zero();
+  Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
+  Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
+
+  Eigen::Vector3f velocity_error = Eigen::Vector3f::Zero();
+  Eigen::Vector3f position_error = Eigen::Vector3f::Zero();
+
+  void gps_lost() {
+    this->position_error = Eigen::Vector3f::Zero();
+    this->velocity_error = Eigen::Vector3f::Zero();
+  }
+
+  Eigen::Vector3f velocity_error_mult = {0.1f, 0.1f, 0.0f};
+  Eigen::Vector3f position_error_mult = {0.1f, 0.1f, 0.0f};
+
+  /*
+   * Tau is the time that the filter takes to reach 1-e^(-1) of the difference
+   * to a constant target
+   * so at t=tau, were 63% of the way there
+   * at t=3*tau, were 95% of the way there
+   */
+  Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, 10.0};
+  Eigen::Vector3f tau_gps_vel = {4.0f, 4.0f, INFINITY};
+
+  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, INFINITY};
+  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 4.0f};
+
+  Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
+
+  void predict(float dt, Eigen::Vector3f accel) {
+    Eigen::Vector3f accel_err_rmvd =
+        accel.array() -
+        (this->velocity_error.array() * this->velocity_error_mult.array()) / dt;
+
+    Eigen::Vector3f next_velocity =
+        this->velocity + (this->last_accel_world + accel_err_rmvd) * 0.5f * dt;
+
+    this->position = this->position.array() +
+                     (((this->velocity + next_velocity) * 0.5f).array() -
+                      (this->position_error.array() *
+                       this->position_error_mult.array() / dt)) *
+                         dt;
+
+    this->velocity = next_velocity;
+    this->last_accel_world = accel_err_rmvd;
+  }
+
+  void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
+    // alpha = dt / (tau + dt)
+    Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
+    Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
+
+    this->velocity_error = this->velocity - gps_vel;
+    this->position_error = this->position - gps_pos;
+
+    // next_state = (1 - alpha) * state + alpha * measurement
+    this->position =
+        (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
+        alpha_pos.array() * gps_pos.array();
+
+    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) {
+    // 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 sens_fus_mutex = NULL;
+inline sens_fus_compl sens_fus;

main/servo.cpp

@@ -0,0 +1,52 @@
+#include "servo.h"
+
+#include "driver/ledc.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#define SERVO_PIN 7
+#define LEDC_TIMER LEDC_TIMER_0
+#define LEDC_MODE LEDC_LOW_SPEED_MODE
+#define LEDC_CHANNEL LEDC_CHANNEL_0
+#define LEDC_DUTY_RES LEDC_TIMER_13_BIT
+#define LEDC_FREQUENCY 50
+
+uint32_t us_to_duty(int us) {
+  // 20000 is the period in microseconds (50Hz)
+  // 8191 is the max duty cycle for 13-bit resolution
+  return (uint32_t)((float)us / 20000.0f * 8191.0f);
+}
+
+ledc_timer_config_t ledc_timer = {.speed_mode = LEDC_MODE,
+                                  .duty_resolution = LEDC_DUTY_RES,
+                                  .timer_num = LEDC_TIMER,
+                                  .freq_hz = LEDC_FREQUENCY,
+                                  .clk_cfg = LEDC_AUTO_CLK};
+
+ledc_channel_config_t ledc_channel = {.gpio_num = SERVO_PIN,
+                                      .speed_mode = LEDC_MODE,
+                                      .channel = LEDC_CHANNEL,
+                                      .timer_sel = LEDC_TIMER,
+                                      .duty = 0};
+void servo_init() {
+  ledc_timer_config(&ledc_timer);
+  ledc_channel_config(&ledc_channel);
+}
+
+#define DUTY_CYCLE_DOWN 2000
+#define DUTY_CYCLE_UP 850
+
+void servo_set(SERVO_OPTIONS opt) {
+  switch (opt) {
+  case UP:
+    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, us_to_duty(DUTY_CYCLE_UP));
+    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
+    break;
+  case DOWN:
+    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, us_to_duty(DUTY_CYCLE_DOWN));
+    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
+  case OFF:
+    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, 0);
+    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
+  }
+}

main/servo.h

@@ -0,0 +1,11 @@
+#pragma once
+
+enum SERVO_OPTIONS {
+  UP,
+  DOWN,
+  OFF,
+};
+
+void servo_init();
+
+void servo_set(SERVO_OPTIONS opt);

sdkconfig.defaults

@@ -0,0 +1,2 @@
+
+CONFIG_FREERTOS_HZ=1000