From 3da916d1aa05b4fe0efcf115eeddd6cde32d82a1 Mon Sep 17 00:00:00 2001 From: franchioping Date: Tue, 7 Apr 2026 10:48:44 +0100 Subject: [PATCH] working nav kinda --- main/drone.cpp | 2 +- main/drone.h | 11 ++++--- main/env_sens.cpp | 16 ++++------ main/env_sens.h | 10 +++++++ main/gps.cpp | 13 +++++--- main/gps.h | 22 ++++++++------ main/imu.cpp | 21 +++++++------ main/main.cpp | 66 +++++++++++++++++++++++------------------ main/nav.h | 11 ------- main/packet_handler.cpp | 12 +------- main/packet_handler.h | 9 ------ main/radio.cpp | 11 +------ main/sens_fus.h | 23 +++++++------- 13 files changed, 106 insertions(+), 121 deletions(-) diff --git a/main/drone.cpp b/main/drone.cpp index dba9f22..449f6c1 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -148,7 +148,7 @@ void drone_controller_task(void *params) { } } -const gpio_num_t motor_pins[4] = {GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC, +const gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC}; void motor_throttles_task(void *params) { diff --git a/main/drone.h b/main/drone.h index 82ca379..da3c505 100644 --- a/main/drone.h +++ b/main/drone.h @@ -1,9 +1,5 @@ #pragma once -#include "Eigen/Core" -#include "drone_comms.h" -#include "drone_controller.h" - #ifdef PS #undef PS #endif @@ -14,16 +10,19 @@ #include +#include "drone_comms.h" +#include "drone_controller.h" + 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; +inline dcont::ModeInput current_input_mode = dcont::ModeInput::Acro; void drone_controller_task(void *params); void motor_throttles_task(void *params); -inline float motor_throttles[4]; +inline float motor_throttles[4] = {0.5, 0.5, 0.5, 0.5}; inline bool killswitch_active; diff --git a/main/env_sens.cpp b/main/env_sens.cpp index 28666ce..690f3d4 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -1,21 +1,13 @@ #include "env_sens.h" + +#include "sens_fus.h" + #include "esp_log.h" #include #include #include -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - -#include - #include "freertos/idf_additions.h" -#include "sens_fus.h" #define SEALEVELPRESSURE_HPA (1030) @@ -34,6 +26,8 @@ void setup() { if (!bme.begin()) { ESP_LOGE(TAG, "Couldn't find a valid sensor"); + + ESP.restart(); return; } ESP_LOGI(TAG, "BARO SETUP COMPLETE."); diff --git a/main/env_sens.h b/main/env_sens.h index c66dafd..62f8a93 100644 --- a/main/env_sens.h +++ b/main/env_sens.h @@ -1,5 +1,15 @@ #pragma once +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include + #include "freertos/idf_additions.h" namespace env_sens { diff --git a/main/gps.cpp b/main/gps.cpp index f73402e..3d3fad9 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -1,7 +1,9 @@ #include "gps.h" -#include "Eigen/Core" + +#include "esp32-hal.h" #include "esp_log.h" #include "sens_fus.h" +#include static const char *TAG = "GPS_TASK"; @@ -12,6 +14,7 @@ void gps_poll_task(void *_) { ESP_LOGI(TAG, "GPS TASK INIT."); + uint64_t last_time = millis(); while (true) { bool has_new_data = false; bool available = false; @@ -34,19 +37,21 @@ void gps_poll_task(void *_) { } if (has_new_data && available && sens_fus_mutex) { + + uint64_t current_time = millis(); if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) { sens_fus.measure_gps( - 1.0f, local_coords.value(), + (current_time - last_time) / 1000.0f, local_coords.value(), Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f)); - ESP_LOGI(TAG, "applied gps measure to sens_fus"); xSemaphoreGive(sens_fus_mutex); + last_time = current_time; } else { ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update."); } } - vTaskDelay(pdMS_TO_TICKS(100)); // 10Hz polling + vTaskDelay(pdMS_TO_TICKS(10)); // 10Hz polling } } diff --git a/main/gps.h b/main/gps.h index ab4f8fa..cb7d097 100644 --- a/main/gps.h +++ b/main/gps.h @@ -56,7 +56,12 @@ struct GPS { std::optional waypoint_to_xyz(float latitude, float longitude, float height) { if (this->origin_lat == INFINITY || this->origin_long == INFINITY) { - return std::nullopt; + if (this->gps_avaliable()) { + this->origin_lat = this->gps->latitudeDegrees; + this->origin_long = this->gps->longitudeDegrees; + } else { + return std::nullopt; + } } float dLat = (latitude - this->origin_lat) * TO_RAD; @@ -72,8 +77,7 @@ struct GPS { } std::optional get_coordinates() { - if (this->gps->fix == false || (this->gps->latitudeDegrees == 0.0 && - this->gps->longitudeDegrees == 0.0)) { + if (this->gps->fix == false) { return std::nullopt; } float latitude = this->gps->latitudeDegrees; @@ -83,12 +87,12 @@ struct GPS { } void poll() { - // char c = this->gps->read(); - // if (this->gps->newNMEAreceived()) { - // char *line = this->gps->lastNMEA(); - // ESP_LOGI("GPS", "NMEA LINE: %s", line); - // this->gps->parse(line); - // } + char c = this->gps->read(); + if (this->gps->newNMEAreceived()) { + char *line = this->gps->lastNMEA(); + // ESP_LOGI("GPS", "NMEA LINE: %s", line); + this->gps->parse(line); + } } }; diff --git a/main/imu.cpp b/main/imu.cpp index 9dbd4c2..470dfa4 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -1,8 +1,5 @@ #include "imu.h" -#include "esp_log.h" -#include "esp_timer.h" -#include "hal/spi_types.h" -#include "sens_fus.h" +#include "Esp.h" #ifdef PS #undef PS @@ -14,6 +11,11 @@ #include +#include "esp_log.h" +#include "esp_timer.h" +#include "hal/spi_types.h" +#include "sens_fus.h" + #include "freertos/idf_additions.h" static const char *TAG = "IMU"; @@ -28,6 +30,7 @@ void setup_imu() { if (!imu->initialize()) { ESP_LOGE(TAG, "BNO08x Init failure."); + ESP.restart(); } imu->dynamic_calibration_autosave_enable(); @@ -75,16 +78,16 @@ void setup_imu() { dcont::Vec3C accel_global = dcont::apply_rot(&local_state->accel, &local_state->rot); - if (xSemaphoreTake(sens_fus_mutex, (TickType_t)0) == pdTRUE) { + if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) { - ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x, - accel_global.y, accel_global.z, dt); + // ESP_LOGE(TAG, "accel: (%f, %f, %f), dt: %f", accel_global.x, + // accel_global.y, accel_global.z, dt); sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y, accel_global.z)); - ESP_LOGE(TAG, "vel: (%f, %f, %f)", sens_fus.velocity.x(), - sens_fus.velocity.z(), sens_fus.velocity.y()); + // ESP_LOGE(TAG, "vel: (%f, %f, %f)", sens_fus.velocity.x(), + // sens_fus.velocity.z(), sens_fus.velocity.y()); xSemaphoreGive(sens_fus_mutex); } else { ESP_LOGE(TAG, "Failed to get sens_fus mutex."); diff --git a/main/main.cpp b/main/main.cpp index cf2406c..3538de2 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,14 +1,3 @@ - -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - -#include "Eigen/Core" - #include "driver/gpio.h" #include "drone.h" #include "drone_comms.h" @@ -17,8 +6,7 @@ #include "freertos/FreeRTOS.h" #include "freertos/idf_additions.h" #include "freertos/projdefs.h" -#include "freertos/task.h" -#include +#include "freertos/task.h" #include < cstddef> #include #include @@ -35,6 +23,7 @@ static const char *TAG = "MAIN"; extern "C" void app_main(void) { sens_fus_mutex = xSemaphoreCreateMutex(); + configASSERT(sens_fus_mutex); nav_mutex = xSemaphoreCreateMutex(); initArduino(); @@ -54,24 +43,24 @@ extern "C" void app_main(void) { // xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL); // - 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 - ); - - // xTaskCreatePinnedToCore(motor_throttles_task, // Function name - // "motor_throttles_task", // Name for debugging - // 1024 * 4, // Stack size in bytes - // NULL, // Parameters - // 30, // Priority (higher = more urgent) + // 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 // ); + xTaskCreatePinnedToCore(motor_throttles_task, // Function name + "motor_throttles_task", // Name for debugging + 1024 * 4, // Stack size in bytes + NULL, // Parameters + 30, // Priority (higher = more urgent) + NULL, // Task handle + 1 // Core ID + ); + ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); setup_imu(); @@ -92,19 +81,31 @@ extern "C" void app_main(void) { std::optional coords; float lat, lon, alt; - if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { + bool gps_values = false; + bool fix = false; + uint8_t sat_count = 0; + if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) { if (gps->gps_avaliable()) { coords = gps->get_coordinates(); lat = gps->gps->latitudeDegrees; lon = gps->gps->longitudeDegrees; alt = gps->gps->altitude; + gps_values = true; } + sat_count = gps->gps->satellites; + fix = gps->gps->fix; xSemaphoreGive(gps_mutex); + if (gps_values) { + + ESP_LOGI(TAG, + "loc -> lat: %f, long: %f, height: %f, sat_c: %d, fix: %b", + lat, lon, alt, sat_count, fix); + } + 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]); } @@ -125,6 +126,13 @@ extern "C" void app_main(void) { ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1], local_vel[2]); } + + ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0], + motor_throttles[1], motor_throttles[2], motor_throttles[3]); + + ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)", + current_controller_input.lx, current_controller_input.ly, + current_controller_input.rx, current_controller_input.ry); } vTaskDelay(pdMS_TO_TICKS(10)); diff --git a/main/nav.h b/main/nav.h index fc710c2..09eb806 100644 --- a/main/nav.h +++ b/main/nav.h @@ -1,15 +1,4 @@ #pragma once - -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - -#include - #include "freertos/idf_additions.h" #include "gps.h" #include diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index 32c9797..60298e4 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -1,13 +1,4 @@ - -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - -#include "Eigen/Core" +#include "packet_handler.h" #include "drone.h" #include "drone_comms.h" @@ -16,7 +7,6 @@ #include "freertos/idf_additions.h" #include "gps.h" #include "nav.h" -#include "packet_handler.h" #include "portmacro.h" #include "radio.h" #include "sens_fus.h" diff --git a/main/packet_handler.h b/main/packet_handler.h index e8678b8..1192fb4 100644 --- a/main/packet_handler.h +++ b/main/packet_handler.h @@ -2,17 +2,8 @@ #include -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - #include "drone_comms.h" #include "freertos/idf_additions.h" -#include void handle_packet(uint8_t *packet_addr); diff --git a/main/radio.cpp b/main/radio.cpp index d0cbdce..be7682e 100644 --- a/main/radio.cpp +++ b/main/radio.cpp @@ -1,15 +1,5 @@ #include "radio.h" -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - -#include - #include "Esp.h" #include "esp32-hal-gpio.h" #include "esp_log.h" @@ -68,6 +58,7 @@ void radio_task(void *pvParameters) { radio.setHighPower(true); radio.setCustomBitrate(DEFAULT_COMMS_BITRATE); ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10)); + } else { ESP_LOGE(TAG, "Radio Init FAILED! Restarting."); ESP.restart(); diff --git a/main/sens_fus.h b/main/sens_fus.h index 0a43d9f..bd15742 100644 --- a/main/sens_fus.h +++ b/main/sens_fus.h @@ -34,15 +34,15 @@ 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, 10.0f}; - Eigen::Vector3f tau_gps_vel = {5.0f, 5.0f, INFINITY}; + Eigen::Vector3f tau_gps_pos = {20.0f, 20.0f, INFINITY}; + Eigen::Vector3f tau_gps_vel = {40.0f, 40.0f, INFINITY}; - Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f}; - Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f}; + Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f}; + Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f}; - float tau_yaw = 10.0f; // Yaw remains a scalar + float tau_yaw = 20.0f; // Yaw remains a scalar - Eigen::Matrix3f yaw_rotation_matrix; + Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval(); void update_yaw_matrix() { yaw_rotation_matrix = @@ -81,12 +81,13 @@ struct sens_fus_compl { this->yaw_offset = std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset)); this->update_yaw_matrix(); - } - // res = (1 - alpha) * state + alpha * measurement - this->velocity = - (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() + - alpha_vel.array() * gps_vel.array(); + this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() * + this->velocity.array() + + alpha_vel.array() * gps_vel.array(); + } else if (this->velocity.norm() > 1.0) { + this->velocity *= 1 - (0.90 * dt); + } } void measure_baro(float dt, Eigen::Vector3f baro_pos,