LapCounter initial impl

src/base/task.h

@@ -15,6 +15,7 @@ enum Id : uint8_t {
     Battery,
     Thermocouple,
     Telemetry,
+    LapCounter,
     Count,
     Null,
     All,
@@ -44,6 +45,7 @@ enum Type : uint8_t {
     ConfigTengSetHigh,
     BatteryCal,
     AllConfigUpdated,
+    AllTrackLoaded,
 };
 
 } // namespace task

src/custom_types.cpp

@@ -0,0 +1,71 @@
+// 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 lat0_cos = cos((lat0_r + lat1_r) * 0.5f);
+
+
+  Vec2 res;
+
+  res.x_ = (lng1_r - lng0_r) * lat0_cos * R;
+  res.y_ = (lat1_r - lat0_r) * R;
+  return res;
+}
+
+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 vec2Midpoint(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;
+}
+
+Vec2 vec2Sum(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 vec2SqDist(const Vec2& A, const Vec2& B) {
+  float deltaX = B.x_ - A.x_;
+  float deltaY = B.y_ - A.y_;
+  return deltaX * deltaX + deltaY * deltaY;
+}
+
+
+float vec2Cross(const Vec2& B, const Vec2& M) {
+  return B.x_ * M.y_ - B.y_ * M.x_;
+}

src/custom_types.h

@@ -23,6 +23,19 @@ struct LatLng {
     float lng_;
 };
 
+struct Vec2 {
+    float x_;
+    float y_;
+};
+
+Vec2 eqRectProjection(const LatLng& target, const LatLng& ref);
+LatLng eqRectInverse(const Vec2& point, const LatLng& ref);
+Vec2 vec2Midpoint(const Vec2& A, const Vec2& B);
+Vec2 vec2Sum(const Vec2& A, const Vec2& B);
+float vecMod(const Vec2& in);
+float vec2SqDist(const Vec2& A, const Vec2& B);
+float vec2Cross(const Vec2& B, const Vec2& M);
+
 struct TrackData {
     uint16_t magic_ = CONFIG_MAGIC;
     unsigned short id_;
@@ -31,6 +44,13 @@ struct TrackData {
     LatLng point_b_;
 };
 
+struct GlobalTrackData {
+    bool loaded_ = false;
+    Vec2 center_;
+    float circle_radius_sq_;
+    TrackData root_;
+};
+
 struct GpsSubData {
     uint32_t age_;
     bool valid_;

src/data/general_store.cpp

@@ -5,6 +5,7 @@
 
  volatile float vbat_global = 0;
  volatile float teng_global = 0;
+ volatile int gps_trigger_global = 0;
 
  void vbatGlobalRead(float& out) {
      out = vbat_global;
@@ -21,3 +22,11 @@
  void tengGlobalWrite(const float& in) {
      teng_global = in;
  }
+
+void gpsTriggerGlobalRead(int& out) {
+    out = gps_trigger_global;
+}
+
+void gpsTriggerGlobalWrite(const int& in) {
+    gps_trigger_global = in;
+}

src/data/general_store.h

@@ -5,9 +5,13 @@
 
 extern volatile float vbat_global;
 extern volatile float teng_global;
+extern volatile int gps_trigger_global;
 
 void vbatGlobalRead(float& out);
 void vbatGlobalWrite(const float& in);
 
 void tengGlobalRead(float& out);
 void tengGlobalWrite(const float& in);
+
+void gpsTriggerGlobalRead(int& out);
+void gpsTriggerGlobalWrite(const int& in);

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);
-int trackGlobalRead(unsigned short idx, TrackData& out);
-void trackGlobalWrite(const TrackData& in);
+void trackGlobalRead(GlobalTrackData& out);
+int trackGlobalRead(unsigned short idx, GlobalTrackData& out);
+void trackGlobalWrite(const GlobalTrackData& in);
 
 void trackTempGlobalRead(TrackData& out);
 void trackTempGlobalWrite(const TrackData& in);

src/modules/cmd/cmd.cpp

@@ -104,6 +104,10 @@ Cmd::CommandId Cmd::parseCommandName(const char *input) {
     return DisplayGpsDebug;
   }
 
+  if (strcmp(input, "DISPLAY_GPS_LINE_DEBUG") == 0) {
+    return DisplayGpsLineDebug;
+  }
+
   if (strcmp(input, "DISPLAY_DRIVER_PRIMARY") == 0) {
     return DisplayDriverPrimary;
   }
@@ -149,7 +153,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 +168,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("\tName: ") + String(track_data.name_));
-    logger_->info(String("\tPoint A lat: ") + String(track_data.point_a_.lat_, 6));
-    logger_->info(String("\tPoint A lng: ") + String(track_data.point_a_.lng_, 6));
-    logger_->info(String("\tPoint B lat: ") + String(track_data.point_b_.lat_, 6));
-    logger_->info(String("\tPoint B lng: ") + String(track_data.point_b_.lng_, 6));
+    logger_->info(String("\tID: ") + String(track_data.root_.id_));
+    logger_->info(String("\tName: ") + String(track_data.root_.name_));
+    logger_->info(String("\tPoint A lat: ") + String(track_data.root_.point_a_.lat_, 6));
+    logger_->info(String("\tPoint A lng: ") + String(track_data.root_.point_a_.lng_, 6));
+    logger_->info(String("\tPoint B lat: ") + String(track_data.root_.point_b_.lat_, 6));
+    logger_->info(String("\tPoint B lng: ") + String(track_data.root_.point_b_.lng_, 6));
   }
 #endif
 
@@ -365,6 +369,7 @@ int Cmd::handleDisplayGpsDebug(unsigned short argc) {
   return router::send(module::Lcd, task::DisplayGpsDebug);
 }
 
+
 int Cmd::handleDisplayDriverPrimary(unsigned short argc) {
   if (argc != 1) {
 #ifdef ERROR

src/modules/cmd/cmd.h

@@ -24,6 +24,7 @@ private:
     ConfigDump,
     TrackAutodetect,
     DisplayGpsDebug,
+    DisplayGpsLineDebug,
     DisplayDriverPrimary,
     BatteryCal,
     BatteryPrintVbat,

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) {
@@ -207,6 +213,11 @@ int Config::taskConfigDetectTrack(unsigned long timeout_ms) {
       this->taskComplete();
       if (load_result == 0) {
         no_tracks_notice_shown_ = false;
+        #ifdef INFO
+        if (logger_ != nullptr) {
+          logger_->info("Track detected: " + String(loaded_track_.name_));
+        }
+        #endif
         router::send(module::Lcd, task::DisplayMsgTrackDetectOk, 4000);
         return 0;
       }
@@ -230,6 +241,11 @@ int Config::handleActiveTask(unsigned long timeout_ms) {
   switch (active_task_.type_) {
   case task::ConfigTrackDetect: {
     if (!is_track_loaded_ || active_task_.data_ == 1) {
+      #ifdef DEBUG
+      if (logger_ != nullptr) {
+        logger_->debug("Detect track called");
+      }
+      #endif
       return taskConfigDetectTrack(timeout_ms);
     }
     this->taskComplete();
@@ -363,7 +379,18 @@ int Config::loadTrack(unsigned int idx) {
   }
 
   loaded_track_ = track_data;
-  trackGlobalWrite(loaded_track_);
+  GlobalTrackData track;
+
+  track.loaded_ = true;
+
+  Vec2 point_b =eqRectProjection(track_data.point_b_, track_data.point_a_);
+  track.center_ = vec2Midpoint(point_b, (Vec2){0.0f,0.0f});
+  float radius = max(10.0f, vecMod(point_b) * 1.25f);
+  track.circle_radius_sq_ = radius*radius;
+
+  track.root_ = track_data;
+  trackGlobalWrite(track);
   is_track_loaded_ = true;
+  router::send(module::All, task::AllTrackLoaded);
   return 0;
 }

src/modules/gps/gps.cpp

@@ -3,12 +3,13 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 
 #include "gps.h"
+#include "math.h"
+
+#include "data/track_store.h"
 
 #define MOD "modules/gps/gps.h"
 
-int Gps::push(const Task &task) {
-  return queue_.push(task);
-}
+int Gps::push(const Task &task) { return queue_.push(task); }
 
 Gps::Gps(HardwareSerial *data_stream)
     : gps_(nullptr), data_stream_(data_stream), logger_(nullptr) {
@@ -33,6 +34,7 @@ int Gps::init() {
 
 int Gps::loop(unsigned long timeout_ms) {
   unsigned long timeout = millis() + timeout_ms;
+  bool timed_out = false;
 
   while (data_stream_->available() > 0) {
     if (gps_->encode(data_stream_->read())) {
@@ -46,10 +48,105 @@ int Gps::loop(unsigned long timeout_ms) {
     }
 
     if (millis() > timeout) {
-      return 1;
+      timed_out = true;
+      break;
     }
   }
 
+  Task active;
+  int res = queue_.pop(active);
+  if (res == 0) {
+    if (active.target_ == module::Gps) {
+
+    } else if (active.target_ == module::All) {
+      switch (active.type_) {
+      case task::AllTrackLoaded: {
+#ifdef DEBUG
+        if (logger_ != nullptr) {
+          logger_->debug("GPS received track loaded sig");
+        }
+#endif
+        GlobalTrackData track;
+        trackGlobalRead(track);
+        lap_active_ = track.loaded_;
+        start_line_trigger_ = trigger_status::Idle;
+        gpsTriggerGlobalWrite(start_line_trigger_);
+        arm_sign_ = 0;
+        if (!track.loaded_) {
+          break;
+        }
+        track_point_a_ = track.root_.point_a_;
+        track_point_b_ = track.root_.point_b_;
+        track_vec_b_ = eqRectProjection(track_point_b_, track_point_a_);
+        track_vec_center_ = track.center_;
+        start_line_length_ = vecMod(track_vec_b_);
+        start_line_sq_dist_ = track.circle_radius_sq_;
+        break;
+      }
+
+      default:
+        break;
+      }
+    }
+  }
+
+  if (lap_active_ && millis() - last_check_ > check_interval_) {
+    unsigned long now = millis();
+    GpsData gps;
+    gpsGlobalRead(gps);
+    LatLng vehicle_position = {gps.lat_.value_, gps.lng_.value_};
+
+    Vec2 vehicle_pos_vec = eqRectProjection(vehicle_position, track_point_a_);
+    float center_dist_sq = vec2SqDist(track_vec_center_, vehicle_pos_vec);
+    float cross = vec2Cross(track_vec_b_, vehicle_pos_vec);
+    int sign = 0;
+    if (cross < 0) {
+      sign = -1;
+    } else if (cross > 0) {
+      sign = 1;
+    }
+
+    switch (start_line_trigger_) {
+    case trigger_status::Idle: {
+      if (center_dist_sq < start_line_sq_dist_) {
+        start_line_trigger_ = trigger_status::Armed;
+        gpsTriggerGlobalWrite(start_line_trigger_);
+        arm_sign_ = sign;
+        state_changed_at_ = now;
+      }
+      break;
+    };
+
+    case trigger_status::Armed: {
+      if (center_dist_sq > start_line_sq_dist_) {
+        start_line_trigger_ = trigger_status::Idle;
+        gpsTriggerGlobalWrite(start_line_trigger_);
+        arm_sign_ = 0;
+        state_changed_at_ = 0;
+      } else if ((sign == -1 && arm_sign_ == 1) || (sign == 1 && arm_sign_ == -1)) {
+        start_line_trigger_ = trigger_status::Trigd;
+        gpsTriggerGlobalWrite(start_line_trigger_);
+        arm_sign_ = 0;
+        state_changed_at_ = now;
+      }
+      break;
+    };
+
+    case trigger_status::Trigd: {
+      if (center_dist_sq > start_line_sq_dist_) {
+        start_line_trigger_ = trigger_status::Idle;
+        gpsTriggerGlobalWrite(start_line_trigger_);
+        arm_sign_ = 0;
+        state_changed_at_ = now;
+      }
+      break;
+    };
+
+    default:
+      break;
+    }
+    last_check_ = now;
+  }
   return 0;
 }
 

src/modules/gps/gps.h

@@ -11,8 +11,18 @@
 #include "base/ring_buffer.h"
 #include "base/module_base.h"
 #include "data/gps_store.h"
+#include "data/general_store.h"
 #include "base/router.h"
 
+namespace trigger_status {
+  
+enum TriggerStatus {
+  Idle = 0,
+  Armed = 1,
+  Trigd = 2,
+};
+}
+
 class Gps : public ModuleBase {
 private:
   TinyGPSPlus *gps_;
@@ -20,6 +30,21 @@ private:
   SystemLogger *logger_;
   RingBuffer<Task, 16> queue_;
   uint32_t last_fix_value_ = 0;
+  trigger_status::TriggerStatus start_line_trigger_ = trigger_status::Idle;
+
+  LatLng track_point_a_;
+  LatLng track_point_b_;
+  Vec2 track_vec_b_;
+  Vec2 track_vec_center_;
+  float start_line_length_ = 0;
+  float start_line_sq_dist_ = 0;
+
+  bool lap_active_ = false;
+  unsigned long last_check_ = 0;
+  unsigned long check_interval_ = 100;
+  unsigned long state_changed_at_ = 0;
+  int arm_sign_ = 0;
+
 
 public:
   int push(const Task &task) override;

src/modules/lap_counter/lap_counter.cpp

@@ -0,0 +1,14 @@
+// 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 "lap_counter.h"
+#include "base/router.h"
+#include "data/general_store.h"
+
+int LapCounter::push(const Task &task) { return queue_.push(task); }
+
+LapCounter::LapCounter() : logger_(nullptr) {};
+
+LapCounter::LapCounter(SystemLogger *logger) : logger_(logger) {};
+
+LapCounter::~LapCounter() {}

src/modules/lap_counter/lap_counter.h

@@ -0,0 +1,22 @@
+// 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
+#pragma once
+#include "base/module_base.h"
+#include "base/ring_buffer.h"
+#include "custom_types.h"
+#include "modules/logger/system_logger.h"
+
+class LapCounter : public ModuleBase {
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+
+public:
+  int push(const Task &task) override;
+  LapCounter();
+  LapCounter(SystemLogger *logger);
+  ~LapCounter();
+  int init();
+  int loop();
+};

src/modules/lcd/lcd.cpp

@@ -6,6 +6,8 @@
 
 #include <Wire.h>
 #include <string.h>
+#include "modules/gps/gps.h"
+#include "data/general_store.h"
 
 #define MOD "modules/lcd/lcd.h"
 
@@ -156,25 +158,53 @@ int Lcd::renderDriverPrimary() {
   float teng;
   tengGlobalRead(teng);
   
+  int line_trigger;
+  gpsTriggerGlobalRead(line_trigger);
+
   display_->setCursor(0,0);
   this->print("GPS:");
   if (gps_data.num_fix_ != 0) {
-    this->print("V");
+    this->print("Y");
   } else {
-    this->print("X");
+    this->print("N");
   }
 
-  display_->setCursor(3,2);
-  this->print("SPEED: ");
-  this->print(gps_data.speed_.value_);
+  display_->setCursor(7,0);
+  this->print("TRIG:");
+  switch (line_trigger)
+  {
+  case 0:
+    this->print("I");
+    break;
+  
+  case 1:
+    this->print("A");
+    break;
+
+  case 2:
+    this->print("T");
+    break;
+
+  default:
+    this->print("NULL");
+    break;
+  }
+  
+  display_->setCursor(0,2);
+  this->print("SPD:");
+  if (gps_data.speed_.valid_) {
+    this->print(gps_data.speed_.value_, 1);
+  } else {
+    this->print("NA");
+  }
   
   display_->setCursor(0,3);
-  this->print("Vbat:");
-  this->print(vbat);
+  this->print("V:");
+  this->print(vbat, 1);
 
   display_->setCursor(10,3);
-  this->print("Teng:");
-  this->print(teng);
+  this->print("T:");
+  this->print(teng, 1);
   
   return 0;
 }
@@ -188,6 +218,15 @@ int Lcd::renderMsgGpsFix() {
   return 0;
 }
 
+int Lcd::renderMsgGpsTrigger() {
+  this->clear();
+  display_->setCursor(6, 1);
+  this->print("GPS INFO");
+  display_->setCursor(4, 2);
+  this->print("LINE TRIGGER");
+  return 0;
+}
+
 int Lcd::renderMsgTrackDetectOk() {
   this->clear();
   display_->setCursor(6, 0);
@@ -195,11 +234,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);
-  this->print(track_data.name_);
+  display_->setCursor((20 - strlen(track_data.root_.name_)) / 2, 2);
+  this->print(track_data.root_.name_);
   return 0;
 }
 
@@ -445,7 +484,8 @@ int Lcd::loop(unsigned long timeout_ms) {
         break;
         
       case task::DisplayMsgEngineTempHigh:
-        activateMessage(screen::MsgEngineTempLow, next_task.data_);
+        activateMessage(screen::MsgEngineTempHigh, next_task.data_);
+        break;
 
       default:
         break;
@@ -489,6 +529,10 @@ int Lcd::loop(unsigned long timeout_ms) {
       this->renderMsgGpsFix();
       break;
 
+    case screen::MsgGpsTrigger:
+      this->renderMsgGpsTrigger();
+      break;
+
     case screen::MsgTrackDetectOk:
       this->renderMsgTrackDetectOk();
       break;

src/modules/lcd/lcd.h

@@ -23,6 +23,7 @@ enum LcdScreen : uint8_t {
   GpsDebug,
   DriverPrimary,
   MsgGpsFix,
+  MsgGpsTrigger,
   MsgTrackDetectOk,
   MsgConfigNoTracks,
   MsgBatteryLow,
@@ -67,6 +68,7 @@ private:
   int renderGpsDebug();
   int renderDriverPrimary();
   int renderMsgGpsFix();
+  int renderMsgGpsTrigger();
   int renderMsgTrackDetectOk();
   int renderMsgConfigNoTracks();
   int renderMsgBatteryLow();