i dont even know atp, everything is kinda working lwk just polishing and fixing nav/sens fusion. TODO: Automatic stabilization and landing sequence

components/drone_comms

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

main/drone.cpp

@@ -8,6 +8,7 @@
 
 #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"
@@ -20,6 +21,7 @@
 #include <cstdint>
 #include <cstring>
 #include <optional>
+#include <stdlib.h>
 
 #define CONTROLLER_TASK_FREQUENCY 1000.0;
 
@@ -31,45 +33,43 @@ dcont::ControllerConfig default_config() {
 
   // Position Loop (Position -> Velocity)
   config.stack.position_pid = {
-      {1.0f, 1.0f, 1.0f}, // kp
+      .kp = {1.0f, 1.0f, 1.0f}, // kp
-      {0.0f, 0.0f, 0.0f}, // ki
+      .ki = {0.0f, 0.0f, 0.0f}, // ki
-      {0.0f, 0.0f, 0.0f}, // kd
+      .kd = {0.0f, 0.0f, 0.0f}, // kd
-      5.0f                // frequency (Hz)
+      .frequency = 25.0f        // frequency (Hz)
   };
 
-  // Velocity Loop (Velocity -> Acceleration/Tilt)
+  // Velocity Loop (Velocity -> Acceleration/Rotation)
-  config.stack.linvel_pid = {
+  config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f},
-      {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, 15.0f};
+                             .ki = {0.0f, 0.0f, 0.0f},
-  // Rotation Loop (Angle -> Angular Rate)
+                             .kd = {0.0f, 0.0f, 0.0f},
-  config.stack.rotation_pid = {
+                             .frequency = 50.0f};
-      {4.0f, 4.0f, 4.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, 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 = {{0.1f, 0.1f, 1.0f},
+  config.stack.rate_pid = {.kp = {0.1f, 0.1f, 1.0f},
-                           {0.01f, 0.01f, 0.01f},
+                           .ki = {0.01f, 0.01f, 0.01f},
-                           {0.001f, 0.001f, 0.001f},
+                           .kd = {0.001f, 0.001f, 0.001f},
-                           100.0f};
+                           .frequency = 1000.0f};
   // 2. Set Constraints
   config.stack.max_rate = 3.14f;  // ~180 degrees/s
-  config.stack.max_linvel = 10.0f; // 10 m/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
-  /*
-    * roll, pitch, yaw
-      {1.0f, -1.0f, 1.0f},   // Front Right
-      {-1.0f, -1.0f, -1.0f}, // Front Left
-      {-1.0f, 1.0f, 1.0f},   // Rear Left
-      {1.0f, 1.0f, -1.0f}    // Rear Right
-   */
 
   float mixer[4][3] = {
-      {1.0f, -1.0f, 1.0f},   // Front Right
+      // roll, pitch, yaw
-      {-1.0f, -1.0f, -1.0f}, // Front Left
+      {-1.0, -1.0, -1.0}, // Front Left
-      {-1.0f, 1.0f, 1.0f},   // Rear Left
+      {1.0, 1.0, -1.0},   // Rear Right
-      {1.0f, 1.0f, -1.0f}    // Rear Right
+      {-1.0, 1.0, 1.0},   // Rear Left
+      {1.0, -1.0, 1.0},   // Front Right
   };
 
   for (int i = 0; i < 4; i++) {
@@ -90,9 +90,10 @@ void drone_cont_stabilize() {
   // afterwards
 }
 
+constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
+
 void drone_controller_task(void *params) {
   setup_drone();
-  uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
 
   imu_state imu_state_local;
   Eigen::Vector3f position_local = Eigen::Vector3f::Zero();
@@ -118,6 +119,9 @@ void drone_controller_task(void *params) {
     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);
@@ -136,18 +140,21 @@ void drone_controller_task(void *params) {
     } else {
       auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero());
 
-      dcont::set_input(drone_controller,
+      dcont::set_input(
-                       dcont::Input{{cont_input.ly, cont_input.lx,
+          drone_controller,
-                                     cont_input.rx, cont_input.ry},
+          dcont::Input{.joystick = {.throttle_input = cont_input.ly,
-                                    {0.0, 0.0, 0.0},
+                                    .roll_input = cont_input.rx,
-                                    {0.0, 0.0, 0.0},
+                                    .yaw_input = cont_input.lx,
-                                    {0.0, 0.0, 0.0},
+                                    .pitch_input = cont_input.ry},
-                                    {coords.x(), coords.y(), coords.z()},
+                       .acceleration = {0.0, 0.0, 0.0},
-                                    current_input_mode});
+                       .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,
+    // memcpy(dcont::get_throttles(drone_controller).values, motor_throttles,
-           sizeof(motor_throttles));
+    //        sizeof(motor_throttles));
 
     vTaskDelay(pdMS_TO_TICKS(wait_ms));
   }
@@ -156,12 +163,12 @@ void drone_controller_task(void *params) {
 const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14,
                                   GPIO_NUM_15};
 
-// const bool reversed[4] = {false, true, false, false};
-
 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;
     motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
     motors[i]->begin();
   }
@@ -172,13 +179,13 @@ void motor_throttles_task(void *params) {
     for (int i = 0; i < 4; i++) {
       motors[i]->sendThrottlePercent(0);
     }
-    vTaskDelay(1);
+    vTaskDelay(2);
   }
 
   while (true) {
     for (int i = 0; i < 4; i++) {
       motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f);
     }
-    vTaskDelay(1);
+    vTaskDelay(2);
   }
 }

main/drone.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "esp32-hal.h"
+#include <cstdint>
 #ifdef PS
 #undef PS
 #endif
@@ -25,5 +27,5 @@ void drone_controller_task(void *params);
 void motor_throttles_task(void *params);
 
 // 3     4     2     1
-inline float motor_throttles[4] = {0.01, 0.01, 0.01, 0.01};
+inline float *motor_throttles;
 inline bool killswitch_active = false;

main/env_sens.cpp

@@ -48,8 +48,10 @@ void setup() {
 }
 
 float get_temperature() {
-  sensors_event_t temp_event;
 
+  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);
@@ -57,11 +59,15 @@ float get_temperature() {
     }
 
     return temp_event.temperature;
+  }
+  return env_sens::term_read;
 }
 
 float get_pressure() {
-  sensors_event_t e;
 
+  if (millis() - env_sens::time_baro_read > 20) {
+
+    sensors_event_t e;
     if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) {
 
       bme_pressure->getEvent(&e);
@@ -69,6 +75,8 @@ float get_pressure() {
     }
 
     return e.pressure;
+  }
+  return env_sens::baro_read;
 }
 
 float calculateAltitude(float pressure, float seaLevelPressure,

main/env_sens.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cstdint>
 #ifdef PS
 #undef PS
 #endif
@@ -24,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/imu.cpp

@@ -36,10 +36,12 @@ void setup_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.");
     ESP.restart();
   }
+  imu->print_product_ids();
 
   imu->dynamic_calibration_autosave_enable();
   imu->dynamic_calibration_enable(BNO08xCalSel::all);
@@ -49,6 +51,7 @@ void setup_imu() {
   imu->rpt.cal_gyro.enable(2500UL);
 
   imu->register_cb([imu, local_state]() {
+    ESP_LOGI("IMU", "CALLBACK");
     bool needs_updating = false;
     if (sens_fus_mutex == NULL || imu_state_mutex == NULL)
       return;
@@ -59,8 +62,12 @@ void setup_imu() {
 
       auto sens_rot = imu->rpt.rv_game.get_quat();
       auto sens_euler = imu->rpt.rv_game.get_euler();
-      local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
+      local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k,
-      local_state->rot_euler = {sens_euler.x, sens_euler.y, sens_euler.z};
+                          sens_rot.real}; // FIXME: WRONG ROTATION
+      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()) {
@@ -68,7 +75,7 @@ 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()) {
@@ -76,7 +83,7 @@ void setup_imu() {
       needs_updating = true;
 
       auto sens_accel = imu->rpt.linear_accelerometer.get();
-      local_state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
+      local_state->accel = {sens_accel.x, -sens_accel.y, -sens_accel.z};
       int64_t current_time = esp_timer_get_time();
 
       if (local_state->last_time != -1) {

main/main.cpp

@@ -63,21 +63,27 @@ extern "C" void app_main(void) {
                           1     // Core ID
   );
 
-  ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
-
   // setup_imu();
+  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_broadcast_time = 0;
+
   while (true) {
     while (packet_tx_queue &&
            xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
       handle_packet(&packet_data[0]);
     }
 
+    if (millis() > last_broadcast_time + 100) {
+      send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
+      last_broadcast_time = millis();
+    }
+
     if (millis() > last_print_time + 5000) {
       last_print_time = millis();
 
@@ -137,6 +143,6 @@ extern "C" void app_main(void) {
                current_controller_input.rx, current_controller_input.ry);
     }
 
-    vTaskDelay(pdMS_TO_TICKS(10));
+    vTaskDelay(pdMS_TO_TICKS(50));
   }
 }

main/packet_handler.cpp

@@ -3,14 +3,18 @@
 #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 <cstdint>
+#include <sys/unistd.h>
 
 #ifdef PS
 #undef PS
@@ -22,15 +26,15 @@
 
 #include <Eigen/Dense>
 
-void handle_request(uint8_t *packet_addr);
-
 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) {
-    handle_request(packet_addr);
+    packet_command_request *packet =
+        (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
+    send_packet_getter(packet->packet_requested);
   }
 
   // NAV SETTERS
@@ -64,6 +68,7 @@ void handle_packet(uint8_t *packet_addr) {
 
     if (xSemaphoreTake(controller_input_semaphore, 10)) {
       current_controller_input = *packet;
+      time_last_controller = millis();
       xSemaphoreGive(controller_input_semaphore);
     }
   }
@@ -94,23 +99,23 @@ void handle_nav_update(uint8_t *packet_addr) {
   }
 }
 
-void handle_request(uint8_t *packet_addr) {
+void send_packet_getter(PACKET_TYPE requested_type) {
 
-  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) {
     // TODO: send pitch, roll, yaw
-    resp_packet =
+    resp_packet = create_packet_pooled(
-        create_packet_pooled(PACKET_TYPE::INFO_DRONE_POSITION,
+        PACKET_TYPE::INFO_DRONE_POSITION,
         packet_info_drone_position{
-                                 {sens_fus.position.x(), sens_fus.position.y(),
+            .trans = {sens_fus.position.x(), sens_fus.position.y(),
                       sens_fus.position.z()},
-                                 {sens_fus.velocity.x(), sens_fus.velocity.y(),
+            .vel = {sens_fus.velocity.x(), sens_fus.velocity.y(),
                     sens_fus.velocity.z()},
-                                 {0.0, 0.0, 0.0}});
+            .rot = {imu_state_var.rot_euler.x(), imu_state_var.rot_euler.y(),
+                    imu_state_var.rot_euler.z()},
+            .pressure = env_sens::get_pressure(),
+            .temperature = env_sens::get_temperature()});
   }
 
   if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {

main/packet_handler.h

@@ -3,9 +3,14 @@
 #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/sens_fus.h

@@ -1,4 +1,5 @@
 #pragma once
+#include "Eigen/Core"
 #include <cmath>
 
 #ifdef PS
@@ -25,6 +26,7 @@ inline float getYawDifference(const Eigen::Vector3f &v_gps,
 struct sens_fus_compl {
   Eigen::Vector3f position = Eigen::Vector3f::Zero();
   Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
+  Eigen::Vector3f velocity_last_gps_measurment = Eigen::Vector3f::Zero();
   Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
   float yaw_offset = 0.0f;
 
@@ -75,7 +77,12 @@ struct sens_fus_compl {
 
     // 2. Yaw Correction (only if moving > 1.0 m/s)
     if (gps_vel.norm() > 1.0f) {
-      float yaw_delta = getYawDifference(gps_vel, this->velocity);
+      Eigen::Vector3f delta_v_imu =
+          this->velocity - this->velocity_last_gps_measurment;
+      Eigen::Vector3f delta_v_gps = this->velocity - gps_vel;
+
+      float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu);
+
       this->yaw_offset += yaw_delta * alpha_yaw;
 
       this->yaw_offset =
@@ -88,6 +95,8 @@ struct sens_fus_compl {
     } else if (this->velocity.norm() > 1.0) {
       this->velocity *= 1 - (0.90 * dt);
     }
+
+    this->velocity_last_gps_measurment = this->velocity;
   }
 
   void measure_baro(float dt, Eigen::Vector3f baro_pos,