before fucking shit up

components/drone_comms

@@ -1 +1 @@
-Subproject commit 350c761b9fe11f551e5c8ddf11e84252d2f496b4
+Subproject commit ac25a39777bffe724f80de5ec86bc1b0773833a2

main/drone.cpp

@@ -2,6 +2,7 @@
 
 #include "DShotRMT.h"
 #include "Eigen/Core"
+#include "Eigen/Geometry"
 #include "driver/rmt_tx.h"
 #include "drone_comms.h"
 #include "drone_controller.h"
@@ -81,80 +82,14 @@ dcont::ControllerConfig default_config() {
   return config;
 }
 
-void setup_drone() { drone_controller = dcont::create(default_config()); }
-
-void drone_cont_stabilize() {
-  // TODO: Implement stabilization. if |angvel| > something, we should make it 0
-  // first, then if falling fast, pull rotation to the side, then when falling
-  // is stabilized, turn slowly up until pointing up. let velocity dissipate
-  // afterwards
-}
-
 constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
 
 void drone_controller_task(void *params) {
-  setup_drone();
+  drone_cont = new drone_cont_state;
-
+  drone_cont->init();
-  imu_state imu_state_local;
-  Eigen::Vector3f position_local = Eigen::Vector3f::Zero();
-  Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero();
 
   while (true) {
-    if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) {
+    drone_cont->update();
-      imu_state_local = imu_state_var;
-      xSemaphoreGive(imu_state_mutex);
-    }
-    if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) {
-      position_local = sens_fus.position;
-      velocity_local = sens_fus.velocity;
-      xSemaphoreGive(sens_fus_mutex);
-    }
-
-    dcont::set_cur_time(drone_controller, millis() / 1000.0f);
-    dcont::set_cur_angvel(drone_controller, v3f_to_v3c(imu_state_local.angvel));
-    dcont::set_cur_linvel(drone_controller, v3f_to_v3c(velocity_local));
-    dcont::set_cur_pos(drone_controller, v3f_to_v3c(position_local));
-    dcont::set_cur_rot(drone_controller, imu_state_local.rot);
-
-    packet_controller_input cont_input;
-    if (current_input_mode == dcont::ModeInput::Acro &&
-        xSemaphoreTake(controller_input_semaphore, 10)) {
-      if (millis() - time_last_controller > 1) {
-        current_controller_input = {0, 0, 0, 0};
-      }
-      cont_input = current_controller_input;
-
-      xSemaphoreGive(controller_input_semaphore);
-    }
-
-    waypoint wayp;
-    if (current_input_mode == dcont::ModeInput::Position &&
-        xSemaphoreTake(nav_mutex, 10)) {
-      wayp = nav_man.get_current_waypoint();
-
-      xSemaphoreGive(nav_mutex);
-    }
-    if (current_input_mode == dcont::ModeInput::Position &&
-        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 = cont_input.ly,
-                                    .roll_input = cont_input.rx,
-                                    .yaw_input = cont_input.lx,
-                                    .pitch_input = cont_input.ry},
-                       .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 = current_input_mode});
-    }
-
-    // memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
-    //        sizeof(motor_throttles));
 
     vTaskDelay(pdMS_TO_TICKS(wait_ms));
   }
@@ -184,7 +119,12 @@ void motor_throttles_task(void *params) {
 
   while (true) {
     for (int i = 0; i < 4; i++) {
-      motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f);
+      float throttle = motor_throttles[i] * 100.0f * 0.7f;
+      if (atomic_load(&killswitch_active)) {
+        throttle = 0.0;
+      }
+      // motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f * 0.7f);
+      motors[i]->sendThrottlePercent(10.0f);
     }
     vTaskDelay(2);
   }

main/drone.h

@@ -1,7 +1,10 @@
 #pragma once
 
 #include "esp32-hal.h"
+#include <atomic>
+#include <cmath>
 #include <cstdint>
+#include <stdatomic.h>
 #ifdef PS
 #undef PS
 #endif
@@ -15,17 +18,204 @@
 #include "drone_comms.h"
 #include "drone_controller.h"
 
+#include "packet_handler.h"
+
+#include "imu.h"
+#include "nav.h"
+#include "packet_handler.h"
+#include "sens_fus.h"
+
+#define CONNECTION_LOST_THRESHOLD 200
+
 void setup_drone();
 
 inline dcont::Vec3C v3f_to_v3c(Eigen::Vector3f v) { return {v[0], v[1], v[2]}; }
 
-inline dcont::StackedController *drone_controller = nullptr;
-inline dcont::ModeInput current_input_mode = dcont::ModeInput::Acro;
-
 void drone_controller_task(void *params);
 
 void motor_throttles_task(void *params);
 
 // 3     4     2     1
-inline float *motor_throttles;
+inline float *motor_throttles = nullptr;
-inline bool killswitch_active = false;
+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() {
+    memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
+           sizeof(float) * 4);
+  }
+
+  void update_input() {
+
+    packet_controller_input cont_input;
+
+    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};
+        }
+        cont_input = current_controller_input;
+
+        xSemaphoreGive(controller_input_semaphore);
+
+        dcont::set_input(
+            drone_controller,
+            dcont::Input{.joystick = {.throttle_input = cont_input.ly,
+                                      .roll_input = cont_input.rx,
+                                      .yaw_input = cont_input.lx,
+                                      .pitch_input = cont_input.ry},
+                         .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});
+      } 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();
+
+        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,13 @@
 #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"
 
@@ -113,6 +115,9 @@ 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;
 

main/gps.cpp

@@ -52,6 +52,6 @@ void gps_poll_task(void *_) {
       }
     }
 
-    vTaskDelay(pdMS_TO_TICKS(10)); // 10Hz polling
+    vTaskDelay(pdMS_TO_TICKS(5)); // 10Hz polling
   }
 }

main/gps.h

@@ -13,8 +13,7 @@
 #include "HardwareSerial.h"
 
 #include "esp_log.h"
-#include <Adafruit_GPS.h>
+#include <Adafruit_GPS.h> #include <cmath>
-#include <cmath>
 #include <optional>
 
 const float TO_RAD = M_PI / 180.0f;
@@ -91,10 +90,12 @@ struct GPS {
   }
 
   void poll() {
-    char c = this->gps->read();
+    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);
+      // ESP_LOGI("GPS", "NMEA LINE: %s", line);
       this->gps->parse(line);
     }
   }

main/imu.cpp

@@ -1,5 +1,7 @@
 #include "imu.h"
+#include "Eigen/Geometry"
 #include "Esp.h"
+#include "drone_controller.h"
 
 #ifdef PS
 #undef PS
@@ -28,7 +30,7 @@ static const char *TAG = "IMU";
 #define IMU_MISO GPIO_NUM_5 // SDA
 #define IMU_SCLK GPIO_NUM_6 // SCL
 
-void setup_imu() {
+BNO08x *setup_imu() {
   imu_state *local_state = new imu_state;
   imu_state_mutex = xSemaphoreCreateMutex();
 
@@ -46,28 +48,39 @@ void setup_imu() {
   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, local_state]() {
-    ESP_LOGI("IMU", "CALLBACK");
+    // ESP_LOGI("IMU", "CALLBACK");
     bool needs_updating = false;
     if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
       return;
 
-    if (imu->rpt.rv_game.has_new_data()) {
+    if (imu->rpt.rv.has_new_data()) {
 
       needs_updating = true;
 
-      auto sens_rot = imu->rpt.rv_game.get_quat();
+      auto sens_rot = imu->rpt.rv.get_quat();
-      auto sens_euler = imu->rpt.rv_game.get_euler();
+      auto sens_euler = imu->rpt.rv.get_euler();
-      local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k,
+      local_state->rot =
-                          sens_rot.real}; // FIXME: WRONG ROTATION
+          Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k);
-      local_state->rot_euler = {sens_euler.x, -sens_euler.y, -sens_euler.z};
+
-      ESP_LOGI("IMU", "rot: roll %f, pitch %f, yaw %f",
+      // Eigen::Quaternionf q_global_yaw(
-               local_state->rot_euler.x(), local_state->rot_euler.y(),
+      //     Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));
-               local_state->rot_euler.z());
+      // 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()) {
@@ -75,23 +88,36 @@ void setup_imu() {
       needs_updating = true;
 
       auto cal_gyro = imu->rpt.cal_gyro.get();
-      local_state->angvel = {cal_gyro.x, -cal_gyro.y, -cal_gyro.z};
+      local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z};
     }
 
     if (imu->rpt.linear_accelerometer.has_new_data()) {
 
       needs_updating = true;
 
-      auto sens_accel = imu->rpt.linear_accelerometer.get();
+      auto sens_accel_lin = imu->rpt.linear_accelerometer.get();
-      local_state->accel = {sens_accel.x, -sens_accel.y, -sens_accel.z};
+      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 (local_state->last_time != -1) {
 
         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->accel, &local_state->rot);
+            dcont::apply_rot(&local_state->lin_accel, &quatc);
+
+        local_state->lin_accel_global = accel_global;
 
         if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
 
@@ -120,4 +146,5 @@ void setup_imu() {
   });
 
   ESP_LOGI(TAG, "IMU Setup Success.");
+  return imu;
 }

main/imu.h

@@ -25,14 +25,16 @@
 #include <Eigen/Dense>
 
 struct imu_state {
-  dcont::Vec3C accel = {0, 0, 0};
+  dcont::Vec3C lin_accel = {0, 0, 0};
-  dcont::QuatC rot = {0, 0, 0, 1};
+  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;
 };
 
-void setup_imu();
+BNO08x *setup_imu();
 
 inline SemaphoreHandle_t imu_state_mutex = NULL;
 inline imu_state imu_state_var;

main/main.cpp

@@ -1,12 +1,13 @@
 #include "driver/gpio.h"
-#include "drone.h"
+#include "drone.h" #include "drone_comms.h"
-#include "drone_comms.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 <cstdint>
 #include <optional>
 
@@ -20,6 +21,8 @@
 
 static const char *TAG = "MAIN";
 
+#define TIME_RELEASE_QUEUE_TO_ACTIVATION 1000
+
 extern "C" void app_main(void) {
 
   sens_fus_mutex = xSemaphoreCreateMutex();
@@ -30,20 +33,19 @@ extern "C" void app_main(void) {
   gpio_install_isr_service(0);
   Serial.begin(115200);
 
-  // xTaskCreatePinnedToCore(radio_task,   // Function name
+  xTaskCreatePinnedToCore(radio_task,   // Function name
-  //                         "radio_rxtx", // Name for debugging
+                          "radio_rxtx", // Name for debugging
-  //                         4096,         // Stack size in bytes
+                          4096,         // Stack size in bytes
-  //                         NULL,         // Parameters
+                          NULL,         // Parameters
-  //                         5,            // Priority (higher = more urgent)
+                          5,            // Priority (higher = more urgent)
-  //                         NULL,         // Task handle
+                          NULL,         // Task handle
-  //                         0             // Core ID
+                          0             // Core ID
-  // );
+  );
-  //
+
-  // xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL,
+  xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5,
-  // 1,
+                          NULL, 0);
-  //                         NULL, 0);
+
-  //
+  xTaskCreatePinnedToCore(gps_poll_task, "gps_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
@@ -63,7 +65,7 @@ extern "C" void app_main(void) {
   //                         1     // Core ID
   // );
 
-  // setup_imu();
+  setup_imu();
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
 
   Eigen::Vector3f local_pos = {0, 0, 0};
@@ -71,20 +73,49 @@ extern "C" void app_main(void) {
   bool nav_data_ready = false;
 
   uint64_t last_print_time = 0;
-  uint64_t last_broadcast_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) {
-    while (packet_tx_queue &&
+    while (packet_rx_queue &&
-           xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
+           xQueueReceive(packet_rx_queue, &packet_data[0], 1)) {
       handle_packet(&packet_data[0]);
     }
 
-    if (millis() > last_broadcast_time + 100) {
+    if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
       send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
-      last_broadcast_time = millis();
+      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 (millis() > last_print_time + 5000) {
+    if (imu_state_var.lin_accel_global.z < -8.0 && !released) {
+      released = true;
+      time_activation_queue = millis();
+    }
+
+    if (released &&
+        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);
+      std::atomic_store(&killswitch_active, false);
+      // std::atomic_store(&drone_cont->current_input_mode,
+      // INPUT_TYPE::AUTO_NAV);
+    }
+
+    // Logging
+    if (millis() > last_print_time + 1000) {
       last_print_time = millis();
 
       std::optional<Eigen::Vector3f> coords;
@@ -135,13 +166,21 @@ extern "C" void app_main(void) {
                  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(50));
   }

main/packet_handler.cpp

@@ -42,26 +42,20 @@ void handle_packet(uint8_t *packet_addr) {
            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));
     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));
-    switch (packet->input_type) {
+    atomic_store(&drone_cont->current_input_mode, packet->input_type);
-    case INPUT_TYPE::ACRO:
+
-      current_input_mode = dcont::ModeInput::Acro;
-      break;
-    case INPUT_TYPE::LEVEL:
-    case INPUT_TYPE::STABILIZE_FALL:
-      ESP_LOGE("PACKET_HANDLER",
-               "INPUT TYPES NOT IMPLEMENTED. DEFAULTING TO POSITION (AUTONAV)");
-    case INPUT_TYPE::AUTO_NAV:
-      current_input_mode = dcont::ModeInput::Position;
-    }
   } else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) {
     packet_controller_input *packet =
         (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
@@ -104,16 +98,19 @@ 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) {
-    // TODO: send pitch, roll, yaw
+
+    Eigen::Vector3f local_vel = imu_state_var.rot.inverse() * sens_fus.velocity;
+
     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 = {sens_fus.velocity.x(), sens_fus.velocity.y(),
+            .accel = {imu_state_var.lin_accel.x, imu_state_var.lin_accel.y,
-                    sens_fus.velocity.z()},
+                      imu_state_var.lin_accel.z},
-            .rot = {imu_state_var.rot_euler.x(), imu_state_var.rot_euler.y(),
+            .vel = {local_vel.x(), local_vel.y(), local_vel.z()},
-                    imu_state_var.rot_euler.z()},
+            .rot = {imu_state_var.rot.w(), imu_state_var.rot.x(),
+                    imu_state_var.rot.y(), imu_state_var.rot.z()},
             .pressure = env_sens::get_pressure(),
             .temperature = env_sens::get_temperature()});
   }
@@ -126,7 +123,11 @@ void send_packet_getter(PACKET_TYPE requested_type) {
       resp_packet = create_packet_pooled(
           PACKET_TYPE::INFO_DRONE_STATUS,
           packet_info_drone_status{
-              {gps->origin_long, gps->origin_lat}, millis(), 0});
+              .origin = {gps->origin_long, gps->origin_lat},
+              .time_since_boot = millis(),
+              .unix_timestamp_millis = 0,
+              .gps_fix = gps->gps_avaliable()});
+      ESP_LOGI("STATUS", "Status sent");
       xSemaphoreGive(gps_mutex);
     }
   }

main/sens_fus.h

@@ -37,20 +37,20 @@ struct sens_fus_compl {
    * 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 = {20.0f, 20.0f, INFINITY};
+  Eigen::Vector3f tau_gps_pos = {2.0f, 2.0f, INFINITY};
-  Eigen::Vector3f tau_gps_vel = {40.0f, 40.0f, INFINITY};
+  Eigen::Vector3f tau_gps_vel = {INFINITY, INFINITY, INFINITY};
 
-  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f};
+  Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 10.0f};
-  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f};
+  Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
 
-  float tau_yaw = 20.0f;
+  float tau_yaw = 10.0f; // Yaw remains a scalar
 
   Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval();
 
   void update_yaw_matrix() {
-    yaw_rotation_matrix =
+    // yaw_rotation_matrix =
-        Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ())
+    //     Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ())
-            .toRotationMatrix();
+    //         .toRotationMatrix();
   }
 
   void predict(float dt, Eigen::Vector3f accel) {
@@ -77,12 +77,11 @@ struct sens_fus_compl {
         alpha_pos.array() * gps_pos.array();
 
     // 2. Yaw Correction (only if moving > 1.0 m/s)
-    if (gps_vel.norm() > 1.0f) {
+    if (gps_vel.norm() > 0.2f) {
       Eigen::Vector3f delta_v_imu =
           this->velocity - this->velocity_last_gps_measurment;
       Eigen::Vector3f delta_v_gps = this->velocity - gps_vel;
 
-      if (abs(delta_v_gps.norm() - delta_v_imu.norm()) < 1.0 * dt) {
       float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu);
 
       this->yaw_offset += yaw_delta * alpha_yaw;
@@ -90,11 +89,11 @@ struct sens_fus_compl {
       this->yaw_offset =
           std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
       this->update_yaw_matrix();
-      }
+
       this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() *
                            this->velocity.array() +
                        alpha_vel.array() * gps_vel.array();
-    } else if (this->velocity.norm() > 1.0) {
+    } else if (this->velocity.norm() > 0.2f) {
       this->velocity *= 1 - (0.90 * dt);
     }