test rollback after

components/DShotRMT

@@ -1 +1 @@
-Subproject commit 262feec75cbc8b6de3b776b025b85fa534685e71
+Subproject commit 86d682313234556720dc63ad53aace47ab3d190c

components/RFM69_LowPowerLab

@@ -1 +1 @@
-Subproject commit 034caf2f11a15c67a8a1985b9a02ac3d5026671b
+Subproject commit 9b083db90723c4911ac79edb6c1aa35c4c127cb8

components/drone_comms

@@ -1 +1 @@
-Subproject commit d7debd6a59298850dd7371a90ed331175b64ab12
+Subproject commit 24803f37ed9a5824682f7e83535c43392db21c20

components/drone_controller

@@ -1 +1 @@
-Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502
+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/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,11 +1,15 @@
 #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>
 
 #include "freertos/idf_additions.h"
-#include "nav.h"
 
 #define SEALEVELPRESSURE_HPA (1030)
 
@@ -16,20 +20,29 @@ Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
 
 static const constexpr char *TAG = "BARO";
 
+#define BARO_SDA GPIO_NUM_47 // SDI
+#define BARO_SCL GPIO_NUM_48
+
 namespace env_sens {
 void setup() {
 
   baro_mutex = xSemaphoreCreateMutex();
 
-  if (!bme.begin()) {
+  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;
   }
   ESP_LOGI(TAG, "BARO SETUP COMPLETE.");
-  bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X8,
-                  Adafruit_BME280::SAMPLING_X8, Adafruit_BME280::SAMPLING_NONE,
-                  Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_20);
+  bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X1,
+                  Adafruit_BME280::SAMPLING_X1, Adafruit_BME280::SAMPLING_NONE,
+                  Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_500);
 
   bme_temp->printSensorDetails();
   bme_pressure->printSensorDetails();
@@ -37,18 +50,35 @@ void setup() {
 }
 
 float get_temperature() {
-  sensors_event_t temp_event;
 
-  bme_temp->getEvent(&temp_event);
-  return temp_event.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() {
-  sensors_event_t e;
 
-  bme_pressure->getEvent(&e);
+  if (millis() - env_sens::time_baro_read > 20) {
 
-  return e.pressure;
+    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,
@@ -85,31 +115,34 @@ void baro_poll_task(void *_) {
 
     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;
 
-      if (nav_mutex && xSemaphoreTake(nav_mutex, (TickType_t)20) == pdTRUE) {
+      Eigen::Vector3f baro_pos = sens_fus.position;
+      baro_pos.z() = filtered_alt;
 
-        Eigen::Vector3f baro_pos = nav_filter.position;
-        baro_pos.z() = filtered_alt;
-
-        Eigen::Vector3f baro_vel = nav_filter.velocity;
-        baro_vel.z() = v_z;
+      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
-        nav_filter.measure_baro(dt, baro_pos, baro_vel);
+        sens_fus.measure_baro(dt, baro_pos, baro_vel);
 
-        xSemaphoreGive(nav_mutex);
+        xSemaphoreGive(sens_fus_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(50));
+    // BME280 config has a 20ms standby, so 20ms-50ms poll is ideal
+    vTaskDelay(pdMS_TO_TICKS(500));
   }
 }
 

main/env_sens.h

@@ -1,5 +1,16 @@
 #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 {
 
@@ -14,6 +25,12 @@ 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

@@ -1,36 +1,64 @@
 #include "gps.h"
+
+#include "esp32-hal.h"
 #include "esp_log.h"
-#include "nav.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) {
-    if (xSemaphoreTake(gps_mutex, (TickType_t)1) == pdTRUE) {
+    bool available = false;
+    Eigen::Vector2f local_vel = Eigen::Vector2f::Zero();
+    std::optional<Eigen::Vector3f> local_coords;
 
-      // ESP_LOGI(TAG, "Polling gps.");
+    if (xSemaphoreTake(gps_mutex, pdMS_TO_TICKS(50)) == pdTRUE) {
       gps->poll();
-      if (gps->gps_avaliable() && 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);
+
+      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");
     }
-    vTaskDelay(pdMS_TO_TICKS(100));
+
+    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

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cstdint>
 #ifdef PS
 #undef PS
 #endif
@@ -15,13 +16,16 @@
 #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;
@@ -33,9 +37,36 @@ struct GPS {
     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->begin(9600);
     this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
 
     this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
@@ -57,7 +88,12 @@ struct GPS {
   std::optional<Eigen::Vector3f>
   waypoint_to_xyz(float latitude, float longitude, float height) {
     if (this->origin_lat == INFINITY || this->origin_long == INFINITY) {
-      return std::nullopt;
+      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;
@@ -73,8 +109,7 @@ struct GPS {
   }
 
   std::optional<Eigen::Vector3f> get_coordinates() {
-    if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 ||
-        this->gps->longitudeDegrees != 0.0) {
+    if (this->gps->fix == false) {
       return std::nullopt;
     }
     float latitude = this->gps->latitudeDegrees;
@@ -84,14 +119,13 @@ struct GPS {
   }
 
   void poll() {
-    while (this->gps->read()) {
-      if (this->gps->newNMEAreceived()) {
-        char *line = this->gps->lastNMEA();
-        // ESP_LOGI("GPS", "NMEA LINE: %s", line);
-        if (!this->gps->parse(line)) {
-          continue;
-        }
-      }
+    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);
     }
   }
 };

main/imu.cpp

@@ -1,9 +1,7 @@
 #include "imu.h"
-#include "esp_log.h"
-#include "esp_timer.h"
-#include "freertos/idf_additions.h"
-#include "hal/spi_types.h"
-#include "nav.h"
+#include "Eigen/Geometry"
+#include "Esp.h"
+#include "drone_controller.h"
 
 #ifdef PS
 #undef PS
@@ -15,50 +13,141 @@
 
 #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";
 
-BNO08x *setup_imu(imu_state *state) {
-  BNO08x *imu = new BNO08x(
-      bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25,
-                      GPIO_NUM_33, GPIO_NUM_36, GPIO_NUM_32, 2000000, false));
+#define IMU_CS GPIO_NUM_3   // Green
+#define IMU_MOSI GPIO_NUM_2 // DI - White
+#define IMU_RST GPIO_NUM_1  // Red
 
+#define IMU_INT GPIO_NUM_4  // Yellow
+#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();
   }
+  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.accelerometer.enable(10000UL);
+  imu->rpt.cal_gyro.enable(2500UL);
 
-  imu->register_cb([imu, state]() {
-    if (imu->rpt.rv_game.has_new_data()) {
-      auto sens_rot = imu->rpt.rv_game.get_quat();
-      state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
+  imu->register_cb([imu, local_state]() {
+    // ESP_LOGI("IMU", "CALLBACK");
+    bool needs_updating = false;
+    if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
+      return;
+
+    if (imu->rpt.rv.has_new_data()) {
+
+      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()) {
 
-      auto sens_accel = imu->rpt.linear_accelerometer.get();
-      state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
+      needs_updating = true;
+
+      auto sens_accel_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) {
-        float dt = ((float)(current_time - state->last_time)) / 1000000.0f;
-        Vec3C accel_global = apply_rot(&state->accel, &state->rot);
+      if (local_state->last_time != -1) {
 
-        if (xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
-          nav_filter.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
-                                                 accel_global.z));
-          xSemaphoreGive(nav_mutex);
+        float dt =
+            ((float)(current_time - local_state->last_time)) / 1000000.0f;
+
+        dcont::QuatC quatc{.i = local_state->rot.x(),
+                           .j = local_state->rot.y(),
+                           .k = local_state->rot.z(),
+                           .w = local_state->rot.w()};
+
+        dcont::Vec3C accel_global =
+            dcont::apply_rot(&local_state->lin_accel, &quatc);
+
+        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 mutex.");
+          ESP_LOGE(TAG, "Failed to get sens_fus mutex.");
         }
       }
-      state->last_time = current_time;
+      local_state->last_time = current_time;
+    }
+
+    if (needs_updating) {
+      if (xSemaphoreTake(imu_state_mutex, 0)) {
+        imu_state_var = *local_state;
+        xSemaphoreGive(imu_state_mutex);
+      } else {
+        ESP_LOGE(TAG, "FAILED TO GET IMU STATE MUTEX");
+      }
     }
   });
 

main/imu.h

@@ -1,6 +1,8 @@
 #pragma once
 
 #include "drone_controller.h"
+
+#include "freertos/idf_additions.h"
 #include <cstdint>
 
 #ifdef LOW
@@ -12,10 +14,27 @@
 
 #include "BNO08x.hpp"
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 struct imu_state {
-  Vec3C accel = {0, 0, 0};
-  QuatC rot = {0, 0, 0, 1};
+  dcont::Vec3C lin_accel = {0, 0, 0};
+  dcont::Vec3C lin_accel_global = {0, 0, 0};
+  // dcont::Vec3C accel = {0, 0, 0};
+  Eigen::Quaternionf rot = {0, 0, 0, 1};
   int64_t last_time = -1;
+  Eigen::Vector3f angvel;
+  Eigen::Vector3f rot_euler;
 };
 
-BNO08x *setup_imu(imu_state *state);
+BNO08x *setup_imu();
+
+inline SemaphoreHandle_t imu_state_mutex = NULL;
+inline imu_state imu_state_var;

main/main.cpp

@@ -1,131 +1,228 @@
-
+#include "Eigen/Core"
 #include "driver/gpio.h"
+#include "drone.h"
 #include "drone_comms.h"
-#include "esp32-hal.h"
-#include "esp_log.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 <Arduino.h>
-#include <cstddef>
-#include <cstdint>
+#include <atomic>
+#include <cmath> #include <cstdint>
+#include <optional>
 
 #include "env_sens.h"
 #include "gps.h"
-#include "nav.h"
-#include "radio.h"
-
 #include "imu.h"
+#include "nav.h"
+#include "packet_handler.h"
+#include "radio.h"
+#include "sens_fus.h"
+#include "servo.h"
 
 static const char *TAG = "MAIN";
 
-void handle_packet(uint8_t *packet_addr);
+#define TIME_RELEASE_QUEUE_TO_ACTIVATION 500
 
 extern "C" void app_main(void) {
 
+  sens_fus_mutex = xSemaphoreCreateMutex();
+  configASSERT(sens_fus_mutex);
   nav_mutex = xSemaphoreCreateMutex();
+
   initArduino();
   gpio_install_isr_service(0);
   Serial.begin(115200);
 
-  static imu_state imu_state;
-  auto _ = setup_imu(&imu_state);
-
   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
-                          1             // Core ID
+                          0             // Core ID
   );
 
-  xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
+  xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5,
+                          NULL, 0);
 
-  xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
+  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 (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
-      handle_packet(&packet_data[0]);
+
+    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();
     }
 
-    if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
-      if (gps->gps_avaliable()) {
-        ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
-                 gps->gps->latitudeDegrees, gps->gps->longitudeDegrees,
-                 gps->gps->altitude);
+    if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) {
 
-        auto D_pos_cond = gps->get_coordinates();
-        if (D_pos_cond.has_value()) {
-          auto D_pos = D_pos_cond.value();
+      if (gps->gps_avaliable()) {
+        waypoint current_wayp = nav_man.get_current_waypoint();
+        auto pos = sens_fus.position;
+
+        if ((current_wayp.coords_in_axis.value_or(
+                 Eigen::Vector3f(INFINITY, INFINITY, INFINITY)) -
+             pos)
+                .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]);
         }
       }
-      xSemaphoreGive(gps_mutex);
-    }
-    env_sens::dbg_sens();
+      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));
-  }
-}
-
-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));
-    PACKET_TYPE requested_type = packet->packet_requested;
-    std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
-
-    if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
-      resp_packet = create_packet_pooled(
-          PACKET_TYPE::INFO_DRONE_POSITION,
-          packet_info_drone_position{
-              {nav_filter.position.x(), nav_filter.position.y(),
-               nav_filter.position.z()},
-              {nav_filter.velocity.x(), nav_filter.velocity.y(),
-               nav_filter.velocity.z()},
-              {0.0, 0.0, 0.0}});
-    }
-
-    if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
-
-      if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
-
-        resp_packet = create_packet_pooled(
-            PACKET_TYPE::INFO_DRONE_STATUS,
-            packet_info_drone_status{
-                {gps->origin_long, gps->origin_lat}, millis(), 0});
-        xSemaphoreGive(gps_mutex);
+      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());
     }
 
-    if (resp_packet.first != nullptr) {
-      xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
-    }
+    vTaskDelay(pdMS_TO_TICKS(5));
   }
 }

main/nav.h

@@ -1,98 +1,74 @@
 #pragma once
+
+#include "drone_controller.h"
+
 #include "freertos/idf_additions.h"
+#include "gps.h"
 #include <cmath>
+#include <cstdint>
+#include <optional>
 
-#ifdef PS
-#undef PS
-#endif
+#define WAYPOINT_COUNT 8
 
-#ifdef F
-#undef F
-#endif
+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;
+};
 
-#include <Eigen/Dense>
+struct drone_nav {
+  waypoint waypoints[WAYPOINT_COUNT + 1];
+  uint8_t current_waypoint;
 
-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 set_active_mask(uint8_t mask) {
+    for (int i = 0; i < WAYPOINT_COUNT; i++) {
+      this->waypoints[i].active = (mask & (1 << i)) != 0;
+    }
   }
 
-  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);
+  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;
+  }
 
-    // 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();
+  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;
+  }
 
-    // 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;
+  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;
 
-      this->yaw_offset =
-          std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
+      return;
     }
 
-    // 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();
+    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 nav_compl nav_filter;
+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

@@ -1,9 +1,11 @@
 #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 <Arduino.h>
+#include "packet_handler.h"
 #include <RFM69.h>
 #include <SPI.h>
 #include <cstdint>
@@ -11,12 +13,15 @@
 
 #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
+// 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
@@ -42,8 +47,8 @@ void radio_task(void *pvParameters) {
   pinMode(RFM69_CS, OUTPUT);
   pinMode(RFM69_INT, INPUT);
 
-  SPIClass vspi(VSPI);
-  vspi.begin(SPI_SCLK, SPI_MISO, SPI_MOSI, 34);
+  SPIClass hspi(HSPI);
+  hspi.begin(RFM69_SCLK, RFM69_MISO, RFM69_MOSI, RFM69_CS);
 
   pinMode(RFM69_RST, OUTPUT);
   digitalWrite(RFM69_RST, HIGH);
@@ -51,12 +56,15 @@ void radio_task(void *pvParameters) {
   digitalWrite(RFM69_RST, LOW);
   vTaskDelay(pdMS_TO_TICKS(50));
 
-  RFM69 radio(RFM69_CS, RFM69_INT, true, &vspi);
+  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);
+    // 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();
@@ -94,6 +102,7 @@ void radio_task(void *pvParameters) {
         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);
       }
 
@@ -131,6 +140,6 @@ void radio_task(void *pvParameters) {
       confirmed_at_new_rate = false;
     }
 
-    vTaskDelay(pdMS_TO_TICKS(10));
+    vTaskDelay(pdMS_TO_TICKS(1));
   }
 }

main/radio.h

@@ -5,7 +5,5 @@
 
 inline QueueHandle_t packet_rx_queue = NULL;
 inline QueueHandle_t packet_tx_queue = NULL;
-inline SemaphoreHandle_t controller_input_semaphore = NULL;
-inline packet_controller_input current_controller_input;
 
 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