Updated GlobalTrackData struct to store precomputed circle radius

src/custom_types.cpp

@@ -4,14 +4,13 @@
 #include "custom_types.h"
 #include "math.h"
 
-Vec2 eqRectProjection(const LatLng target, const LatLng ref) {
+Vec2 eqRectProjection(const LatLng& target, const LatLng& ref) {
   const float R = 6371000.0;
   float lat0_r = ref.lat_ * M_PI / 180;
   float lng0_r = ref.lng_ * M_PI / 180;
   float lat1_r = target.lat_ * M_PI / 180;
   float lng1_r = target.lng_ * M_PI / 180;
-  float lat_avg = (lat0_r + lat1_r) * 0.5f;
+  float lat0_cos = cos((lat0_r + lat1_r) * 0.5f);
-  float lat0_cos = cos(lat_avg);
 
 
   Vec2 res;
@@ -21,14 +20,46 @@ Vec2 eqRectProjection(const LatLng target, const LatLng ref) {
   return res;
 }
 
-Vec2 abMidpoint(const Vec2 A, const Vec2 B) {
+LatLng eqRectInverse(const Vec2& point, const LatLng& ref) {
+    const float R = 6371000.0f;
+
+    float lat0 = ref.lat_ * M_PI / 180.0f;
+    float lng0 = ref.lng_ * M_PI / 180.0f;
+
+    float cos_lat0 = cos(lat0);
+
+    // Recover latitude
+    float lat = lat0 + (point.y_ / R);
+
+    // Recover longitude
+    float lng = lng0 + (point.x_ / (R * cos_lat0));
+
+    LatLng res;
+    res.lat_ = lat * 180.0f / M_PI;
+    res.lng_ = lng * 180.0f / M_PI;
+
+    return res;
+}
+
+Vec2 abMidpoint(const Vec2& A, const Vec2& B) {
   Vec2 res;
   res.x_ = (A.x_ + B.x_) * 0.5f;
   res.y_ = (A.y_ + B.y_) * 0.5f;
   return res;
 }
 
-float abSqDist(const Vec2 A, const Vec2 B) {
+Vec2 abSum(const Vec2& A, const Vec2& B) {
+  Vec2 res;
+  res.x_ = A.x_ + B.x_;
+  res.y_ = A.y_ + B.y_;
+  return res;
+}
+
+float vecMod(const Vec2& in) {
+  return sqrtf(in.x_*in.x_ + in.y_*in.y_);
+}
+
+float abSqDist(const Vec2& A, const Vec2& B) {
   float deltaX = B.x_ - A.x_;
   float deltaY = B.y_ - A.y_;
   return deltaX * deltaX + deltaY * deltaY;

src/custom_types.h

@@ -28,9 +28,12 @@ struct Vec2 {
     float y_;
 };
 
-Vec2 eqRectProjection(const LatLng target, const LatLng ref);
+Vec2 eqRectProjection(const LatLng& target, const LatLng& ref);
-Vec2 abMidpoint(const Vec2 A, const Vec2 B);
+LatLng eqRectInverse(const Vec2& point, const LatLng& ref);
-float abSqDist(const Vec2 A, const Vec2 B);
+Vec2 abMidpoint(const Vec2& A, const Vec2& B);
+Vec2 abSum(const Vec2& A, const Vec2& B);
+float vecMod(const Vec2& in);
+float abSqDist(const Vec2& A, const Vec2& B);
 
 struct TrackData {
     uint16_t magic_ = CONFIG_MAGIC;
@@ -41,8 +44,9 @@ struct TrackData {
 };
 
 struct GlobalTrackData {
-    bool loaded = false;
+    bool loaded_ = false;
     Vec2 center_;
+    float circle_radius_;
     TrackData root_;
 };
 

src/data/track_store.cpp

@@ -21,7 +21,7 @@ volatile TrackData track_data_temp_global = {};
         return 1;
      }
      
-     out.loaded = true;
+     out.loaded_ = true;
      out.root_ = track_data;
      return 0;
  }

src/modules/config/config.cpp

@@ -69,7 +69,7 @@ int Config::deleteTrack(unsigned short idx) {
     is_track_loaded_ = false;
     loaded_track_ = {};
     GlobalTrackData track;
-    track.loaded = false;
+    track.loaded_ = false;
     track.root_ = loaded_track_;
     trackGlobalWrite(track);
   }
@@ -92,7 +92,7 @@ int Config::resetConfig() {
   task_memory_stale_ = true;
   no_tracks_notice_shown_ = false;
     GlobalTrackData track;
-    track.loaded = false;
+    track.loaded_ = false;
     track.root_ = loaded_track_;
     trackGlobalWrite(track);
 
@@ -380,7 +380,16 @@ int Config::loadTrack(unsigned int idx) {
 
   loaded_track_ = track_data;
   GlobalTrackData track;
-  track.loaded = true;
+  track.loaded_ = true;
+  Vec2 point_b =eqRectProjection(track_data.point_b_, track_data.point_a_);
+  track.center_ = abMidpoint(point_b, (Vec2){0.0f,0.0f});
+  track.circle_radius_ = max(10.0f, vecMod(point_b) * 1.25f);
+  #ifdef DEBUG
+  if (logger_ != nullptr) {
+    logger_->debug("Radius: " + String(track.circle_radius_));
+    logger_->debug("Line Length: " + String(vecMod(point_b)));
+  }
+  #endif
   track.root_ = track_data;
   trackGlobalWrite(track);
   is_track_loaded_ = true;

src/modules/gps/gps.cpp

@@ -4,11 +4,11 @@
 
 #include "gps.h"
 
+#include "data/track_store.h"
+
 #define MOD "modules/gps/gps.h"
 
-int Gps::push(const Task &task) {
+int Gps::push(const Task &task) { return queue_.push(task); }
-  return queue_.push(task);
-}
 
 Gps::Gps(HardwareSerial *data_stream)
     : gps_(nullptr), data_stream_(data_stream), logger_(nullptr) {
@@ -52,8 +52,6 @@ int Gps::loop(unsigned long timeout_ms) {
 
   if (lap_active_) {
     if (start_line_trigger_ == trigger_status::Idle) {
-      float current_lat = gps_->location.lat();  
-      float current_lng = gps_->location.lat();  
     }
   }
 
@@ -63,14 +61,13 @@ int Gps::loop(unsigned long timeout_ms) {
     if (active.target_ == module::Gps) {
 
     } else if (active.target_ == module::All) {
-      switch (active.type_)
+      switch (active.type_) {
-      {
       case task::AllTrackLoaded:
-        #ifdef DEBUG
+#ifdef DEBUG
         if (logger_ != nullptr) {
           logger_->debug("GPS received track loaded sig");
         }
-        #endif
+#endif
         lap_active_ = true;
         break;