From 5be19b3f11d00674c32daf9fefd36344502bd7b7 Mon Sep 17 00:00:00 2001 From: franchioping Date: Mon, 30 Mar 2026 22:09:23 +0100 Subject: [PATCH] idk if working since azores --- components/drone_comms | 2 +- main/CMakeLists.txt | 2 +- main/env_sens.cpp | 67 ++++++++++++++++++++++--- main/env_sens.h | 4 ++ main/gps.cpp | 36 +++++++++++++ main/gps.h | 51 +++++++++++-------- main/imu.cpp | 63 +++++++++++------------ main/imu.h | 7 --- main/main.cpp | 111 ++++++++++++++++++++++++++++------------- main/nav.h | 98 ++++++++++++++++++++++++++++++++++++ main/pos.h | 65 ------------------------ main/radio.cpp | 92 ++++++++++++++++++++++++++++++++++ main/radio.h | 10 ++++ 13 files changed, 435 insertions(+), 173 deletions(-) create mode 100644 main/gps.cpp create mode 100644 main/nav.h delete mode 100644 main/pos.h create mode 100644 main/radio.cpp create mode 100644 main/radio.h diff --git a/components/drone_comms b/components/drone_comms index e93d423..1a1fccc 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit e93d423065d1f100fbd9da05383ab555261aa6d4 +Subproject commit 1a1fccca83a4a24670715efa4b532d8a7c4e51ca diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index b9b72d5..5ddbe79 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,7 +1,7 @@ file(GLOB_RECURSE SOURCES "*.cpp") idf_component_register(SRCS "${SOURCES}" - INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab) + INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab drone_comms) target_compile_options(${COMPONENT_LIB} PRIVATE "-Wno-gnu-array-member-paren-init" diff --git a/main/env_sens.cpp b/main/env_sens.cpp index 951e02f..2f8ad9d 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -1,30 +1,35 @@ +#include "env_sens.h" #include "esp_log.h" #include #include #include -#define SEALEVELPRESSURE_HPA (1013.25) +#include "freertos/idf_additions.h" +#include "nav.h" + +#define SEALEVELPRESSURE_HPA (1030) Adafruit_BME280 bme; // use I2C interface Adafruit_Sensor *bme_temp = bme.getTemperatureSensor(); Adafruit_Sensor *bme_pressure = bme.getPressureSensor(); Adafruit_Sensor *bme_humidity = bme.getHumiditySensor(); -static const constexpr char *TAG = "IMU"; +static const constexpr char *TAG = "BARO"; namespace env_sens { - void setup() { + baro_mutex = xSemaphoreCreateMutex(); + if (!bme.begin()) { ESP_LOGE(TAG, "Couldn't find a valid sensor"); return; } - bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X2, - Adafruit_BME280::SAMPLING_X2, Adafruit_BME280::SAMPLING_NONE, - Adafruit_BME280::FILTER_OFF, - Adafruit_BME280::STANDBY_MS_62_5); + ESP_LOGI(TAG, "BARO SETUP COMPLETE."); + bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X8, + Adafruit_BME280::SAMPLING_X8, Adafruit_BME280::SAMPLING_NONE, + Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_10); bme_temp->printSensorDetails(); bme_pressure->printSensorDetails(); @@ -33,20 +38,23 @@ void setup() { float get_temperature() { sensors_event_t temp_event; + bme_temp->getEvent(&temp_event); return temp_event.temperature; } float get_pressure() { sensors_event_t e; + bme_pressure->getEvent(&e); + return e.pressure; } float calculateAltitude(float pressure, float seaLevelPressure, float tempCelsius) { float altitude = - (((pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) * + (((std::pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) * (tempCelsius + 273.15)) / 0.0065; return altitude; @@ -62,4 +70,47 @@ void dbg_sens() { get_pressure(), get_altitude()); } +void baro_poll_task(void *_) { + env_sens::setup(); + + float last_alt = env_sens::get_altitude(); + uint32_t last_time = xTaskGetTickCount(); + + float filtered_alt = last_alt; + const float alt_lpf = 0.1f; + + while (true) { + uint32_t now = xTaskGetTickCount(); + float dt = (now - last_time) * portTICK_PERIOD_MS / 1000.0f; + + if (dt > 0.001f) { // Prevent division by zero + float current_alt = env_sens::get_altitude(); + + filtered_alt = (alt_lpf * current_alt) + (1.0f - alt_lpf) * filtered_alt; + + float v_z = (filtered_alt - last_alt) / dt; + + if (nav_mutex && xSemaphoreTake(nav_mutex, (TickType_t)20) == pdTRUE) { + + Eigen::Vector3f baro_pos = nav_filter.position; + baro_pos.z() = filtered_alt; + + Eigen::Vector3f baro_vel = nav_filter.velocity; + baro_vel.z() = v_z; + + // Update the filter with Baro data + nav_filter.measure_baro(dt, baro_pos, baro_vel); + + xSemaphoreGive(nav_mutex); + } + + last_alt = filtered_alt; + last_time = now; + } + + // BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal + vTaskDelay(pdMS_TO_TICKS(20)); + } +} + } // namespace env_sens diff --git a/main/env_sens.h b/main/env_sens.h index 9b6379d..c66dafd 100644 --- a/main/env_sens.h +++ b/main/env_sens.h @@ -1,5 +1,6 @@ #pragma once +#include "freertos/idf_additions.h" namespace env_sens { void setup(); @@ -11,5 +12,8 @@ float get_pressure(); void dbg_sens(); float get_altitude(); +void baro_poll_task(void *_); } // namespace env_sens + +inline SemaphoreHandle_t baro_mutex = NULL; diff --git a/main/gps.cpp b/main/gps.cpp new file mode 100644 index 0000000..a0d8238 --- /dev/null +++ b/main/gps.cpp @@ -0,0 +1,36 @@ +#include "gps.h" +#include "esp_log.h" +#include "nav.h" + +static const char *TAG = "GPS_TASK"; + +void gps_poll_task(void *_) { + + gps = new GPS(); + gps->begin(); + gps_mutex = xSemaphoreCreateMutex(); + + ESP_LOGI(TAG, "GPS TASK INIT."); + + while (true) { + if (xSemaphoreTake(gps_mutex, (TickType_t)1) == pdTRUE) { + + // ESP_LOGI(TAG, "Polling gps."); + gps->poll(); + if (gps->gps_avaliable() && nav_mutex && + xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) { + auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero()); + auto coords = gps->get_coordinates(); + if (coords.has_value()) { + nav_filter.measure_gps(1.0, gps->get_coordinates().value(), + Eigen::Vector3f(vel.x(), vel.y(), 0.0)); + } + xSemaphoreGive(nav_mutex); + } + xSemaphoreGive(gps_mutex); + } else { + ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX"); + } + vTaskDelay(pdMS_TO_TICKS(100)); + } +} diff --git a/main/gps.h b/main/gps.h index 98c40fd..f487b37 100644 --- a/main/gps.h +++ b/main/gps.h @@ -11,6 +11,7 @@ #include #include "HardwareSerial.h" + #include "esp_log.h" #include #include @@ -22,7 +23,7 @@ const float KNOTS_TO_M_SEC = 0.5144444f; const float earth_radius = 6371000.0f; struct GPS { - Adafruit_GPS gps; + Adafruit_GPS *gps; float origin_lat; float origin_long; @@ -33,22 +34,22 @@ struct GPS { } void begin() { - this->gps = Adafruit_GPS(&Serial2); - this->gps.begin(9600); - this->gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); + this->gps = new Adafruit_GPS(&Serial2); + this->gps->begin(9600); + this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); - this->gps.sendCommand(PMTK_API_SET_FIX_CTL_5HZ); - this->gps.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ); + this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ); + this->gps->sendCommand(PMTK_SET_NMEA_UPDATE_5HZ); } - bool gps_avaliable() { return this->gps.fix; } + bool gps_avaliable() { return this->gps->fix; } std::optional velocity() { - if (!this->gps.fix) { + if (!this->gps->fix) { return std::nullopt; } - float speed = this->gps.speed * KNOTS_TO_M_SEC; - float angle = this->gps.angle * TO_RAD; + float speed = this->gps->speed * KNOTS_TO_M_SEC; + float angle = this->gps->angle * TO_RAD; return Eigen::Vector2f(cos(M_PI_2 - angle), sin(M_PI_2 - angle)) * speed; } @@ -66,28 +67,36 @@ struct GPS { float y = dLat * earth_radius; float x = dLon * earth_radius * std::cos(meanLat); - float z = this->gps.altitude; + float z = this->gps->altitude; return Eigen::Vector3f(x, y, z); } std::optional get_coordinates() { - if (this->gps.fix == false && this->gps.latitudeDegrees != 0.0 && - this->gps.longitudeDegrees != 0.0) { + if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 || + this->gps->longitudeDegrees != 0.0) { return std::nullopt; } - float latitude = this->gps.latitudeDegrees; - float longitude = this->gps.longitudeDegrees; + float latitude = this->gps->latitudeDegrees; + float longitude = this->gps->longitudeDegrees; - return this->waypoint_to_xyz(latitude, longitude, this->gps.altitude); + return this->waypoint_to_xyz(latitude, longitude, this->gps->altitude); } void poll() { - this->gps.read(); - if (this->gps.newNMEAreceived()) { - ESP_LOGD("GPS", "NMEA LINE: %s", this->gps.lastNMEA()); - if (!this->gps.parse(this->gps.lastNMEA())) - return; + 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; + } + } } } }; + +inline SemaphoreHandle_t gps_mutex; +inline GPS *gps = nullptr; + +void gps_poll_task(void *_); diff --git a/main/imu.cpp b/main/imu.cpp index 2f08be8..e475b66 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -2,68 +2,63 @@ #include "esp_log.h" #include "esp_timer.h" #include "freertos/idf_additions.h" +#include "hal/spi_types.h" +#include "nav.h" + +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include static const char *TAG = "IMU"; -SemaphoreHandle_t imuStateMutex = NULL; - BNO08x *setup_imu(imu_state *state) { - BNO08x *imu = new BNO08x(); + BNO08x *imu = new BNO08x( + bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25, + GPIO_NUM_33, GPIO_NUM_36, GPIO_NUM_32, 2000000, false)); if (!imu->initialize()) { ESP_LOGE(TAG, "BNO08x Init failure."); return nullptr; } - if (imuStateMutex == NULL) { - imuStateMutex = xSemaphoreCreateMutex(); - } - imu->dynamic_calibration_autosave_enable(); imu->dynamic_calibration_enable(BNO08xCalSel::all); imu->rpt.rv_game.enable(2500UL); imu->rpt.linear_accelerometer.enable(2500UL); - // imu->rpt.rv_game.tare(); imu->register_cb([imu, state]() { if (imu->rpt.rv_game.has_new_data()) { auto sens_rot = imu->rpt.rv_game.get_quat(); - auto euler = imu->rpt.rv_game.get_euler(); - if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) { - state->euler_ang = {euler.x, euler.y, euler.z}; - state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real}; - xSemaphoreGive(imuStateMutex); - } + state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real}; } if (imu->rpt.linear_accelerometer.has_new_data()) { - if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) { - auto sens_accel = imu->rpt.linear_accelerometer.get(); - state->accel = {sens_accel.x, sens_accel.y, sens_accel.z}; + auto sens_accel = imu->rpt.linear_accelerometer.get(); + state->accel = {sens_accel.x, sens_accel.y, sens_accel.z}; - int64_t current_time = esp_timer_get_time(); + int64_t current_time = esp_timer_get_time(); - if (state->last_time != -1) { - float dt = (current_time - state->last_time) / 1000000.0f; + if (state->last_time != -1) { + float dt = ((float)(current_time - state->last_time)) / 1000000.0f; + Vec3C accel_global = apply_rot(&state->accel, &state->rot); - Vec3C delta = apply_rot(&state->accel, &state->rot); - - // Trapezoidal Integration for Velocity: v = v0 + 0.5 * (a + a0) * dt - delta.x += state->last_accel.x; - delta.y += state->last_accel.y; - delta.z += state->last_accel.z; - - multiply_vec_by(&delta, dt * 0.5f); - add_to_vec(&state->vel, &delta); - - state->last_accel = state->accel; + if (xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) { + nav_filter.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y, + accel_global.z)); + xSemaphoreGive(nav_mutex); + } else { + ESP_LOGE(TAG, "Failed to get mutex."); } - state->last_time = current_time; - - xSemaphoreGive(imuStateMutex); } + state->last_time = current_time; } }); diff --git a/main/imu.h b/main/imu.h index 1c5615d..4016358 100644 --- a/main/imu.h +++ b/main/imu.h @@ -4,19 +4,12 @@ #include #include "BNO08x.hpp" -#include "BNO08xGlobalTypes.hpp" #include "drone_controller.h" -#include "freertos/idf_additions.h" struct imu_state { Vec3C accel = {0, 0, 0}; - Vec3C last_accel = {0, 0, 0}; QuatC rot = {0, 0, 0, 1}; - Vec3C vel = {0, 0, 0}; int64_t last_time = -1; - Vec3C euler_ang = {0, 0, 0}; }; BNO08x *setup_imu(imu_state *state); - -extern SemaphoreHandle_t imuStateMutex; diff --git a/main/main.cpp b/main/main.cpp index c766e87..593afe8 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,53 +1,92 @@ -#include "esp32-hal.h" +#include "driver/gpio.h" +#include "drone_comms.h" #include "esp_log.h" -#include -#include - +#include "freertos/FreeRTOS.h" #include "freertos/idf_additions.h" #include "freertos/projdefs.h" +#include "freertos/task.h" +#include "imu.h" +#include + +#include "env_sens.h" #include "gps.h" +#include "nav.h" +#include "radio.h" -static const constexpr char *TAG = "Main"; - -SemaphoreHandle_t gps_mutex; -GPS *gps = nullptr; - -void gps_poll_task(void *_) { - while (true) { - if (xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { - gps->poll(); - xSemaphoreGive(gps_mutex); - } - vTaskDelay(pdMS_TO_TICKS(50)); - } -} - -void gps_poll_init() { - gps_mutex = xSemaphoreCreateMutex(); - if (gps_mutex != NULL) { - xTaskCreate(gps_poll_task, "Writer", 2048, NULL, 1, NULL); - } -} +static const char *TAG = "MAIN"; extern "C" void app_main(void) { + + nav_mutex = xSemaphoreCreateMutex(); initArduino(); - gps = new GPS(); - gps_poll_init(); + gpio_install_isr_service(0); + Serial.begin(115200); + static imu_state imu_state; + auto _ = setup_imu(&imu_state); + + // xTaskCreatePinnedToCore(radio_rx_task, // Function name + // "radio_rx", // Name for debugging + // 4096, // Stack size in bytes + // NULL, // Parameters + // 1, // 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, 1, NULL); + + 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; while (true) { - if (xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { - ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", - gps->gps.latitudeDegrees, gps->gps.longitudeDegrees, - gps->gps.altitude); - if (gps->gps_avaliable()) { + // if (controller_input_semaphore && + // xSemaphoreTake(controller_input_semaphore, (TickType_t)30) == pdTRUE) + // { + // inp = current_controller_input; + // xSemaphoreGive(controller_input_semaphore); + // + // ESP_LOGI(TAG, "CONT INPUT: (%f, %f), (%f,%f)", inp.lx, inp.ly, inp.rx, + // inp.ry); + // } - auto D_pos = gps->get_coordinates().value(); - ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1], - D_pos[2]); + 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]); + } } xSemaphoreGive(gps_mutex); } - vTaskDelay(pdMS_TO_TICKS(250)); + env_sens::dbg_sens(); + + if (nav_mutex && xSemaphoreTake(nav_mutex, pdMS_TO_TICKS(10)) == pdTRUE) { + local_pos = nav_filter.position; + local_vel = nav_filter.velocity; + nav_data_ready = true; + xSemaphoreGive(nav_mutex); // RELEASE IMMEDIATELY + } + + if (nav_data_ready) { + ESP_LOGI(TAG, "nav(pos): (%f, %f, %f)", local_pos[0], local_pos[1], + local_pos[2]); + ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1], + local_vel[2]); + } + + vTaskDelay(pdMS_TO_TICKS(1000)); } } diff --git a/main/nav.h b/main/nav.h new file mode 100644 index 0000000..b5b9a18 --- /dev/null +++ b/main/nav.h @@ -0,0 +1,98 @@ +#pragma once +#include "freertos/idf_additions.h" +#include + +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include + +inline float getYawDifference(const Eigen::Vector3f &v_gps, + const Eigen::Vector3f &v_imu) { + float yaw_gps = std::atan2(v_gps.y(), v_gps.x()); + float yaw_imu = std::atan2(v_imu.y(), v_imu.x()); + + float delta_yaw = yaw_gps - yaw_imu; + + return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw)); +} + +struct nav_compl { + Eigen::Vector3f position = Eigen::Vector3f::Zero(); + Eigen::Vector3f velocity = Eigen::Vector3f::Zero(); + float yaw_offset = 0.0f; + + // Time Constants per axis (X, Y, Z) + // Lower = faster tracking of GPS; Higher = smoother/more IMU trust + Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f}; + Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY}; + + Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f}; + Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f}; + + float tau_yaw = 2.0f; // Yaw remains a scalar + + void predict(float dt, Eigen::Vector3f accel) { + // Rotate body-frame accel to world-frame + Eigen::Vector3f accel_rotated = + Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel; + + Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt); + + // Trapezoidal integration for position + this->position += (this->velocity + next_velocity) * 0.5f * dt; + this->velocity = next_velocity; + } + + void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) { + // Calculate Alpha vectors using element-wise operations + // Formula: alpha = dt / (tau + dt) + Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt); + Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt); + float alpha_yaw = dt / (tau_yaw + dt); + + // 1. Position Update (Element-wise LPF) + // res = (1 - alpha) * state + alpha * measurement + this->position = + (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() + + alpha_pos.array() * gps_pos.array(); + + // 2. Yaw Correction (only if moving > 1.0 m/s) + if (gps_vel.norm() > 1.0f) { + float yaw_delta = getYawDifference(gps_vel, this->velocity); + this->yaw_offset += yaw_delta * alpha_yaw; + + this->yaw_offset = + std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset)); + } + + // 3. Velocity Update (Element-wise LPF) + this->velocity = + (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() + + alpha_vel.array() * gps_vel.array(); + } + + void measure_baro(float dt, Eigen::Vector3f baro_pos, + Eigen::Vector3f baro_vel) { + // Calculate Alpha vectors using element-wise operations + // Formula: alpha = dt / (tau + dt) + Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt); + Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt); + + this->position = + (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() + + alpha_pos.array() * baro_pos.array(); + + this->velocity = + (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() + + alpha_vel.array() * baro_vel.array(); + } +}; + +inline SemaphoreHandle_t nav_mutex = NULL; +inline nav_compl nav_filter; diff --git a/main/pos.h b/main/pos.h deleted file mode 100644 index 218450e..0000000 --- a/main/pos.h +++ /dev/null @@ -1,65 +0,0 @@ - -#ifdef PS -#undef PS -#endif - -#ifdef F -#undef F -#endif - -#include - -inline float getYawDifference(const Eigen::Vector3f &v_gps, - const Eigen::Vector3f &v_imu) { - // 1. Extract the 2D components (Project to XY plane) - float yaw_gps = std::atan2(v_gps.y(), v_gps.x()); - float yaw_imu = std::atan2(v_imu.y(), v_imu.x()); - - // 2. Calculate the raw difference - float delta_yaw = yaw_gps - yaw_imu; - - // 3. Normalize the angle to [-PI, PI] - while (delta_yaw > M_PI) - delta_yaw -= 2.0 * M_PI; - while (delta_yaw < -M_PI) - delta_yaw += 2.0 * M_PI; - - return delta_yaw; -} - -struct nav_compl { - Eigen::Vector3f position = Eigen::Vector3f::Zero(); - Eigen::Vector3f velocity = Eigen::Vector3f::Zero(); - float yaw_offset = 0.0; - float yaw_delta_weight = 0.04; - - Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.03f); - Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.03f); - - void predict(float dt, Eigen::Vector3f accel) { - Eigen::Vector3f accel_rotated = - Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3d::UnitZ()) * accel; - - Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt); - - this->position += (this->velocity + next_velocity) * 0.5f * dt; - - this->velocity = next_velocity; - } - - void measure(Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) { - this->position = (Eigen::Vector3f::Ones() - gps_weight_pos) - .cwiseProduct(this->position) + - gps_weight_pos.cwiseProduct(gps_pos); - - if (gps_vel.norm() > 1) { - float yaw_delta = getYawDifference(gps_vel, this->velocity); - yaw_offset = - yaw_offset * (1 - yaw_delta_weight) + yaw_delta * yaw_delta_weight; - } - - this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel) - .cwiseProduct(this->velocity) + - gps_weight_vel.cwiseProduct(gps_vel); - } -}; diff --git a/main/radio.cpp b/main/radio.cpp new file mode 100644 index 0000000..5a5952a --- /dev/null +++ b/main/radio.cpp @@ -0,0 +1,92 @@ +#include "radio.h" +#include "esp32-hal-gpio.h" +#include "esp_log.h" +#include "freertos/idf_additions.h" +#include +#include +#include +#include +#include + +#include + +#define RFM69_RST 14 +#define RFM69_CS 12 +#define RFM69_INT 39 +#define SPI_SCLK 18 +#define SPI_MISO 19 +#define SPI_MOSI 23 + +#define FREQUENCY RF69_433MHZ +#define NODEID 1 +#define NETWORKID 100 + +static const char *TAG = "RADIO_TASK"; + +#define PACKET_QUEUE_CAP 10 + +void radio_rx_task(void *pvParameters) { + ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID()); + packet_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE); + controller_input_semaphore = xSemaphoreCreateMutex(); + + pinMode(RFM69_CS, OUTPUT); + pinMode(RFM69_INT, INPUT); + + // 1. Setup SPI for this task + SPIClass vspi(VSPI); + vspi.begin(SPI_SCLK, SPI_MISO, SPI_MOSI, 34); + + // 3. Hardware Reset + pinMode(RFM69_RST, OUTPUT); + digitalWrite(RFM69_RST, HIGH); + vTaskDelay(pdMS_TO_TICKS(10)); + digitalWrite(RFM69_RST, LOW); + vTaskDelay(pdMS_TO_TICKS(50)); + + RFM69 radio(RFM69_CS, RFM69_INT, true, &vspi); + + if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) { + radio.setHighPower(true); + ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10)); + } else { + ESP_LOGE(TAG, "Radio Init FAILED! Deleting Task."); + vTaskDelete(NULL); + } + + // 4. Polling Loop + while (1) { + if (radio.receiveDone()) { + // ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID, + // radio.RSSI, + // radio.DATALEN); + + memset(packet_data, '\0', sizeof(packet_data)); + memcpy(packet_data, radio.DATA, radio.DATALEN); + + PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]); + if (packet_type == CONTROLLER_INPUT) { + + controller_input *inp = + (controller_input *)(&packet_data[0] + sizeof(PACKET_TYPE)); + + if (xSemaphoreTake(controller_input_semaphore, (TickType_t)50) == + pdTRUE) { + current_controller_input = *inp; + xSemaphoreGive(controller_input_semaphore); + } + + } else { + xQueueSend(packet_queue, &packet_data, portMAX_DELAY); + } + + if (radio.ACKRequested()) { + radio.sendACK(); + } + } + + // Small delay to prevent Watchdog trigger and allow other tasks to run + // 1ms is usually enough for high-speed polling + vTaskDelay(pdMS_TO_TICKS(1)); + } +} diff --git a/main/radio.h b/main/radio.h new file mode 100644 index 0000000..b5d87d9 --- /dev/null +++ b/main/radio.h @@ -0,0 +1,10 @@ +#pragma once + +#include "drone_comms.h" +#include "freertos/idf_additions.h" + +inline QueueHandle_t packet_queue = NULL; +inline SemaphoreHandle_t controller_input_semaphore = NULL; +inline controller_input current_controller_input; + +void radio_rx_task(void *pvParameters);