From 437c61fb13b627f25d12f13f4120b4d743b5b4b7 Mon Sep 17 00:00:00 2001 From: franchioping Date: Sat, 18 Apr 2026 17:16:06 +0100 Subject: [PATCH] landing sequence? --- main/drone.cpp | 6 ++++-- main/drone.h | 8 +++++++- main/main.cpp | 41 +++++++++++++++++++++++++++++++---------- main/nav.h | 28 +++++++++++++++++++++++++++- main/sens_fus.h | 20 ++++++++++---------- 5 files changed, 79 insertions(+), 24 deletions(-) diff --git a/main/drone.cpp b/main/drone.cpp index 93b46c6..c10b3cd 100644 --- a/main/drone.cpp +++ b/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) diff --git a/main/drone.h b/main/drone.h index 9c5c1e1..a9b7371 100644 --- a/main/drone.h +++ b/main/drone.h @@ -1,5 +1,7 @@ #pragma once +#include "nav.h" + #include "esp32-hal.h" #include #include @@ -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) { diff --git a/main/main.cpp b/main/main.cpp index a268de9..99b40fa 100644 --- a/main/main.cpp +++ b/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 +#include #include #include @@ -33,15 +36,15 @@ extern "C" void app_main(void) { gpio_install_isr_service(0); Serial.begin(115200); - // xTaskCreatePinnedToCore(radio_task, // Function name - // "radio_rxtx", // Name for debugging - // 4096, // Stack size in bytes - // NULL, // Parameters - // 5, // Priority (higher = more urgent) - // NULL, // Task handle - // 0 // Core ID - // ); - // + xTaskCreatePinnedToCore(radio_task, // Function name + "radio_rxtx", // Name for debugging + 4096, // Stack size in bytes + NULL, // Parameters + 5, // Priority (higher = more urgent) + NULL, // Task handle + 0 // Core ID + ); + xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5, NULL, 0); @@ -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(); diff --git a/main/nav.h b/main/nav.h index 09eb806..82dba4e 100644 --- a/main/nav.h +++ b/main/nav.h @@ -1,4 +1,7 @@ #pragma once + +#include "drone_controller.h" + #include "freertos/idf_additions.h" #include "gps.h" #include @@ -11,10 +14,11 @@ struct waypoint { Eigen::Vector3f coords; // lat, lon, alt std::optional 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; diff --git a/main/sens_fus.h b/main/sens_fus.h index b5efca4..0958dff 100644 --- a/main/sens_fus.h +++ b/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 position_error_mult = {40.0f, 40.0f, 0.0f}; + 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 @@ -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.array() + - (((this->velocity + next_velocity) * 0.5f).array() - - this->position_error.array() * this->position_error_mult.array()) * - 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;