Initial Vec2 impl ready for algo

src/custom_types.cpp

@@ -0,0 +1,35 @@
+// Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
+// Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
+// SPDX-License-Identifier: GPL-3.0-or-later
+#include "custom_types.h"
+#include "math.h"
+
+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(lat_avg);
+
+
+  Vec2 res;
+
+  res.x_ = (lng1_r - lng0_r) * lat0_cos * R;
+  res.y_ = (lat1_r - lat0_r) * R;
+  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) {
+  float deltaX = B.x_ - A.x_;
+  float deltaY = B.y_ - A.y_;
+  return deltaX * deltaX + deltaY * deltaY;
+}

src/custom_types.h

@@ -23,6 +23,15 @@ struct LatLng {
     float lng_;
 };
 
+struct Vec2 {
+    float x_;
+    float y_;
+};
+
+Vec2 eqRectProjection(const LatLng target, const LatLng ref);
+Vec2 abMidpoint(const Vec2 A, const Vec2 B);
+float abSqDist(const Vec2 A, const Vec2 B);
+
 struct TrackData {
     uint16_t magic_ = CONFIG_MAGIC;
     unsigned short id_;
@@ -31,6 +40,12 @@ struct TrackData {
     LatLng point_b_;
 };
 
+struct GlobalTrackData {
+    bool loaded = false;
+    Vec2 center_;
+    TrackData root_;
+};
+
 struct GpsSubData {
     uint32_t age_;
     bool valid_;

src/data/track_store.cpp

@@ -4,14 +4,14 @@
 #include "data/track_store.h"
 #include "data/eeprom_layout.h"
 
- volatile TrackData track_data_global = {};
+ volatile GlobalTrackData track_data_global = {};
 volatile TrackData track_data_temp_global = {};
 
- void trackGlobalRead(TrackData& out) {
+ void trackGlobalRead(GlobalTrackData& out) {
      copyFromVolatile(out, track_data_global);
  }
 
- int trackGlobalRead(unsigned short idx, TrackData& out) {
+ int trackGlobalRead(unsigned short idx, GlobalTrackData& out) {
      if (idx < 1 || idx > 8) {
         return 1;
      }
@@ -20,11 +20,13 @@ volatile TrackData track_data_temp_global = {};
      if (track_data.magic_ != CONFIG_MAGIC) {
         return 1;
      }
-     out = track_data;
+     
+     out.loaded = true;
+     out.root_ = track_data;
      return 0;
  }
 
- void trackGlobalWrite(const TrackData& in) {
+ void trackGlobalWrite(const GlobalTrackData& in) {
      copyToVolatile(track_data_global, in);
  }
 

src/data/track_store.h

@@ -6,12 +6,12 @@
 #include "custom_types.h"
 #include <EEPROM.h>
 
-extern volatile TrackData track_data_global;
+extern volatile GlobalTrackData track_data_global;
 extern volatile TrackData track_data_temp_global;
 
-void trackGlobalRead(TrackData& out);
+void trackGlobalRead(GlobalTrackData& out);
-int trackGlobalRead(unsigned short idx, TrackData& out);
+int trackGlobalRead(unsigned short idx, GlobalTrackData& out);
-void trackGlobalWrite(const TrackData& in);
+void trackGlobalWrite(const GlobalTrackData& in);
 
 void trackTempGlobalRead(TrackData& out);
 void trackTempGlobalWrite(const TrackData& in);

src/modules/cmd/cmd.cpp

@@ -149,7 +149,7 @@ int Cmd::dumpTrackSlot(unsigned short id) {
   configGlobalRead(config);
   bool occupied = config.track_slot_occupied_[id - 1];
 
-  TrackData track_data;
+  GlobalTrackData track_data;
   int result = trackGlobalRead(id, track_data);
   if (result != 0) {
 #ifdef ERROR
@@ -164,12 +164,12 @@ int Cmd::dumpTrackSlot(unsigned short id) {
   if (logger_ != nullptr) {
     logger_->info("Track dump for slot " + String(id));
     logger_->info(String("\tOccupied flag: ") + String(occupied));
-    logger_->info(String("\tID: ") + String(track_data.id_));
+    logger_->info(String("\tID: ") + String(track_data.root_.id_));
-    logger_->info(String("\tName: ") + String(track_data.name_));
+    logger_->info(String("\tName: ") + String(track_data.root_.name_));
-    logger_->info(String("\tPoint A lat: ") + String(track_data.point_a_.lat_, 6));
+    logger_->info(String("\tPoint A lat: ") + String(track_data.root_.point_a_.lat_, 6));
-    logger_->info(String("\tPoint A lng: ") + String(track_data.point_a_.lng_, 6));
+    logger_->info(String("\tPoint A lng: ") + String(track_data.root_.point_a_.lng_, 6));
-    logger_->info(String("\tPoint B lat: ") + String(track_data.point_b_.lat_, 6));
+    logger_->info(String("\tPoint B lat: ") + String(track_data.root_.point_b_.lat_, 6));
-    logger_->info(String("\tPoint B lng: ") + String(track_data.point_b_.lng_, 6));
+    logger_->info(String("\tPoint B lng: ") + String(track_data.root_.point_b_.lng_, 6));
   }
 #endif
 

src/modules/config/config.cpp

@@ -68,7 +68,10 @@ int Config::deleteTrack(unsigned short idx) {
   if (is_track_loaded_ && loaded_track_.id_ == idx) {
     is_track_loaded_ = false;
     loaded_track_ = {};
-    trackGlobalWrite(loaded_track_);
+    GlobalTrackData track;
+    track.loaded = false;
+    track.root_ = loaded_track_;
+    trackGlobalWrite(track);
   }
 
   int write_result = this->writeConfig();
@@ -88,7 +91,10 @@ int Config::resetConfig() {
   loaded_track_ = {};
   task_memory_stale_ = true;
   no_tracks_notice_shown_ = false;
-  trackGlobalWrite(loaded_track_);
+    GlobalTrackData track;
+    track.loaded = false;
+    track.root_ = loaded_track_;
+    trackGlobalWrite(track);
 
 #ifdef INFO
   if (logger_ != nullptr) {
@@ -373,7 +379,10 @@ int Config::loadTrack(unsigned int idx) {
   }
 
   loaded_track_ = track_data;
-  trackGlobalWrite(loaded_track_);
+  GlobalTrackData track;
+  track.loaded = true;
+  track.root_ = track_data;
+  trackGlobalWrite(track);
   is_track_loaded_ = true;
   router::send(module::All, task::AllTrackLoaded);
   return 0;

src/modules/gps/gps.cpp

@@ -50,6 +50,13 @@ 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();  
+    }
+  }
+  
   Task active; 
   int res = queue_.pop(active);
   if (res == 0) {

src/modules/lcd/lcd.cpp

@@ -195,11 +195,11 @@ int Lcd::renderMsgTrackDetectOk() {
   display_->setCursor(3, 1);
   this->print("TRACK DETECTED");
 
-  TrackData track_data;
+  GlobalTrackData track_data;
   trackGlobalRead(track_data);
 
-  display_->setCursor((20 - strlen(track_data.name_)) / 2, 2);
+  display_->setCursor((20 - strlen(track_data.root_.name_)) / 2, 2);
-  this->print(track_data.name_);
+  this->print(track_data.root_.name_);
   return 0;
 }