work on getting positioning working

main/gps.h

@@ -1,24 +1,91 @@
 #pragma once
 
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
 #include "HardwareSerial.h"
-#include "Wire.h"
+#include "esp_log.h"
 #include <Adafruit_GPS.h>
+#include <cmath>
+#include <optional>
+
+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<Eigen::Vector2f> 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<Eigen::Vector3f>
+  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<Eigen::Vector3f> 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;
     }

main/main.cpp

@@ -1,44 +1,54 @@
-#include "HardwareSerial.h"
-#include "dshot_utils.h"
 
 #include "esp32-hal.h"
 #include "esp_log.h"
 #include <cmath>
-#include <iostream>
+#include <cstdio>
 
-#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]);
       }
-  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);
+      xSemaphoreGive(gps_mutex);
     }
 
-  ESP_LOGI(TAG, "Stopping motor");
-  motor.sendThrottlePercent(0);
+    vTaskDelay(pdMS_TO_TICKS(250));
+  }
 }

main/pos.cpp

@@ -1,27 +0,0 @@
-#include <Eigen/Dense>
-
-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);
-  }
-};

main/pos.h

@@ -0,0 +1,65 @@
+
+#ifdef PS
+#undef PS
+#endif
+
+#ifdef F
+#undef F
+#endif
+
+#include <Eigen/Dense>
+
+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);
+  }
+};