Updated telemetry to use new time_stamp feature

src/base/task.h

@@ -15,6 +15,8 @@ enum Id : uint8_t {
     Battery,
     Thermocouple,
     Telemetry,
+    LapCounter,
+    InjectionCounter,
     Count,
     Null,
     All,
@@ -34,6 +36,8 @@ enum Type : uint8_t {
     DisplayMsgBatteryLow,
     DisplayMsgEngineTempLow,
     DisplayMsgEngineTempHigh,
+    DisplayMsgLapCounterStart,
+    DisplayMsgLapCounterLapTime,
     ConfigTrackDetect,
     ConfigWriteTempTrack,
     ConfigTrackDelete,
@@ -44,6 +48,9 @@ enum Type : uint8_t {
     ConfigTengSetHigh,
     BatteryCal,
     AllConfigUpdated,
+    AllTrackLoaded,
+    AllStartLineTriggered,
+    AllGpsFixOk,
 };
 
 } // 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_;
@@ -43,6 +63,8 @@ struct GpsData {
     GpsSubData lng_;
     GpsSubData speed_;
     GpsSubData course_;
+    uint32_t time_;
+    uint32_t time_write_time_;
     uint32_t num_fix_;
 };
 
@@ -55,3 +77,15 @@ template<typename T>
 inline void copyToVolatile(volatile T& dst, const T& src) {
     memcpy((void*)&dst, &src, sizeof(T));
 }
+
+static inline uint32_t hhmmsscc_to_cs(uint32_t t) {
+  uint32_t hours   =  t / 1000000;
+  uint32_t minutes = (t / 10000) % 100;
+  uint32_t seconds = (t / 100)   % 100;
+  uint32_t cs      =  t % 100;
+
+  return hours * 360000 +
+         minutes * 6000 +
+         seconds * 100 +
+         cs;
+}

src/data/general_store.cpp

@@ -3,21 +3,38 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 #include "general_store.h"
 
- volatile float vbat_global = 0;
+volatile float vbat_global = 0;
- volatile float teng_global = 0;
+volatile float teng_global = 0;
+volatile int gps_trigger_global = 0;
+volatile uint32_t last_lap_time_global = 0;
+volatile uint16_t lap_count_global = 0;
+volatile float speed_avg_global = 0;
+volatile uint16_t injection_ctr_global = 0;
 
- void vbatGlobalRead(float& out) {
+void vbatGlobalRead(float &out) { out = vbat_global; }
-     out = vbat_global;
- }
 
- void vbatGlobalWrite(const float& in) {
+void vbatGlobalWrite(const float &in) { vbat_global = in; }
-     vbat_global = in;
- }
 
- void tengGlobalRead(float& out) {
+void tengGlobalRead(float &out) { out = teng_global; }
-     out = teng_global;
- }
 
- void tengGlobalWrite(const float& in) {
+void tengGlobalWrite(const float &in) { teng_global = in; }
-     teng_global = in;
+
- }
+void gpsTriggerGlobalRead(int &out) { out = gps_trigger_global; }
+
+void gpsTriggerGlobalWrite(const int &in) { gps_trigger_global = in; }
+
+void lastLapTimeGlobalRead(uint32_t &out) { out = last_lap_time_global; }
+
+void lastLapTimeGlobalWrite(const uint32_t &in) { last_lap_time_global = in; }
+
+void lapCountGlobalRead(uint16_t &out) { out = lap_count_global; }
+
+void lapCountGlobalWrite(const uint16_t &in) { lap_count_global = in; }
+
+void speedAvgGlobalRead(float &out) { out = speed_avg_global; }
+
+void speedAvgGlobalWrite(const float &in) { speed_avg_global = in; }
+
+void injectionCtrGlobalRead(uint16_t &out) { out = injection_ctr_global; }
+
+void injectionCtrGlobalWrite(const uint16_t &in) { injection_ctr_global = in; }

src/data/general_store.h

@@ -2,12 +2,33 @@
 // Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
 // SPDX-License-Identifier: GPL-3.0-or-later
 #pragma once
+#include <inttypes.h>
 
 extern volatile float vbat_global;
 extern volatile float teng_global;
+extern volatile int gps_trigger_global;
+extern volatile uint32_t last_lap_time_global;
+extern volatile uint16_t lap_count_global;
+extern volatile float speed_avg_global;
+extern volatile uint16_t injection_ctr_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);
+
+void lastLapTimeGlobalRead(uint32_t& out);
+void lastLapTimeGlobalWrite(const uint32_t& in);
+
+void lapCountGlobalRead(uint16_t& out);
+void lapCountGlobalWrite(const uint16_t& in);
+
+void speedAvgGlobalRead(float& out);
+void speedAvgGlobalWrite(const float& in);
+
+void injectionCtrGlobalRead(uint16_t& out);
+void injectionCtrGlobalWrite(const uint16_t& 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);
+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/main.cpp

@@ -16,6 +16,8 @@
 #include "modules/battery/battery.h"
 #include "modules/thermocouple/thermocouple.h"
 #include "modules/telemetry/telemetry.h"
+#include "modules/lap_counter/lap_counter.h"
+#include "modules/injection_counter/injection_counter.h"
 
 
 SystemLogger *logger = new SystemLogger(&Serial);
@@ -27,6 +29,8 @@ Cmd *command_handler = new Cmd(&Serial, logger);
 Battery *battery_module = new Battery(logger);
 Thermocouple *thermocouple_module = new Thermocouple(logger);
 Telemetry *telemetry_module = new Telemetry(&Serial1, logger);
+LapCounter *lap_counter_modules = new LapCounter(logger);
+InjectionCounter *inj_counter_module = new InjectionCounter(logger);
 
 
 
@@ -40,6 +44,8 @@ void setup() {
   module_registry[module::Battery] = battery_module;
   module_registry[module::Thermocouple] = thermocouple_module;
   module_registry[module::Telemetry] = telemetry_module;
+  module_registry[module::LapCounter] = lap_counter_modules;
+  module_registry[module::InjectionCounter] = inj_counter_module;
 
   display->init();
   display->printMessage("Starting Initialization");
@@ -63,6 +69,7 @@ void setup() {
 
   display->printMessage("GPS Init...");
   gps_module->init();
+  lap_counter_modules->init();
   delay(750);
   display->printMessage("GPS Init Complete");
   delay(750);
@@ -70,6 +77,7 @@ void setup() {
   display->printMessage("Sensors Init...");
   battery_module->init();
   thermocouple_module->init();
+  inj_counter_module->init();
   delay(750);
   display->printMessage("Sensors Init Complete");
   delay(750);
@@ -84,10 +92,12 @@ void setup() {
 
 void loop() {
   gps_module->loop();
+  lap_counter_modules->loop();
   display->loop();
   command_handler->parseTask();
   system_config->loop();
   battery_module->loop();
   thermocouple_module->loop();
   telemetry_module->loop();
+  inj_counter_module->loop();
 }

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("\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
 
@@ -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) {
+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) {
@@ -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())) {
@@ -41,15 +43,112 @@ int Gps::loop(unsigned long timeout_ms) {
       if (last_fix_value_ == 0 && current_fix_value > 0) {
         router::send(module::Lcd, task::DisplayMsgGpsFix, 2000);
         router::send(module::Config, task::ConfigTrackDetect);
+        router::send(module::All, task::AllGpsFixOk);
       }
       last_fix_value_ = current_fix_value;
     }
 
     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;
+        router::send(module::All, task::AllStartLineTriggered);
+      }
+      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;
 }
 
@@ -76,6 +175,9 @@ GpsData Gps::getData() {
   output.course_.valid_ = gps_->course.isValid();
   output.course_.value_ = gps_->course.deg();
 
+  output.time_ = gps_->time.value();
+  output.time_write_time_ = millis() - gps_->time.age();
+
   output.num_fix_ = gps_->sentencesWithFix();
 
   return output;

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/injection_counter/injection_counter.cpp

@@ -0,0 +1,45 @@
+// 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 "injection_counter.h"
+#include "base/router.h"
+#include "data/general_store.h"
+
+int InjectionCounter::push(const Task &task) { return queue_.push(task); }
+
+InjectionCounter::InjectionCounter() : logger_(nullptr) {};
+
+InjectionCounter::InjectionCounter(SystemLogger *logger) : logger_(logger) {};
+
+InjectionCounter::~InjectionCounter() {};
+
+int InjectionCounter::init() { pinMode(INJ_GPIO, INPUT); }
+
+int InjectionCounter::loop() {
+  unsigned long now = millis();
+
+  if (now - last_check_ >= check_interval_) {
+    last_check_ = now;
+
+    int val = digitalRead(INJ_GPIO);
+
+    if (val == 1 && last_switch_ == 0 && !waiting_debounce_) {
+      waiting_debounce_ = true;
+      last_switch_time_ = now;
+    }
+
+    if (waiting_debounce_) {
+      if (now - last_switch_time_ >= debounce_) {
+        if (digitalRead(INJ_GPIO) == 1) {
+          counter_++;
+          injectionCtrGlobalWrite(counter_);
+        }
+        waiting_debounce_ = false;
+      }
+    }
+
+    last_switch_ = val;
+  }
+
+  return 0;
+}

src/modules/injection_counter/injection_counter.h

@@ -0,0 +1,30 @@
+// 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 "modules/logger/system_logger.h"
+
+#define INJ_GPIO 37
+
+class InjectionCounter : public ModuleBase {
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+  unsigned long last_check_;
+  unsigned long check_interval_ = 100;
+  unsigned long debounce_ = 100;
+  int last_switch_ = 0;
+  int last_switch_time_ = 0;
+  bool waiting_debounce_ = 0;
+  uint16_t counter_ = 0;
+
+public:
+  int push(const Task &task) override;
+  InjectionCounter();
+  InjectionCounter(SystemLogger* logger);
+  ~InjectionCounter();
+  int init();
+  int loop();
+};

src/modules/lap_counter/lap_counter.cpp

@@ -0,0 +1,109 @@
+// 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 "custom_types.h"
+#include "data/general_store.h"
+#include "data/gps_store.h"
+#include "data/track_store.h"
+
+int LapCounter::push(const Task &task) { return queue_.push(task); }
+
+LapCounter::LapCounter() : logger_(nullptr) {};
+
+LapCounter::LapCounter(SystemLogger *logger) : logger_(logger) {};
+
+LapCounter::~LapCounter() {}
+
+int LapCounter::init() {
+  counting_ = false;
+  count_ = false;
+  return 0;
+}
+
+int LapCounter::loop() {
+  Task active_task;
+  int res = queue_.pop(active_task);
+
+  if (res == 0) {
+    if (active_task.target_ == module::LapCounter) {
+
+    } else if (active_task.target_ == module::All) {
+      switch (active_task.type_) {
+
+      case task::AllStartLineTriggered: {
+        GpsData gps;
+        gpsGlobalRead(gps);
+
+        uint32_t base_cs = hhmmsscc_to_cs(gps.time_);
+
+        uint32_t elapsed_cs = (millis() - gps.time_write_time_) / 10;
+
+        uint32_t time_cs = base_cs + elapsed_cs;
+
+        if (!counting_) {
+          counting_ = true;
+          last_trigger_time_ = time_cs;
+
+          router::send(module::Lcd, task::DisplayMsgLapCounterStart, 1000);
+
+        } else {
+          uint32_t lap_time = time_cs - last_trigger_time_;
+
+          lap_times_idx_ = (lap_times_idx_ + 1) & 63;
+
+          lap_times_[lap_times_idx_] = lap_time;
+          count_++;
+          lapCountGlobalWrite(count_);
+
+          last_trigger_time_ = time_cs;
+
+          lastLapTimeGlobalWrite(lap_time);
+
+          router::send(module::Lcd, task::DisplayMsgLapCounterLapTime, 1000);
+        }
+
+        break;
+      }
+
+      case task::AllGpsFixOk: {
+        average_enabled_ = true;
+#ifdef DEBUG
+        if (logger_ != nullptr) {
+          logger_->debug("Enabled average counter");
+        }
+#endif
+        break;
+      }
+
+      default:
+        break;
+      }
+    }
+  }
+
+  if (millis() - last_average_time_ > average_loop_time_ && average_enabled_) {
+    GpsData gps;
+    gpsGlobalRead(gps);
+    unsigned long now = millis();
+    float dt = (now - last_average_time_) / 1000.0f;
+    float speed = gps.speed_.value_;
+    if (speed < 1) speed = 0;
+
+    continuous_time_sum_ += dt;
+    if (last_average_time_ == 0) {
+      continuous_speed_sum_ += speed * dt;
+    } else {
+      continuous_speed_sum_ +=
+          (speed + previous_speed_) * 0.5f * dt;
+    }
+    previous_speed_ = speed;
+
+    speedAvgGlobalWrite(continuous_speed_sum_ / continuous_time_sum_);
+
+    last_average_time_ = now;
+  }
+
+  return 0;
+}

src/modules/lap_counter/lap_counter.h

@@ -0,0 +1,40 @@
+// 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"
+#include <inttypes.h>
+
+class LapCounter : public ModuleBase {
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+  
+  bool counting_ = false;
+  uint16_t count_ = 0;
+  uint32_t last_trigger_time_ = 0;
+  uint32_t lap_times_[64];
+  int16_t lap_times_idx_ = -1;
+  
+
+  bool average_enabled_ = false;
+  unsigned long average_loop_time_ = 250;
+  unsigned long last_average_time_ = 0;
+  float continuous_speed_sum_ = 0;
+  float lap_speed_sum_ = 0;
+  float continuous_time_sum_ = 0;
+  float lap_time_sum_ = 0;
+  float previous_speed_ = 0;
+
+
+public:
+  int push(const Task &task) override;
+  LapCounter();
+  LapCounter(SystemLogger *logger);
+  ~LapCounter();
+  int init();
+  int loop();
+};

src/modules/lcd/lcd.cpp

@@ -4,6 +4,8 @@
 
 #include "lcd.h"
 
+#include "data/general_store.h"
+#include "modules/gps/gps.h"
 #include <Wire.h>
 #include <string.h>
 
@@ -64,6 +66,8 @@ bool Lcd::isMessageTask(task::Type type) {
   case task::DisplayMsgBatteryLow:
   case task::DisplayMsgEngineTempLow:
   case task::DisplayMsgEngineTempHigh:
+  case task::DisplayMsgLapCounterLapTime:
+  case task::DisplayMsgLapCounterStart:
     return true;
 
   default:
@@ -71,7 +75,8 @@ bool Lcd::isMessageTask(task::Type type) {
   }
 }
 
-void Lcd::activateMessage(screen::LcdScreen msg_screen, unsigned long duration_ms) {
+void Lcd::activateMessage(screen::LcdScreen msg_screen,
+                          unsigned long duration_ms) {
   if (duration_ms == 0) {
     duration_ms = frame_duration_;
   }
@@ -93,6 +98,7 @@ void Lcd::expireMessageIfNeeded(unsigned long now) {
     message_screen_ = screen::Blank;
     screen_ = data_screen_;
     force_render_ = true;
+    base_rendered_ = false;
   }
 }
 
@@ -146,7 +152,6 @@ int Lcd::renderGpsDebug() {
 }
 
 int Lcd::renderDriverPrimary() {
-  this->clear();
 
   GpsData gps_data;
   gpsGlobalRead(gps_data);
@@ -156,112 +161,236 @@ int Lcd::renderDriverPrimary() {
   float teng;
   tengGlobalRead(teng);
 
-  display_->setCursor(0,0);
+  int line_trigger;
+  gpsTriggerGlobalRead(line_trigger);
+
+  uint16_t num_laps;
+  lapCountGlobalRead(num_laps);
+
+  float average_speed;
+  speedAvgGlobalRead(average_speed);
+
+  uint16_t inj_ctr;
+  injectionCtrGlobalRead(inj_ctr);
+
+  if (!base_rendered_) {
+    this->clear();
+    display_->setCursor(0, 0);
     this->print("GPS:");
-  if (gps_data.num_fix_ != 0) {
+
-    this->print("V");
+    display_->setCursor(7, 0);
-  } else {
+    this->print("LAPS:");
-    this->print("X");
+
+    display_->setCursor(0, 1);
+    this->print("INJ:");
+
+    display_->setCursor(0, 2);
+    this->print("SPD:");
+
+    display_->setCursor(10, 2);
+    this->print("AVG:");
+
+    display_->setCursor(0, 3);
+    this->print("V:");
+
+    display_->setCursor(10, 3);
+    this->print("T:");
+    base_rendered_ = true;
   }
 
-  display_->setCursor(3,2);
+  display_->setCursor(4, 0);
-  this->print("SPEED: ");
+  if (gps_data.num_fix_ != 0) {
-  this->print(gps_data.speed_.value_);
+    this->print("Y");
+  } else {
+    this->print("N");
+  }
 
-  display_->setCursor(0,3);
+  display_->setCursor(12, 0);
-  this->print("Vbat:");
+  if (num_laps < 10)
-  this->print(vbat);
+    this->print('0');
+  this->print(num_laps, 10);
 
-  display_->setCursor(10,3);
+  display_->setCursor(4, 1);
-  this->print("Teng:");
+  if (inj_ctr < 100) {
-  this->print(teng);
+    this->print('0');
+    if (inj_ctr < 10) {
+      this->print('0');
+    }
+  }
+  this->print(inj_ctr, 10);
+
+  display_->setCursor(4, 2);
+  if (gps_data.speed_.value_ < 10.0)
+    this->print('0');
+  this->print(gps_data.speed_.value_, 1);
+
+  display_->setCursor(14, 2);
+  if (average_speed < 10.0)
+    this->print('0');
+  this->print(average_speed, 1);
+
+  display_->setCursor(2, 3);
+  this->print(vbat, 1);
+
+  display_->setCursor(12, 3);
+  this->print(teng, 1);
 
   return 0;
 }
 
 int Lcd::renderMsgGpsFix() {
+  if (!base_rendered_) {
     this->clear();
     display_->setCursor(6, 1);
     this->print("GPS INFO");
     display_->setCursor(7, 2);
     this->print("FIX OK");
+    base_rendered_ = true;
+  }
+  return 0;
+}
+
+int Lcd::renderMsgGpsTrigger() {
+  if (!base_rendered_) {
+    this->clear();
+    display_->setCursor(6, 1);
+    this->print("GPS INFO");
+    display_->setCursor(4, 2);
+    this->print("LINE TRIGGER");
+    base_rendered_ = true;
+  }
   return 0;
 }
 
 int Lcd::renderMsgTrackDetectOk() {
+  if (!base_rendered_) {
     this->clear();
     display_->setCursor(6, 0);
     this->print("GPS INFO");
     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_);
+    base_rendered_ = true;
+  }
   return 0;
 }
 
 int Lcd::renderMsgConfigNoTracks() {
+  if (!base_rendered_) {
     this->clear();
     display_->setCursor(4, 1);
     this->print("CONFIG INFO");
     display_->setCursor(2, 2);
     this->print("NO TRACKS LOADED");
+    base_rendered_ = true;
+  }
   return 0;
 }
 
 int Lcd::renderMsgBatteryLow() {
+  if (!base_rendered_) {
     this->clear();
     display_->setCursor(6, 1);
     this->print("WARNING");
     display_->setCursor(6, 2);
     this->print("VBAT LOW");
+    base_rendered_ = true;
+  }
   return 0;
 }
 
 int Lcd::renderMsgEngineTempLow() {
+  if (!base_rendered_) {
     this->clear();
     display_->setCursor(6, 1);
     this->print("WARNING");
     display_->setCursor(2, 2);
     this->print("ENGINE TEMP LOW");
+    base_rendered_ = true;
+  }
   return 0;
 }
 
 int Lcd::renderMsgEngineTempHigh() {
+  if (!base_rendered_) {
     this->clear();
     display_->setCursor(6, 1);
     this->print("WARNING");
     display_->setCursor(2, 2);
     this->print("ENGINE TEMP HIGH");
+    base_rendered_ = true;
+  }
   return 0;
 }
 
-int Lcd::push(const Task &task) {
+int Lcd::renderMsgLapCounterStart() {
-  return queue_.push(task);
+  if (!base_rendered_) {
+    this->clear();
+    display_->setCursor(5, 1);
+    this->print("LAP COUNTER");
+    display_->setCursor(6, 2);
+    this->print("STARTED");
+    base_rendered_ = true;
+  }
+  return 0;
 }
 
+int Lcd::renderMsgLapCounterLapTime() {
+
+  if (!base_rendered_) {
+    uint32_t time_cs;
+    lastLapTimeGlobalRead(time_cs);
+
+    uint32_t minutes = (time_cs / 6000);
+    uint32_t seconds = (time_cs / 100) % 60;
+    uint32_t cs = time_cs % 100;
+
+    this->clear();
+
+    display_->setCursor(6, 1);
+    this->print("LAP TIME");
+
+    display_->setCursor(6, 2);
+
+    if (minutes < 10)
+      this->print('0');
+    this->print(minutes, 10);
+
+    this->print(':');
+
+    if (seconds < 10)
+      this->print('0');
+    this->print(seconds, 10);
+
+    this->print('.');
+
+    if (cs < 10)
+      this->print('0');
+    this->print(cs, 10);
+    base_rendered_ = true;
+  }
+
+  return 0;
+}
+
+int Lcd::push(const Task &task) { return queue_.push(task); }
+
 Lcd::Lcd()
-    : display_cleared_(false),
+    : display_cleared_(false), logger_(nullptr), screen_(screen::Blank),
-      logger_(nullptr),
+      data_screen_(screen::Blank), message_screen_(screen::Blank),
-      screen_(screen::Blank),
+      last_render_(0), frame_duration_(2000) {
-      data_screen_(screen::Blank),
-      message_screen_(screen::Blank),
-      last_render_(0),
-      frame_duration_(2000) {
   display_ = new LiquidCrystal_I2C(0x27, 20, 4);
 }
 
 Lcd::Lcd(SystemLogger *logger)
-    : display_cleared_(false),
+    : display_cleared_(false), logger_(logger), screen_(screen::Blank),
-      logger_(logger),
+      data_screen_(screen::Blank), message_screen_(screen::Blank),
-      screen_(screen::Blank),
+      last_render_(0), frame_duration_(500) {
-      data_screen_(screen::Blank),
-      message_screen_(screen::Blank),
-      last_render_(0),
-      frame_duration_(2000) {
   display_ = new LiquidCrystal_I2C(0x27, 20, 4);
 }
 
@@ -409,6 +538,7 @@ int Lcd::loop(unsigned long timeout_ms) {
 
     switch (next_task.type_) {
     case task::DisplayGpsDebug:
+      base_rendered_ = false;
       data_screen_ = screen::GpsDebug;
       if (!message_active_) {
         screen_ = data_screen_;
@@ -417,6 +547,7 @@ int Lcd::loop(unsigned long timeout_ms) {
       break;
 
     case task::DisplayDriverPrimary:
+      base_rendered_ = false;
       data_screen_ = screen::DriverPrimary;
       if (!message_active_) {
         screen_ = data_screen_;
@@ -425,27 +556,43 @@ int Lcd::loop(unsigned long timeout_ms) {
       break;
 
     case task::DisplayMsgGpsFix:
+      base_rendered_ = false;
       activateMessage(screen::MsgGpsFix, next_task.data_);
       break;
 
     case task::DisplayMsgTrackDetectOk:
+      base_rendered_ = false;
       activateMessage(screen::MsgTrackDetectOk, next_task.data_);
       break;
 
     case task::DisplayMsgConfigNoTracks:
+      base_rendered_ = false;
       activateMessage(screen::MsgConfigNoTracks, next_task.data_);
       break;
 
     case task::DisplayMsgBatteryLow:
+      base_rendered_ = false;
       activateMessage(screen::MsgBatteryLow, next_task.data_);
       break;
 
     case task::DisplayMsgEngineTempLow:
+      base_rendered_ = false;
       activateMessage(screen::MsgEngineTempLow, next_task.data_);
       break;
 
     case task::DisplayMsgEngineTempHigh:
-        activateMessage(screen::MsgEngineTempLow, next_task.data_);
+      base_rendered_ = false;
+      activateMessage(screen::MsgEngineTempHigh, next_task.data_);
+      break;
+
+    case task::DisplayMsgLapCounterStart:
+      base_rendered_ = false;
+      activateMessage(screen::MsgLapCounterStart, next_task.data_);
+      break;
+
+    case task::DisplayMsgLapCounterLapTime:
+      base_rendered_ = false;
+      activateMessage(screen::MsgLapCounterLapTime, next_task.data_);
 
     default:
       break;
@@ -489,6 +636,10 @@ int Lcd::loop(unsigned long timeout_ms) {
     this->renderMsgGpsFix();
     break;
 
+  case screen::MsgGpsTrigger:
+    this->renderMsgGpsTrigger();
+    break;
+
   case screen::MsgTrackDetectOk:
     this->renderMsgTrackDetectOk();
     break;
@@ -509,6 +660,14 @@ int Lcd::loop(unsigned long timeout_ms) {
     this->renderMsgEngineTempHigh();
     break;
 
+  case screen::MsgLapCounterStart:
+    this->renderMsgLapCounterStart();
+    break;
+
+  case screen::MsgLapCounterLapTime:
+    this->renderMsgLapCounterLapTime();
+    break;
+
   default:
     break;
   }

src/modules/lcd/lcd.h

@@ -23,11 +23,14 @@ enum LcdScreen : uint8_t {
   GpsDebug,
   DriverPrimary,
   MsgGpsFix,
+  MsgGpsTrigger,
   MsgTrackDetectOk,
   MsgConfigNoTracks,
   MsgBatteryLow,
   MsgEngineTempLow,
   MsgEngineTempHigh,
+  MsgLapCounterStart,
+  MsgLapCounterLapTime,
 };
 
 } // namespace screen
@@ -48,6 +51,7 @@ private:
   RingBuffer<Task, 16> queue_;
   Task deferred_task_{};
   bool deferred_task_valid_ = false;
+  bool base_rendered_ = false;
 
   void clear();
   void print(const String &msg);
@@ -67,11 +71,14 @@ private:
   int renderGpsDebug();
   int renderDriverPrimary();
   int renderMsgGpsFix();
+  int renderMsgGpsTrigger();
   int renderMsgTrackDetectOk();
   int renderMsgConfigNoTracks();
   int renderMsgBatteryLow();
   int renderMsgEngineTempLow();
   int renderMsgEngineTempHigh();
+  int renderMsgLapCounterStart();
+  int renderMsgLapCounterLapTime();
 
 public:
   int push(const Task &task) override;

src/modules/telemetry/telemetry.cpp

@@ -28,6 +28,7 @@ int Telemetry::loop() {
         vbatGlobalRead(packet.vbat);
         GpsData gps;
         gpsGlobalRead(gps);
+        packet.time_stamp = gps.time_;
         packet.lat = gps.lat_.value_;
         packet.lng = gps.lng_.value_;
         packet.speed = gps.speed_.value_;

src/telemetry_common

@@ -1 +1 @@
-/home/hector/projects/Exergie/TelemetryCommon/
+/home/hector/projects/Exergie/TelemetryCommon/cpp/