diff --git a/components/drone_comms b/components/drone_comms index ac25a39..d58740e 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit ac25a39777bffe724f80de5ec86bc1b0773833a2 +Subproject commit d58740e83368c9f2cc2b641ff729d04e7033d007 diff --git a/main/env_sens.cpp b/main/env_sens.cpp index 94de0f5..ad5e471 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -20,7 +20,7 @@ Adafruit_Sensor *bme_humidity = bme.getHumiditySensor(); static const constexpr char *TAG = "BARO"; -#define BARO_SDA GPIO_NUM_47 +#define BARO_SDA GPIO_NUM_47 // SDI #define BARO_SCL GPIO_NUM_48 namespace env_sens { @@ -141,7 +141,7 @@ void baro_poll_task(void *_) { last_time = now; } - // BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal + // BME280 config has a 20ms standby, so 20ms-50ms poll is ideal vTaskDelay(pdMS_TO_TICKS(50)); } } diff --git a/main/gps.cpp b/main/gps.cpp index 8519d49..e9bfd0c 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -2,6 +2,7 @@ #include "esp32-hal.h" #include "esp_log.h" +#include "freertos/idf_additions.h" #include "sens_fus.h" #include @@ -16,7 +17,6 @@ void gps_poll_task(void *_) { uint64_t last_time = millis(); while (true) { - bool has_new_data = false; bool available = false; Eigen::Vector2f local_vel = Eigen::Vector2f::Zero(); std::optional local_coords; @@ -25,30 +25,37 @@ 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(); + + available = local_coords.has_value(); } xSemaphoreGive(gps_mutex); - has_new_data = local_coords.has_value(); } else { ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX"); } - if (has_new_data && available && sens_fus_mutex) { + if (sens_fus_mutex) { + if (available) { - uint64_t current_time = millis(); - if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) { - sens_fus.measure_gps( - (current_time - last_time) / 1000.0f, local_coords.value(), - Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f)); + uint64_t current_time = millis(); + if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) { + sens_fus.measure_gps( + (current_time - last_time) / 1000.0f, local_coords.value(), + Eigen::Vector3f(local_vel.x(), local_vel.y(), 0.0f)); - xSemaphoreGive(sens_fus_mutex); - last_time = current_time; + xSemaphoreGive(sens_fus_mutex); + last_time = current_time; + } else { + + ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update."); + } } else { - - ESP_LOGE(TAG, "Sens_fus busy, skipping GPS update."); + if (xSemaphoreTake(sens_fus_mutex, 50) == pdTRUE) { + sens_fus.gps_lost(); + xSemaphoreGive(sens_fus_mutex); + } } } diff --git a/main/gps.h b/main/gps.h index d7195ea..6bb7b8f 100644 --- a/main/gps.h +++ b/main/gps.h @@ -1,5 +1,6 @@ #pragma once +#include #ifdef PS #undef PS #endif @@ -14,14 +15,15 @@ #include "esp_log.h" #include #include +#include #include const float TO_RAD = M_PI / 180.0f; const float KNOTS_TO_M_SEC = 0.5144444f; const float earth_radius = 6371000.0f; -#define GPS_RX_PIN 18 -#define GPS_TX_PIN 17 +#define GPS_TX_PIN 18 +#define GPS_RX_PIN 17 struct GPS { Adafruit_GPS *gps; @@ -34,8 +36,34 @@ struct GPS { origin_long = INFINITY; } + uint64_t unix_timestamp_millis() { + struct tm t; + + t.tm_year = (2000 + this->gps->year) - 1900; + t.tm_mon = this->gps->month - 1; + t.tm_mday = this->gps->day; + t.tm_hour = this->gps->hour; + t.tm_min = this->gps->minute; + t.tm_sec = this->gps->seconds; + t.tm_isdst = 0; // GPS is always UTC, so no Daylight Savings + + // mktime converts struct tm to time_t (Unix timestamp) + time_t unixTime = mktime(&t); + + // Calculate how many milliseconds have passed since the GPS updated its + // data + unsigned long msSinceUpdate = millis() - this->gps->lastDate; + + // Return the base timestamp plus the elapsed seconds + return (uint64_t)unixTime * 1000 + msSinceUpdate + gps->milliseconds; + } + + uint64_t unix_timestamp_seconds() { + return this->unix_timestamp_millis() / 1000; + } + void begin() { - Serial2.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN); + Serial2.begin(9600, SERIAL_8N1, GPS_TX_PIN, GPS_RX_PIN); this->gps = new Adafruit_GPS(&Serial2); // this->gps->begin(9600); this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); @@ -95,7 +123,7 @@ struct GPS { } if (this->gps->newNMEAreceived()) { char *line = this->gps->lastNMEA(); - // ESP_LOGI("GPS", "NMEA LINE: %s", line); + ESP_LOGI("GPS", "NMEA LINE: %s", line); this->gps->parse(line); } } diff --git a/main/imu.cpp b/main/imu.cpp index 06e21be..e998609 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -22,13 +22,13 @@ static const char *TAG = "IMU"; -#define IMU_CS GPIO_NUM_3 -#define IMU_MOSI GPIO_NUM_2 // DI -#define IMU_RST GPIO_NUM_1 +#define IMU_CS GPIO_NUM_3 // Green +#define IMU_MOSI GPIO_NUM_2 // DI - White +#define IMU_RST GPIO_NUM_1 // Red -#define IMU_INT GPIO_NUM_4 -#define IMU_MISO GPIO_NUM_5 // SDA -#define IMU_SCLK GPIO_NUM_6 // SCL +#define IMU_INT GPIO_NUM_4 // Yellow +#define IMU_MISO GPIO_NUM_5 // SDA - White +#define IMU_SCLK GPIO_NUM_6 // SCL - Green BNO08x *setup_imu() { imu_state *local_state = new imu_state; diff --git a/main/main.cpp b/main/main.cpp index c3c8a8a..d493291 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -33,17 +33,17 @@ extern "C" void app_main(void) { gpio_install_isr_service(0); Serial.begin(115200); - xTaskCreatePinnedToCore(radio_task, // Function name - "radio_rxtx", // Name for debugging - 4096, // Stack size in bytes - NULL, // Parameters - 5, // Priority (higher = more urgent) - NULL, // Task handle - 0 // Core ID - ); - - xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5, - NULL, 0); + // xTaskCreatePinnedToCore(radio_task, // Function name + // "radio_rxtx", // Name for debugging + // 4096, // Stack size in bytes + // NULL, // Parameters + // 5, // Priority (higher = more urgent) + // NULL, // Task handle + // 0 // Core ID + // ); + // + // xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, + // 5, NULL, 0); xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0); diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index fd0c55d..39aff4d 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -106,35 +106,31 @@ void send_packet_getter(PACKET_TYPE requested_type) { packet_info_drone_position{ .trans = {sens_fus.position.x(), sens_fus.position.y(), sens_fus.position.z()}, - .accel = {imu_state_var.lin_accel.x, imu_state_var.lin_accel.y, - imu_state_var.lin_accel.z}, + .accel = {imu_state_var.lin_accel_global.x, + imu_state_var.lin_accel_global.y, + imu_state_var.lin_accel_global.z}, .vel = {local_vel.x(), local_vel.y(), local_vel.z()}, .rot = {imu_state_var.rot.w(), imu_state_var.rot.x(), imu_state_var.rot.y(), imu_state_var.rot.z()}, - .pressure = env_sens::get_pressure(), - .temperature = env_sens::get_temperature()}); + .angvel = {imu_state_var.angvel.x(), imu_state_var.angvel.y(), + imu_state_var.angvel.z()}}); } if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) { - - if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY) == pdTRUE) { - - // TODO: Absolute time from GPS instead of 0 + if (gps_mutex && xSemaphoreTake(gps_mutex, portMAX_DELAY)) { resp_packet = create_packet_pooled( PACKET_TYPE::INFO_DRONE_STATUS, packet_info_drone_status{ .origin = {gps->origin_long, gps->origin_lat}, .time_since_boot = millis(), - .unix_timestamp_millis = 0, + .unix_timestamp_millis = gps->unix_timestamp_millis(), .gps_fix = gps->gps_avaliable()}); - ESP_LOGI("STATUS", "Status sent"); xSemaphoreGive(gps_mutex); } } // Navigation if (requested_type == PACKET_TYPE::DRONE_NAV) { - uint8_t active_mask, current; if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) { active_mask = nav_man.get_active_mask(); diff --git a/main/radio.cpp b/main/radio.cpp index 6325a9c..8ff88af 100644 --- a/main/radio.cpp +++ b/main/radio.cpp @@ -15,13 +15,14 @@ // Right to left on hardware. -#define RFM69_MOSI GPIO_NUM_12 -#define RFM69_SCLK GPIO_NUM_11 // SCK -#define RFM69_MISO GPIO_NUM_13 -#define RFM69_CS GPIO_NUM_9 // NSS #define RFM69_INT GPIO_NUM_8 // DI0 +#define RFM69_CS GPIO_NUM_9 // NSS #define RFM69_RST GPIO_NUM_10 +#define RFM69_SCLK GPIO_NUM_11 // SCK +#define RFM69_MOSI GPIO_NUM_12 +#define RFM69_MISO GPIO_NUM_13 + #define FREQUENCY RF69_433MHZ #define NODEID 1 #define GROUNDID 99 diff --git a/main/sens_fus.h b/main/sens_fus.h index ab8e808..b5efca4 100644 --- a/main/sens_fus.h +++ b/main/sens_fus.h @@ -27,9 +27,18 @@ 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; + + Eigen::Vector3f velocity_error = Eigen::Vector3f::Zero(); + Eigen::Vector3f position_error = Eigen::Vector3f::Zero(); + + void gps_lost() { + this->position_error = Eigen::Vector3f::Zero(); + this->velocity_error = Eigen::Vector3f::Zero(); + } + + Eigen::Vector3f velocity_error_mult = {40.0f, 40.0f, 0.0f}; + Eigen::Vector3f position_error_mult = {40.0f, 40.0f, 0.0f}; /* * Tau is the time that the filter takes to reach 1-e^(-1) of the difference @@ -37,67 +46,48 @@ 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 = {2.0f, 2.0f, INFINITY}; - Eigen::Vector3f tau_gps_vel = {INFINITY, INFINITY, INFINITY}; + Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, INFINITY}; + Eigen::Vector3f tau_gps_vel = {4.0f, 4.0f, INFINITY}; - Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 10.0f}; - Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f}; - - float tau_yaw = 10.0f; // Yaw remains a scalar + Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 4.0f}; + Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 4.0f}; Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval(); - void update_yaw_matrix() { - // yaw_rotation_matrix = - // Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) - // .toRotationMatrix(); - } - void predict(float dt, Eigen::Vector3f accel) { - Eigen::Vector3f accel_rotated = yaw_rotation_matrix * accel; + Eigen::Vector3f accel_err_rmvd = + accel.array() - + this->velocity_error.array() * this->velocity_error_mult.array(); Eigen::Vector3f next_velocity = - this->velocity + (this->last_accel_world + accel_rotated) * 0.5f * dt; + this->velocity + (this->last_accel_world + accel_err_rmvd) * 0.5f * dt; - this->position += (this->velocity + next_velocity) * 0.5f * dt; + this->position = + this->position.array() + + (((this->velocity + next_velocity) * 0.5f).array() - + this->position_error.array() * this->position_error_mult.array()) * + dt; this->velocity = next_velocity; - this->last_accel_world = accel_rotated; + this->last_accel_world = accel_err_rmvd; } void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) { // 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); - // res = (1 - alpha) * state + alpha * measurement + this->velocity_error = this->velocity - gps_vel; + this->position_error = this->position - gps_pos; + + // next_state = (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() > 0.2f) { - 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 = - std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset)); - this->update_yaw_matrix(); - - this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() * - this->velocity.array() + - alpha_vel.array() * gps_vel.array(); - } else if (this->velocity.norm() > 0.2f) { - this->velocity *= 1 - (0.90 * dt); - } - - this->velocity_last_gps_measurment = this->velocity; + 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,