diff --git a/components/drone_controller b/components/drone_controller index 9d8fccc..11a55db 160000 --- a/components/drone_controller +++ b/components/drone_controller @@ -1 +1 @@ -Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502 +Subproject commit 11a55db27127a10a99439cde27a04489bb7318d2 diff --git a/main/drone.cpp b/main/drone.cpp new file mode 100644 index 0000000..db0ae89 --- /dev/null +++ b/main/drone.cpp @@ -0,0 +1,99 @@ +#include "drone.h" + +#include "drone_controller.h" + +#include "esp32-hal.h" +#include "freertos/FreeRTOS.h" +#include "freertos/idf_additions.h" +#include "freertos/projdefs.h" +#include "freertos/task.h" +#include "imu.h" +#include "sens_fus.h" +#include + +#define CONTROLLER_TASK_FREQUENCY 400.0; + +dcont::ControllerConfig default_config() { + dcont::ControllerConfig config; + + // 1. Configure the PID Stack + // Note: kp, ki, kd are arrays of 3 [roll, pitch, yaw] + + // Position Loop (Position -> Velocity) + config.stack.position_pid = { + {1.0f, 1.0f, 1.0f}, // kp + {0.0f, 0.0f, 0.0f}, // ki + {0.0f, 0.0f, 0.0f}, // kd + 25.0f // frequency (Hz) + }; + + // Velocity Loop (Velocity -> Acceleration/Tilt) + config.stack.linvel_pid = { + {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, 50.0f}; + + // Rotation Loop (Angle -> Angular Rate) + config.stack.rotation_pid = { + {4.0f, 4.0f, 4.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, 200.0f}; + + // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop + config.stack.rate_pid = {{0.2f, 0.2f, 2.0f}, + {0.05f, 0.05f, 0.05f}, + {0.003f, 0.003f, 0.001f}, + 600.0f}; + + // 2. Set Constraints + config.stack.max_rate = 3.14f; // ~180 degrees/s + config.stack.max_linvel = 10.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 for each motor + float mixer[4][3] = { + {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 + }; + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 3; j++) { + config.motor_map[i][j] = mixer[i][j]; + } + } + + return config; +} + +void setup_drone() { drone_controller = dcont::create(default_config()); } + +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(); + Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero(); + + while (true) { + if (xSemaphoreTake(imu_state_mutex, 1)) { + imu_state_local = imu_state_var; + xSemaphoreGive(imu_state_mutex); + } + if (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); + + vTaskDelay(pdMS_TO_TICKS(wait_ms)); + } +} diff --git a/main/drone.h b/main/drone.h new file mode 100644 index 0000000..12a6e33 --- /dev/null +++ b/main/drone.h @@ -0,0 +1,24 @@ +#pragma once + +#include "Eigen/Core" +#include "drone_comms.h" +#include "drone_controller.h" + +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include + +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; + +void drone_controller_task(void *params); diff --git a/main/imu.cpp b/main/imu.cpp index fc89abd..e40bbc2 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -68,7 +68,8 @@ void setup_imu() { float dt = ((float)(current_time - local_state->last_time)) / 1000000.0f; - Vec3C accel_global = apply_rot(&local_state->accel, &local_state->rot); + dcont::Vec3C accel_global = + dcont::apply_rot(&local_state->accel, &local_state->rot); if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) { sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y, diff --git a/main/imu.h b/main/imu.h index 2c83540..52b457b 100644 --- a/main/imu.h +++ b/main/imu.h @@ -25,8 +25,8 @@ #include struct imu_state { - Vec3C accel = {0, 0, 0}; - QuatC rot = {0, 0, 0, 1}; + dcont::Vec3C accel = {0, 0, 0}; + dcont::QuatC rot = {0, 0, 0, 1}; int64_t last_time = -1; Eigen::Vector3f angvel; Eigen::Vector3f rot_euler; diff --git a/main/main.cpp b/main/main.cpp index d7d1b44..4d7206a 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 "esp32-hal.h" #include "esp_log.h" @@ -10,6 +12,7 @@ #include #include #include +#include #include "env_sens.h" #include "gps.h" @@ -30,8 +33,7 @@ extern "C" void app_main(void) { gpio_install_isr_service(0); Serial.begin(115200); - static imu_state imu_state; - auto _ = setup_imu(&imu_state); + setup_imu(); xTaskCreatePinnedToCore(radio_task, // Function name "radio_rxtx", // Name for debugging @@ -42,6 +44,15 @@ extern "C" void app_main(void) { 0 // Core ID ); + xTaskCreatePinnedToCore(drone_controller_task, // Function name + "drone_controller_task", // Name for debugging + 1024 * 32, // Stack size in bytes + NULL, // Parameters + 20, // Priority (higher = more urgent) + NULL, // Task handle + 1 // Core ID + ); + xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL); xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL); @@ -52,25 +63,28 @@ extern "C" void app_main(void) { Eigen::Vector3f local_vel = {0, 0, 0}; bool nav_data_ready = false; while (true) { - if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { + while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { handle_packet(&packet_data[0]); } + std::optional coords; + float lat, lon, alt; if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { if (gps->gps_avaliable()) { - ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", - gps->gps->latitudeDegrees, gps->gps->longitudeDegrees, - gps->gps->altitude); - - auto D_pos_cond = gps->get_coordinates(); - if (D_pos_cond.has_value()) { - auto D_pos = D_pos_cond.value(); - - ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1], - D_pos[2]); - } + coords = gps->get_coordinates(); + lat = gps->gps->latitudeDegrees; + lon = gps->gps->longitudeDegrees; + alt = gps->gps->altitude; } xSemaphoreGive(gps_mutex); + + if (coords.has_value()) { + auto D_pos = coords.value(); + + ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", lat, lon, alt); + ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1], + D_pos[2]); + } } env_sens::dbg_sens(); diff --git a/main/nav.cpp b/main/nav.cpp deleted file mode 100644 index 775c1e7..0000000 --- a/main/nav.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "nav.h"