Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

landing sequence?

This commit is contained in:
franchioping 2026-04-18 17:16:06 +01:00
parent a9cd5e971f
commit 437c61fb13
5 changed files with 79 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
main/drone.cpp
View File

@ -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)

8
main/drone.h
View File

@ -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) {

41
main/main.cpp
View File

@ -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
// "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();

28
main/nav.h
View File

@ -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;

16
main/sens_fus.h
View File

@ -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,15 +57,15 @@ 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->position = this->position.array() +
(((this->velocity + next_velocity) * 0.5f).array() -
this->position_error.array() * this->position_error_mult.array()) *
(this->position_error.array() *
this->position_error_mult.array() / dt)) *
dt;
this->velocity = next_velocity;