diff --git a/main/drone.cpp b/main/drone.cpp index c3eb8e8..dba9f22 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -94,11 +94,11 @@ void drone_controller_task(void *params) { Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero(); while (true) { - if (xSemaphoreTake(imu_state_mutex, 1)) { + if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) { imu_state_local = imu_state_var; xSemaphoreGive(imu_state_mutex); } - if (xSemaphoreTake(sens_fus_mutex, 1)) { + if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) { position_local = sens_fus.position; velocity_local = sens_fus.velocity; xSemaphoreGive(sens_fus_mutex); @@ -165,6 +165,7 @@ void motor_throttles_task(void *params) { } } + uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY; while (true) { for (int i = 0; i < 4; i++) { if (!killswitch_active) { diff --git a/main/env_sens.cpp b/main/env_sens.cpp index dd6cdd3..28666ce 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -4,6 +4,16 @@ #include #include +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include + #include "freertos/idf_additions.h" #include "sens_fus.h" diff --git a/main/gps.cpp b/main/gps.cpp index 71c3941..f73402e 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -14,6 +14,7 @@ void gps_poll_task(void *_) { while (true) { bool has_new_data = false; + bool available = false; Eigen::Vector2f local_vel = Eigen::Vector2f::Zero(); std::optional local_coords; @@ -21,6 +22,7 @@ void gps_poll_task(void *_) { gps->poll(); if (gps->gps_avaliable()) { + available = true; local_vel = gps->velocity().value_or(Eigen::Vector2f::Zero()); local_coords = gps->get_coordinates(); } @@ -31,18 +33,20 @@ void gps_poll_task(void *_) { ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX"); } - if (has_new_data && sens_fus_mutex) { + if (has_new_data && available && sens_fus_mutex) { if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) { sens_fus.measure_gps( 1.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); } else { - ESP_LOGD(TAG, "Sens_fus busy, skipping GPS update."); + ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update."); } } - vTaskDelay(pdMS_TO_TICKS(50)); // 10Hz polling + vTaskDelay(pdMS_TO_TICKS(100)); // 10Hz polling } } diff --git a/main/gps.h b/main/gps.h index 5ce8b7c..ab4f8fa 100644 --- a/main/gps.h +++ b/main/gps.h @@ -83,15 +83,12 @@ struct GPS { } void poll() { - while (this->gps->read()) { - if (this->gps->newNMEAreceived()) { - char *line = this->gps->lastNMEA(); - // ESP_LOGI("GPS", "NMEA LINE: %s", line); - if (!this->gps->parse(line)) { - continue; - } - } - } + // 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 799ef91..9dbd4c2 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -1,7 +1,6 @@ #include "imu.h" #include "esp_log.h" #include "esp_timer.h" -#include "freertos/idf_additions.h" #include "hal/spi_types.h" #include "sens_fus.h" @@ -15,10 +14,13 @@ #include +#include "freertos/idf_additions.h" + static const char *TAG = "IMU"; void setup_imu() { imu_state *local_state = new imu_state; + imu_state_mutex = xSemaphoreCreateMutex(); BNO08x *imu = new BNO08x( bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25, @@ -33,10 +35,12 @@ void setup_imu() { imu->rpt.rv_game.enable(2500UL); imu->rpt.linear_accelerometer.enable(2500UL); - imu->rpt.cal_gyro.enable(1000UL); + imu->rpt.cal_gyro.enable(2500UL); imu->register_cb([imu, local_state]() { bool needs_updating = false; + if (sens_fus_mutex == NULL || imu_state_mutex == NULL) + return; if (imu->rpt.rv_game.has_new_data()) { @@ -71,9 +75,16 @@ void setup_imu() { dcont::Vec3C accel_global = dcont::apply_rot(&local_state->accel, &local_state->rot); - if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) { + if (xSemaphoreTake(sens_fus_mutex, (TickType_t)0) == pdTRUE) { + + 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()); xSemaphoreGive(sens_fus_mutex); } else { ESP_LOGE(TAG, "Failed to get sens_fus mutex."); @@ -83,7 +94,7 @@ void setup_imu() { } if (needs_updating) { - if (xSemaphoreTake(imu_state_mutex, 2)) { + if (xSemaphoreTake(imu_state_mutex, 0)) { imu_state_var = *local_state; xSemaphoreGive(imu_state_mutex); } diff --git a/main/main.cpp b/main/main.cpp index 5d697b7..cf2406c 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,5 +1,14 @@ +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + #include "Eigen/Core" + #include "driver/gpio.h" #include "drone.h" #include "drone_comms.h" @@ -9,7 +18,6 @@ #include "freertos/idf_additions.h" #include "freertos/projdefs.h" #include "freertos/task.h" -#include #include #include #include @@ -33,8 +41,6 @@ extern "C" void app_main(void) { gpio_install_isr_service(0); Serial.begin(115200); - setup_imu(); - xTaskCreatePinnedToCore(radio_task, // Function name "radio_rxtx", // Name for debugging 4096, // Stack size in bytes @@ -43,11 +49,11 @@ extern "C" void app_main(void) { NULL, // Task handle 0 // Core ID ); - + // xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL); - + // 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 @@ -57,28 +63,32 @@ extern "C" void app_main(void) { 1 // Core ID ); - xTaskCreatePinnedToCore(motor_throttles_task, // Function name - "motor_throttles_task", // Name for debugging - 1024 * 4, // 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(); + 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; while (true) { - while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { + while (packet_tx_queue && + xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { handle_packet(&packet_data[0]); } if (millis() > last_print_time + 1000) { + last_print_time = millis(); std::optional coords; float lat, lon, alt; diff --git a/main/nav.h b/main/nav.h index 3852e15..fc710c2 100644 --- a/main/nav.h +++ b/main/nav.h @@ -1,12 +1,5 @@ #pragma once -#include "Eigen/Core" -#include "freertos/idf_additions.h" -#include "gps.h" -#include -#include -#include - #ifdef PS #undef PS #endif @@ -17,6 +10,12 @@ #include +#include "freertos/idf_additions.h" +#include "gps.h" +#include +#include +#include + #define WAYPOINT_COUNT 8 struct waypoint { diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index 231346d..32c9797 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -1,6 +1,14 @@ -#include "packet_handler.h" + +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif #include "Eigen/Core" + #include "drone.h" #include "drone_comms.h" #include "drone_controller.h" @@ -8,6 +16,7 @@ #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 1192fb4..e8678b8 100644 --- a/main/packet_handler.h +++ b/main/packet_handler.h @@ -2,8 +2,17 @@ #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 5e06a12..d0cbdce 100644 --- a/main/radio.cpp +++ b/main/radio.cpp @@ -1,9 +1,20 @@ #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" #include "freertos/idf_additions.h" -#include +#include "packet_handler.h" #include #include #include diff --git a/main/sens_fus.h b/main/sens_fus.h index 9660d12..0a43d9f 100644 --- a/main/sens_fus.h +++ b/main/sens_fus.h @@ -1,5 +1,4 @@ #pragma once -#include "freertos/idf_additions.h" #include #ifdef PS @@ -12,6 +11,8 @@ #include +#include "freertos/idf_additions.h" + inline float getYawDifference(const Eigen::Vector3f &v_gps, const Eigen::Vector3f &v_imu) { float yaw_gps = std::atan2(v_gps.y(), v_gps.x()); @@ -41,8 +42,7 @@ struct sens_fus_compl { float tau_yaw = 10.0f; // Yaw remains a scalar - Eigen::Matrix3f - yaw_rotation_matrix; // Pre-compute this when yaw_offset changes + Eigen::Matrix3f yaw_rotation_matrix; void update_yaw_matrix() { yaw_rotation_matrix =