From 435dfd4205a2ea9bcccf06390614f88ff8550c1f Mon Sep 17 00:00:00 2001 From: franchioping Date: Mon, 20 Apr 2026 02:52:05 +0100 Subject: [PATCH] working yaw --- flake.nix | 11 ++++++++-- main/drone.cpp | 54 ++++++++++++++++++++++++++++++++--------------- main/drone.h | 28 ++++++++++++++----------- main/main.cpp | 57 +++++++++++++++++++++++++++----------------------- 4 files changed, 93 insertions(+), 57 deletions(-) diff --git a/flake.nix b/flake.nix index 22fc9ff..cb1c623 100644 --- a/flake.nix +++ b/flake.nix @@ -25,8 +25,15 @@ devShells = forEachSupportedSystem ({ pkgs }: { default = pkgs.mkShell.override { } { packages = with pkgs; - [ esp-idf-full rustup openssl stdenv.cc.cc.lib libclang ] - ++ (if system == "aarch64-darwin" then [ ] else [ gdb ]); + [ + esp-idf-full + rustup + openssl + stdenv.cc.cc.lib + libclang + (pkgs.python3.withPackages + (python-pkgs: with python-pkgs; [ pandas matplotlib ])) + ] ++ (if system == "aarch64-darwin" then [ ] else [ gdb ]); shellHook = '' export CLANGD_QUERY_DRIVER="$(which clang),$(which clang++)" export CMAKE_EXPORT_COMPILE_COMMANDS=1 diff --git a/main/drone.cpp b/main/drone.cpp index 1449de7..dc30826 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -2,14 +2,14 @@ #include "DShotRMT.h" #include "Eigen/Core" -#include "Eigen/Geometry" -#include "driver/rmt_tx.h" +#include "Eigen/Geometry" #include "driver/rmt_tx.h" #include "drone_comms.h" #include "drone_controller.h" #include "dshot_definitions.h" #include "esp32-hal.h" #include "esp_log.h" +#include "esp_log_timestamp.h" #include "freertos/FreeRTOS.h" #include "freertos/idf_additions.h" #include "freertos/projdefs.h" @@ -25,7 +25,7 @@ #include #include -#define CONTROLLER_TASK_FREQUENCY 1000.0; +#define CONTROLLER_TASK_FREQUENCY 500.0; dcont::ControllerConfig default_config() { dcont::ControllerConfig config; @@ -46,17 +46,24 @@ dcont::ControllerConfig default_config() { .ki = {0.01f, 0.01f, 0.01f}, .kd = {0.0f, 0.0f, 0.0f}, .frequency = 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}; + config.stack.rotation_pid = { + .kp = {1.0f, 1.0f, 2.0f}, + .ki = {0.01f, 0.01f, 0.02f}, + .kd = {0.1f, 0.1f, 0.0f}, + .integral_cap = {4.0f, 4.0f, 4.0f}, + .frequency = 200.0f, + }; // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop - config.stack.rate_pid = {.kp = {0.1f, 0.1f, 1.0f}, - .ki = {0.01f, 0.01f, 0.01f}, - .kd = {0.001f, 0.001f, 0.001f}, - .frequency = 1000.0f}; + config.stack.rate_pid = { + .kp = {0.0f, 0.0f, 1.0f}, + .ki = {0.00f, 0.00f, 0.0f}, + .kd = {0.000f, 0.000f, 0.0f}, + .integral_cap = {1.0f, 1.0f, 1.0f}, + .frequency = 500.0f, + }; // 2. Set Constraints config.stack.max_rate = 3.14f; // ~180 degrees/s config.stack.max_linvel = 3.0f; // 10 m/s @@ -92,6 +99,12 @@ void drone_controller_task(void *params) { while (true) { drone_cont->update(); + // char *csv_str = (char + // *)dcont::debug_stacked(drone_cont->drone_controller, + // drone_cont->last_input); + // + // ESP_LOGI("DCONT_DBG", "START%sEND", csv_str); + // free(csv_str); vTaskDelay(pdMS_TO_TICKS(1)); } @@ -100,15 +113,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 gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_14, GPIO_NUM_46, -// GPIO_NUM_16}; - 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.02; + motor_throttles[i] = 0.0; motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false); motors[i]->begin(); } @@ -121,15 +131,25 @@ void motor_throttles_task(void *params) { } vTaskDelay(2); } + dcont::reset_pid_states(drone_cont->drone_controller); + int i = 0; while (true) { for (int i = 0; i < 4; i++) { float throttle = - std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.3f; + std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.4f; if (atomic_load(&killswitch_active)) { throttle = 0.0; } - motors[i]->sendThrottlePercent(5.0); + motors[i]->sendThrottlePercent(throttle); + } + + i++; + if (i >= 50) { + + ESP_LOGI("MOTORSSS", "throttles (%f,%f,%f,%f)", motor_throttles[0], + motor_throttles[1], motor_throttles[2], motor_throttles[3]); + i = 0; } vTaskDelay(2); } diff --git a/main/drone.h b/main/drone.h index af1ed61..913ab3a 100644 --- a/main/drone.h +++ b/main/drone.h @@ -48,6 +48,7 @@ dcont::ControllerConfig default_config(); struct drone_cont_state { bool angvel_stablilized; bool fall_vel_stabilized; + dcont::Input last_input; Eigen::Vector3f pos; Eigen::Vector3f vel; @@ -156,7 +157,8 @@ struct drone_cont_state { switch (atomic_load(&this->current_input_mode)) { case INPUT_TYPE::ACRO: { - if (xSemaphoreTake(controller_input_semaphore, 10)) { + if (controller_input_semaphore && + xSemaphoreTake(controller_input_semaphore, 10)) { // if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) { // current_controller_input = {0, 0, 0, 0}; // } @@ -164,17 +166,19 @@ struct drone_cont_state { xSemaphoreGive(controller_input_semaphore); - dcont::set_input( - drone_controller, - dcont::Input{.joystick = {.throttle_input = cont_input.ly, - .roll_input = cont_input.rx, - .yaw_input = cont_input.lx, - .pitch_input = cont_input.ry}, - .acceleration = {0.0, 0.0, 0.0}, - .rotation = {1.0, 0.0, 0.0}, - .velocity = {0.0, 0.0, 0.0}, - .position = {0.0, 0.0, 0.0}, - .mode = dcont::ModeInput::Rotation}); + auto inp = dcont::Input{.joystick = {.throttle_input = 0.5, + .roll_input = cont_input.rx, + .yaw_input = cont_input.lx, + .pitch_input = cont_input.ry}, + .acceleration = {0.0, 0.0, 0.0}, + .rotation = {0.0, 0.0, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {0.0, 0.0, 0.0}, + .mode = dcont::ModeInput::Rotation}; + + dcont::set_input(drone_controller, inp); + + this->last_input = inp; } else { drone_cont_stabilize(); } diff --git a/main/main.cpp b/main/main.cpp index 355bf0d..5a1108d 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -3,13 +3,15 @@ #include "drone.h" #include "drone_comms.h" #include "drone_controller.h" -#include "esp32-hal.h" #include "esp_log.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" #include -#include #include +#include +#include #include #include "env_sens.h" @@ -49,6 +51,29 @@ extern "C" void app_main(void) { xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0); + // vTaskDelay(5000); + // for (int i = 0; i < 4; i++) { + // motor_throttles[i] = 10.0; + // vTaskDelay(2000); + // motor_throttles[i] = 0.0; + // } + + xTaskCreate( + [](void *pvParameters) { + while (true) { + while (packet_rx_queue && + xQueueReceive(packet_rx_queue, &packet_data[0], 20)) { + handle_packet(&packet_data[0]); + } + vTaskDelay(10); + } + }, + "lambda_recv_task", 8192, NULL, 5, NULL); + + setup_imu(); + + vTaskDelay(10); + xTaskCreatePinnedToCore(motor_throttles_task, // Function name "motor_throttles_task", // Name for debugging 1024 * 4, // Stack size in bytes @@ -68,27 +93,6 @@ extern "C" void app_main(void) { NULL, // Task handle 0 // Core ID ); - - // vTaskDelay(5000); - // for (int i = 0; i < 4; i++) { - // motor_throttles[i] = 10.0; - // vTaskDelay(2000); - // motor_throttles[i] = 0.0; - // } - - xTaskCreate( - [](void *pvParameters) { - while (true) { - while (packet_rx_queue && - xQueueReceive(packet_rx_queue, &packet_data[0], 20)) { - handle_packet(&packet_data[0]); - } - vTaskDelay(1); - } - }, - "lambda_recv_task", 8192, NULL, 5, NULL); - - setup_imu(); servo_init(); ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); @@ -120,7 +124,7 @@ extern "C" void app_main(void) { if (nav_mutex && xSemaphoreTake(nav_mutex, 10)) { - if (gps->gps_avaliable()) { + if (gps && gps->gps_avaliable()) { waypoint current_wayp = nav_man.get_current_waypoint(); auto pos = sens_fus.position; @@ -211,8 +215,9 @@ extern "C" void app_main(void) { if (motor_throttles != nullptr) { - ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0], - motor_throttles[1], motor_throttles[2], motor_throttles[3]); + ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f), kill: %d", + motor_throttles[0], motor_throttles[1], motor_throttles[2], + motor_throttles[3], std::atomic_load(&killswitch_active)); } if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) {