From a64795bce0d7cbc575cfa09f3557d9b28f70087c Mon Sep 17 00:00:00 2001 From: franchioping Date: Tue, 14 Apr 2026 16:00:52 +0100 Subject: [PATCH] i dont even know atp, everything is kinda working lwk just polishing and fixing nav/sens fusion. TODO: Automatic stabilization and landing sequence --- components/drone_comms | 2 +- main/drone.cpp | 91 ++++++++++++++++++++++------------------- main/drone.h | 4 +- main/env_sens.cpp | 32 +++++++++------ main/env_sens.h | 7 ++++ main/imu.cpp | 15 +++++-- main/main.cpp | 12 ++++-- main/packet_handler.cpp | 35 +++++++++------- main/packet_handler.h | 5 +++ main/sens_fus.h | 11 ++++- 10 files changed, 135 insertions(+), 79 deletions(-) diff --git a/components/drone_comms b/components/drone_comms index 1fee7c4..350c761 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit 1fee7c4e5d9367a90a259a0ecbc751ae5f53e9b3 +Subproject commit 350c761b9fe11f551e5c8ddf11e84252d2f496b4 diff --git a/main/drone.cpp b/main/drone.cpp index be64755..11fe2ba 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -8,6 +8,7 @@ #include "dshot_definitions.h" #include "esp32-hal.h" +#include "esp_log.h" #include "freertos/FreeRTOS.h" #include "freertos/idf_additions.h" #include "freertos/projdefs.h" @@ -20,6 +21,7 @@ #include #include #include +#include #define CONTROLLER_TASK_FREQUENCY 1000.0; @@ -31,45 +33,43 @@ dcont::ControllerConfig default_config() { // 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 - 5.0f // frequency (Hz) + .kp = {1.0f, 1.0f, 1.0f}, // kp + .ki = {0.0f, 0.0f, 0.0f}, // ki + .kd = {0.0f, 0.0f, 0.0f}, // kd + .frequency = 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}, 15.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}, 50.0f}; + // Velocity Loop (Velocity -> Acceleration/Rotation) + config.stack.linvel_pid = {.kp = {1.0f, 1.0f, 1.0f}, + .ki = {0.0f, 0.0f, 0.0f}, + .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}; // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop - config.stack.rate_pid = {{0.1f, 0.1f, 1.0f}, - {0.01f, 0.01f, 0.01f}, - {0.001f, 0.001f, 0.001f}, - 100.0f}; + 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}; // 2. Set Constraints - config.stack.max_rate = 3.14f; // ~180 degrees/s - config.stack.max_linvel = 10.0f; // 10 m/s + config.stack.max_rate = 3.14f; // ~180 degrees/s + config.stack.max_linvel = 3.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 - {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 - */ 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 + // roll, pitch, yaw + {-1.0, -1.0, -1.0}, // Front Left + {1.0, 1.0, -1.0}, // Rear Right + {-1.0, 1.0, 1.0}, // Rear Left + {1.0, -1.0, 1.0}, // Front Right }; for (int i = 0; i < 4; i++) { @@ -90,9 +90,10 @@ void drone_cont_stabilize() { // afterwards } +constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY; + 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(); @@ -118,6 +119,9 @@ void drone_controller_task(void *params) { packet_controller_input cont_input; if (current_input_mode == dcont::ModeInput::Acro && xSemaphoreTake(controller_input_semaphore, 10)) { + if (millis() - time_last_controller > 1) { + current_controller_input = {0, 0, 0, 0}; + } cont_input = current_controller_input; xSemaphoreGive(controller_input_semaphore); @@ -136,18 +140,21 @@ void drone_controller_task(void *params) { } else { auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero()); - dcont::set_input(drone_controller, - dcont::Input{{cont_input.ly, cont_input.lx, - cont_input.rx, cont_input.ry}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0}, - {coords.x(), coords.y(), coords.z()}, - current_input_mode}); + 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 = {0.0, 0.0, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {coords.x(), coords.y(), coords.z()}, + .mode = current_input_mode}); } - memcpy(dcont::get_throttles(drone_controller).values, motor_throttles, - sizeof(motor_throttles)); + // memcpy(dcont::get_throttles(drone_controller).values, motor_throttles, + // sizeof(motor_throttles)); vTaskDelay(pdMS_TO_TICKS(wait_ms)); } @@ -156,12 +163,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 bool reversed[4] = {false, true, false, false}; - 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; motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false); motors[i]->begin(); } @@ -172,13 +179,13 @@ void motor_throttles_task(void *params) { for (int i = 0; i < 4; i++) { motors[i]->sendThrottlePercent(0); } - vTaskDelay(1); + vTaskDelay(2); } while (true) { for (int i = 0; i < 4; i++) { motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f); } - vTaskDelay(1); + vTaskDelay(2); } } diff --git a/main/drone.h b/main/drone.h index 42a715f..f368f1d 100644 --- a/main/drone.h +++ b/main/drone.h @@ -1,5 +1,7 @@ #pragma once +#include "esp32-hal.h" +#include #ifdef PS #undef PS #endif @@ -25,5 +27,5 @@ void drone_controller_task(void *params); void motor_throttles_task(void *params); // 3 4 2 1 -inline float motor_throttles[4] = {0.01, 0.01, 0.01, 0.01}; +inline float *motor_throttles; inline bool killswitch_active = false; diff --git a/main/env_sens.cpp b/main/env_sens.cpp index 0fa5014..eae2bf8 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -48,27 +48,35 @@ void setup() { } float get_temperature() { - sensors_event_t temp_event; - if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) { + if (millis() - env_sens::time_term_read > 20) { - bme_temp->getEvent(&temp_event); - xSemaphoreGive(baro_mutex); + sensors_event_t temp_event; + if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) { + + bme_temp->getEvent(&temp_event); + xSemaphoreGive(baro_mutex); + } + + return temp_event.temperature; } - - return temp_event.temperature; + return env_sens::term_read; } float get_pressure() { - sensors_event_t e; - if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) { + if (millis() - env_sens::time_baro_read > 20) { - bme_pressure->getEvent(&e); - xSemaphoreGive(baro_mutex); + sensors_event_t e; + if (baro_mutex && xSemaphoreTake(baro_mutex, 30)) { + + bme_pressure->getEvent(&e); + xSemaphoreGive(baro_mutex); + } + + return e.pressure; } - - return e.pressure; + return env_sens::baro_read; } float calculateAltitude(float pressure, float seaLevelPressure, diff --git a/main/env_sens.h b/main/env_sens.h index 62f8a93..e33158e 100644 --- a/main/env_sens.h +++ b/main/env_sens.h @@ -1,5 +1,6 @@ #pragma once +#include #ifdef PS #undef PS #endif @@ -24,6 +25,12 @@ void dbg_sens(); float get_altitude(); void baro_poll_task(void *_); +inline uint64_t time_term_read = 0; +inline uint64_t time_baro_read = 0; + +inline float term_read = 0; +inline float baro_read = 0; + } // namespace env_sens inline SemaphoreHandle_t baro_mutex = NULL; diff --git a/main/imu.cpp b/main/imu.cpp index ed4d7da..8603dd7 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -36,10 +36,12 @@ void setup_imu() { new BNO08x(bno08x_config_t(SPI2_HOST, IMU_MOSI, IMU_MISO, IMU_SCLK, IMU_CS, IMU_INT, IMU_RST, 2000000, false)); + ESP_LOGI(TAG, "INITIALIZING IMU"); if (!imu->initialize()) { ESP_LOGE(TAG, "BNO08x Init failure."); ESP.restart(); } + imu->print_product_ids(); imu->dynamic_calibration_autosave_enable(); imu->dynamic_calibration_enable(BNO08xCalSel::all); @@ -49,6 +51,7 @@ void setup_imu() { imu->rpt.cal_gyro.enable(2500UL); imu->register_cb([imu, local_state]() { + ESP_LOGI("IMU", "CALLBACK"); bool needs_updating = false; if (sens_fus_mutex == NULL || imu_state_mutex == NULL) return; @@ -59,8 +62,12 @@ void setup_imu() { auto sens_rot = imu->rpt.rv_game.get_quat(); auto sens_euler = imu->rpt.rv_game.get_euler(); - local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real}; - local_state->rot_euler = {sens_euler.x, sens_euler.y, sens_euler.z}; + local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, + sens_rot.real}; // FIXME: WRONG ROTATION + local_state->rot_euler = {sens_euler.x, -sens_euler.y, -sens_euler.z}; + ESP_LOGI("IMU", "rot: roll %f, pitch %f, yaw %f", + local_state->rot_euler.x(), local_state->rot_euler.y(), + local_state->rot_euler.z()); } if (imu->rpt.cal_gyro.has_new_data()) { @@ -68,7 +75,7 @@ void setup_imu() { needs_updating = true; auto cal_gyro = imu->rpt.cal_gyro.get(); - local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z}; + local_state->angvel = {cal_gyro.x, -cal_gyro.y, -cal_gyro.z}; } if (imu->rpt.linear_accelerometer.has_new_data()) { @@ -76,7 +83,7 @@ void setup_imu() { needs_updating = true; auto sens_accel = imu->rpt.linear_accelerometer.get(); - local_state->accel = {sens_accel.x, sens_accel.y, sens_accel.z}; + local_state->accel = {sens_accel.x, -sens_accel.y, -sens_accel.z}; int64_t current_time = esp_timer_get_time(); if (local_state->last_time != -1) { diff --git a/main/main.cpp b/main/main.cpp index de0de9b..7b3636a 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -63,21 +63,27 @@ extern "C" void app_main(void) { 1 // Core ID ); - ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); - // setup_imu(); + ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); Eigen::Vector3f local_pos = {0, 0, 0}; Eigen::Vector3f local_vel = {0, 0, 0}; bool nav_data_ready = false; uint64_t last_print_time = 0; + uint64_t last_broadcast_time = 0; + while (true) { while (packet_tx_queue && xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { handle_packet(&packet_data[0]); } + if (millis() > last_broadcast_time + 100) { + send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION); + last_broadcast_time = millis(); + } + if (millis() > last_print_time + 5000) { last_print_time = millis(); @@ -137,6 +143,6 @@ extern "C" void app_main(void) { current_controller_input.rx, current_controller_input.ry); } - vTaskDelay(pdMS_TO_TICKS(10)); + vTaskDelay(pdMS_TO_TICKS(50)); } } diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index 60298e4..597a97d 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -3,14 +3,18 @@ #include "drone.h" #include "drone_comms.h" #include "drone_controller.h" +#include "env_sens.h" +#include "esp32-hal.h" #include "esp_log.h" #include "freertos/idf_additions.h" #include "gps.h" +#include "imu.h" #include "nav.h" #include "portmacro.h" #include "radio.h" #include "sens_fus.h" #include +#include #ifdef PS #undef PS @@ -22,15 +26,15 @@ #include -void handle_request(uint8_t *packet_addr); - void handle_waypoint_update(uint8_t *packet_addr, uint8_t index); void handle_nav_update(uint8_t *packet_addr); void handle_packet(uint8_t *packet_addr) { PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr); if (packet_type == PACKET_TYPE::COMMAND_REQUEST) { - handle_request(packet_addr); + packet_command_request *packet = + (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE)); + send_packet_getter(packet->packet_requested); } // NAV SETTERS @@ -64,6 +68,7 @@ void handle_packet(uint8_t *packet_addr) { if (xSemaphoreTake(controller_input_semaphore, 10)) { current_controller_input = *packet; + time_last_controller = millis(); xSemaphoreGive(controller_input_semaphore); } } @@ -94,23 +99,23 @@ void handle_nav_update(uint8_t *packet_addr) { } } -void handle_request(uint8_t *packet_addr) { +void send_packet_getter(PACKET_TYPE requested_type) { - packet_command_request *packet = - (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE)); - PACKET_TYPE requested_type = packet->packet_requested; std::pair resp_packet = {nullptr, 0}; if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) { // TODO: send pitch, roll, yaw - resp_packet = - create_packet_pooled(PACKET_TYPE::INFO_DRONE_POSITION, - packet_info_drone_position{ - {sens_fus.position.x(), sens_fus.position.y(), - sens_fus.position.z()}, - {sens_fus.velocity.x(), sens_fus.velocity.y(), - sens_fus.velocity.z()}, - {0.0, 0.0, 0.0}}); + resp_packet = create_packet_pooled( + PACKET_TYPE::INFO_DRONE_POSITION, + packet_info_drone_position{ + .trans = {sens_fus.position.x(), sens_fus.position.y(), + sens_fus.position.z()}, + .vel = {sens_fus.velocity.x(), sens_fus.velocity.y(), + sens_fus.velocity.z()}, + .rot = {imu_state_var.rot_euler.x(), imu_state_var.rot_euler.y(), + imu_state_var.rot_euler.z()}, + .pressure = env_sens::get_pressure(), + .temperature = env_sens::get_temperature()}); } if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) { diff --git a/main/packet_handler.h b/main/packet_handler.h index 1192fb4..26b401f 100644 --- a/main/packet_handler.h +++ b/main/packet_handler.h @@ -3,9 +3,14 @@ #include #include "drone_comms.h" +#include "esp32-hal.h" #include "freertos/idf_additions.h" void handle_packet(uint8_t *packet_addr); +void send_packet_getter(PACKET_TYPE requested_type); + inline SemaphoreHandle_t controller_input_semaphore = NULL; inline packet_controller_input current_controller_input; + +inline uint64_t time_last_controller = 0; diff --git a/main/sens_fus.h b/main/sens_fus.h index bd15742..64ca11d 100644 --- a/main/sens_fus.h +++ b/main/sens_fus.h @@ -1,4 +1,5 @@ #pragma once +#include "Eigen/Core" #include #ifdef PS @@ -25,6 +26,7 @@ inline float getYawDifference(const Eigen::Vector3f &v_gps, struct sens_fus_compl { Eigen::Vector3f position = Eigen::Vector3f::Zero(); Eigen::Vector3f velocity = Eigen::Vector3f::Zero(); + Eigen::Vector3f velocity_last_gps_measurment = Eigen::Vector3f::Zero(); Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero(); float yaw_offset = 0.0f; @@ -75,7 +77,12 @@ struct sens_fus_compl { // 2. Yaw Correction (only if moving > 1.0 m/s) if (gps_vel.norm() > 1.0f) { - float yaw_delta = getYawDifference(gps_vel, this->velocity); + Eigen::Vector3f delta_v_imu = + this->velocity - this->velocity_last_gps_measurment; + Eigen::Vector3f delta_v_gps = this->velocity - gps_vel; + + float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu); + this->yaw_offset += yaw_delta * alpha_yaw; this->yaw_offset = @@ -88,6 +95,8 @@ struct sens_fus_compl { } else if (this->velocity.norm() > 1.0) { this->velocity *= 1 - (0.90 * dt); } + + this->velocity_last_gps_measurment = this->velocity; } void measure_baro(float dt, Eigen::Vector3f baro_pos,