landing sequence?

main/drone.cpp

@@ -8,7 +8,9 @@
 #include "drone_controller.h"
 
 #include "dshot_definitions.h"
-#include "esp32-hal.h" #include "esp_log.h" #include "freertos/FreeRTOS.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"
@@ -40,7 +42,7 @@ dcont::ControllerConfig default_config() {
 
   // Velocity Loop (Velocity -> Acceleration/Rotation)
   config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f},
-                             .ki = {0.0f, 0.0f, 0.0f},
+                             .ki = {0.01f, 0.01f, 0.01f},
                              .kd = {0.0f, 0.0f, 0.0f},
                              .frequency = 50.0f};
   // Rotation Loop (Rotation/Accel -> Angular Rate)

main/drone.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "nav.h"
+
 #include "esp32-hal.h"
 #include <atomic>
 #include <cmath>
@@ -21,12 +23,13 @@
 #include "packet_handler.h"
 
 #include "imu.h"
-#include "nav.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]}; }
@@ -178,6 +181,9 @@ struct drone_cont_state {
         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) {

main/main.cpp

@@ -1,5 +1,7 @@
+#include "Eigen/Core"
 #include "driver/gpio.h"
-#include "drone.h" #include "drone_comms.h"
+#include "drone.h"
+#include "drone_comms.h"
 #include "drone_controller.h"
 #include "esp32-hal.h"
 #include "esp_log.h"
@@ -8,6 +10,7 @@
 #include "freertos/projdefs.h"
 #include "freertos/task.h"
 #include <atomic>
+#include <cmath>
 #include <cstdint>
 #include <optional>
 
@@ -33,15 +36,15 @@ 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, 5,
                           NULL, 0);
 
@@ -98,6 +101,24 @@ extern "C" void app_main(void) {
       last_status_broadcast_time = millis();
     }
 
+    if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) {
+
+      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();

main/nav.h

@@ -1,4 +1,7 @@
 #pragma once
+
+#include "drone_controller.h"
+
 #include "freertos/idf_additions.h"
 #include "gps.h"
 #include <cmath>
@@ -11,10 +14,11 @@ struct waypoint {
   Eigen::Vector3f coords; // lat, lon, alt
   std::optional<Eigen::Vector3f> coords_in_axis = std::nullopt;
   bool active = false; // active or to be skipped
+  bool landing = false;
 };
 
 struct drone_nav {
-  waypoint waypoints[WAYPOINT_COUNT];
+  waypoint waypoints[WAYPOINT_COUNT + 1];
   uint8_t current_waypoint;
 
   void set_active_mask(uint8_t mask) {
@@ -42,6 +46,28 @@ struct drone_nav {
     }
     return wayp;
   }
+
+  void waypoint_reached() {
+    waypoint wayp = waypoints[this->current_waypoint];
+    if (wayp.landing) {
+      this->waypoints[8] = waypoint{
+          .coords = Eigen::Vector3f(wayp.coords.x(), wayp.coords.y(), 0.0f),
+          .coords_in_axis = std::nullopt};
+      current_waypoint = 8;
+
+      return;
+    }
+
+    int i = this->current_waypoint + 1;
+    while (i != this->current_waypoint) {
+      bool active = waypoints[i].active;
+      if (active) {
+        this->current_waypoint = i;
+      }
+      i++;
+      i = i % 8;
+    }
+  }
 };
 
 inline SemaphoreHandle_t nav_mutex = NULL;

main/sens_fus.h

@@ -37,8 +37,8 @@ struct sens_fus_compl {
     this->velocity_error = Eigen::Vector3f::Zero();
   }
 
-  Eigen::Vector3f velocity_error_mult = {40.0f, 40.0f, 0.0f};
+  Eigen::Vector3f velocity_error_mult = {0.1f, 0.1f, 0.0f};
-  Eigen::Vector3f position_error_mult = {40.0f, 40.0f, 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
@@ -46,10 +46,10 @@ 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 = {4.0f, 4.0f, INFINITY};
+  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, 4.0f};
+  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();
@@ -57,16 +57,16 @@ struct sens_fus_compl {
   void predict(float dt, Eigen::Vector3f accel) {
     Eigen::Vector3f accel_err_rmvd =
         accel.array() -
-        this->velocity_error.array() * this->velocity_error_mult.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 = this->position.array() +
-        this->position.array() +
+                     (((this->velocity + next_velocity) * 0.5f).array() -
-        (((this->velocity + next_velocity) * 0.5f).array() -
+                      (this->position_error.array() *
-         this->position_error.array() * this->position_error_mult.array()) *
+                       this->position_error_mult.array() / dt)) *
-            dt;
+                         dt;
 
     this->velocity = next_velocity;
     this->last_accel_world = accel_err_rmvd;