From 587d977018ced71dcef981d374162151d21f02e8 Mon Sep 17 00:00:00 2001 From: franchioping Date: Thu, 26 Mar 2026 23:01:20 +0000 Subject: [PATCH] work on getting positioning working --- main/gps.h | 83 ++++++++++++++++++++++++++++++++++++++++++++++----- main/main.cpp | 70 ++++++++++++++++++++++++------------------- main/pos.cpp | 27 ----------------- main/pos.h | 65 ++++++++++++++++++++++++++++++++++++++++ 4 files changed, 180 insertions(+), 65 deletions(-) delete mode 100644 main/pos.cpp create mode 100644 main/pos.h diff --git a/main/gps.h b/main/gps.h index 966b6ff..98c40fd 100644 --- a/main/gps.h +++ b/main/gps.h @@ -1,24 +1,91 @@ #pragma once +#ifdef PS +#undef PS +#endif + +#ifdef F +#undef F +#endif + +#include + #include "HardwareSerial.h" -#include "Wire.h" +#include "esp_log.h" #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; struct GPS { Adafruit_GPS gps; - GPS(TwoWire *theWire) { - Serial2.begin(9600); - gps = Adafruit_GPS(&Serial2); - gps.begin(0x10); - gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); + float origin_lat; + float origin_long; - gps.sendCommand(PMTK_API_SET_FIX_CTL_5HZ); - gps.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ); + GPS() { + + origin_lat = INFINITY; + origin_long = INFINITY; + } + + void begin() { + this->gps = 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); + } + + bool gps_avaliable() { return this->gps.fix; } + + std::optional velocity() { + if (!this->gps.fix) { + return std::nullopt; + } + 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; + } + + std::optional + waypoint_to_xyz(float latitude, float longitude, float height) { + if (this->origin_lat == INFINITY || this->origin_long == INFINITY) { + return std::nullopt; + } + + float dLat = (latitude - this->origin_lat) * TO_RAD; + float dLon = (longitude - this->origin_long) * TO_RAD; + + float meanLat = (this->origin_lat) * TO_RAD; + + float y = dLat * earth_radius; + float x = dLon * earth_radius * std::cos(meanLat); + 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) { + return std::nullopt; + } + float latitude = this->gps.latitudeDegrees; + float longitude = this->gps.longitudeDegrees; + + 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; } diff --git a/main/main.cpp b/main/main.cpp index eba3a5a..a215392 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,44 +1,54 @@ -#include "HardwareSerial.h" -#include "dshot_utils.h" #include "esp32-hal.h" #include "esp_log.h" #include -#include +#include -#include "DShotRMT.h" -#include "env_sens.h" +#include "freertos/idf_additions.h" +#include "freertos/projdefs.h" +#include "gps.h" static const constexpr char *TAG = "Main"; -const gpio_num_t MOTOR_PIN = GPIO_NUM_22; -DShotRMT motor(MOTOR_PIN, DSHOT300, true); +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); + } +} extern "C" void app_main(void) { initArduino(); - printf("beg: %d", motor.begin().result_code); + gps = new GPS(); - ESP_LOGI(TAG, "Arming ESC..."); - // Send 0 throttle for 4 seconds to arm - unsigned long armTime = millis(); - while (millis() - armTime < 4000) { - motor.sendThrottlePercent(0); - // delay(1); + 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()) { + + auto D_pos = gps->get_coordinates().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)); } - delay(50); - - ESP_LOGI(TAG, "Motor Armed. Ramping up..."); - // Send 10% throttle for 5 seconds - unsigned long runTime = millis(); - while (millis() - runTime < 5000) { - auto res = motor.sendThrottlePercent(10); - printf("%d, %d\n", res.result_code, res.erpm); - // - // ESP_LOGI(TAG, "current: %d", - // motor.getTelemetry().telemetry_data.current); - // delay(1); - } - - ESP_LOGI(TAG, "Stopping motor"); - motor.sendThrottlePercent(0); } diff --git a/main/pos.cpp b/main/pos.cpp deleted file mode 100644 index 08ccd45..0000000 --- a/main/pos.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include - -struct nav_compl { - Eigen::Vector3f position = Eigen::Vector3f::Zero(); - Eigen::Vector3f velocity = Eigen::Vector3f::Zero(); - - Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.01f); - Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.01f); - - void predict(float dt, Eigen::Vector3f accel) { - Eigen::Vector3f next_velocity = this->velocity + (accel * 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); - - this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel) - .cwiseProduct(this->velocity) + - gps_weight_vel.cwiseProduct(gps_vel); - } -}; diff --git a/main/pos.h b/main/pos.h new file mode 100644 index 0000000..218450e --- /dev/null +++ b/main/pos.h @@ -0,0 +1,65 @@ + +#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); + } +};