Updated telemetry to use new time_stamp feature

src/base/task.h

@@ -16,6 +16,7 @@ enum Id : uint8_t {
     Thermocouple,
     Telemetry,
     LapCounter,
+    InjectionCounter,
     Count,
     Null,
     All,
@@ -35,6 +36,8 @@ enum Type : uint8_t {
     DisplayMsgBatteryLow,
     DisplayMsgEngineTempLow,
     DisplayMsgEngineTempHigh,
+    DisplayMsgLapCounterStart,
+    DisplayMsgLapCounterLapTime,
     ConfigTrackDetect,
     ConfigWriteTempTrack,
     ConfigTrackDelete,
@@ -46,6 +49,8 @@ enum Type : uint8_t {
     BatteryCal,
     AllConfigUpdated,
     AllTrackLoaded,
+    AllStartLineTriggered,
+    AllGpsFixOk,
 };
 
 } // namespace task

src/custom_types.cpp

@@ -68,4 +68,4 @@ float vec2SqDist(const Vec2& A, const Vec2& B) {
 
 float vec2Cross(const Vec2& B, const Vec2& M) {
   return B.x_ * M.y_ - B.y_ * M.x_;
-}
+}

src/custom_types.h

@@ -63,6 +63,8 @@ struct GpsData {
     GpsSubData lng_;
     GpsSubData speed_;
     GpsSubData course_;
+    uint32_t time_;
+    uint32_t time_write_time_;
     uint32_t num_fix_;
 };
 
@@ -75,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,30 +3,38 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 #include "general_store.h"
 
- volatile float vbat_global = 0;
- volatile float teng_global = 0;
- volatile int gps_trigger_global = 0;
+volatile float vbat_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) {
-     out = vbat_global;
- }
+void vbatGlobalRead(float &out) { out = vbat_global; }
 
- void vbatGlobalWrite(const float& in) {
-     vbat_global = in;
- }
+void vbatGlobalWrite(const float &in) { vbat_global = in; }
 
- void tengGlobalRead(float& out) {
-     out = teng_global;
- }
+void tengGlobalRead(float &out) { out = teng_global; }
 
- void tengGlobalWrite(const float& in) {
-     teng_global = in;
- }
+void tengGlobalWrite(const float &in) { teng_global = in; }
 
-void gpsTriggerGlobalRead(int& out) {
-    out = gps_trigger_global;
-}
+void gpsTriggerGlobalRead(int &out) { out = gps_trigger_global; }
 
-void gpsTriggerGlobalWrite(const int& in) {
-    gps_trigger_global = in;
-}
+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,10 +2,15 @@
 // 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);
@@ -15,3 +20,15 @@ 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/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/gps/gps.cpp

@@ -43,6 +43,7 @@ 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;
     }
@@ -128,6 +129,7 @@ int Gps::loop(unsigned long timeout_ms) {
         gpsTriggerGlobalWrite(start_line_trigger_);
         arm_sign_ = 0;
         state_changed_at_ = now;
+        router::send(module::All, task::AllStartLineTriggered);
       }
       break;
     };
@@ -173,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/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

@@ -3,7 +3,10 @@
 // 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); }
 
@@ -11,4 +14,96 @@ LapCounter::LapCounter() : logger_(nullptr) {};
 
 LapCounter::LapCounter(SystemLogger *logger) : logger_(logger) {};
 
-LapCounter::~LapCounter() {}
+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

@@ -6,11 +6,29 @@
 #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;

src/modules/lcd/lcd.cpp

@@ -4,10 +4,10 @@
 
 #include "lcd.h"
 
+#include "data/general_store.h"
+#include "modules/gps/gps.h"
 #include <Wire.h>
 #include <string.h>
-#include "modules/gps/gps.h"
-#include "data/general_store.h"
 
 #define MOD "modules/lcd/lcd.h"
 
@@ -60,20 +60,23 @@ void Lcd::print(int i, int base) {
 
 bool Lcd::isMessageTask(task::Type type) {
   switch (type) {
-    case task::DisplayMsgGpsFix:
-    case task::DisplayMsgTrackDetectOk:
-    case task::DisplayMsgConfigNoTracks:
-    case task::DisplayMsgBatteryLow:
-    case task::DisplayMsgEngineTempLow:
-    case task::DisplayMsgEngineTempHigh:
-      return true;
+  case task::DisplayMsgGpsFix:
+  case task::DisplayMsgTrackDetectOk:
+  case task::DisplayMsgConfigNoTracks:
+  case task::DisplayMsgBatteryLow:
+  case task::DisplayMsgEngineTempLow:
+  case task::DisplayMsgEngineTempHigh:
+  case task::DisplayMsgLapCounterLapTime:
+  case task::DisplayMsgLapCounterStart:
+    return true;
 
-    default:
-      return false;
+  default:
+    return false;
   }
 }
 
-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_;
   }
@@ -95,6 +98,7 @@ void Lcd::expireMessageIfNeeded(unsigned long now) {
     message_screen_ = screen::Blank;
     screen_ = data_screen_;
     force_render_ = true;
+    base_rendered_ = false;
   }
 }
 
@@ -148,159 +152,245 @@ int Lcd::renderGpsDebug() {
 }
 
 int Lcd::renderDriverPrimary() {
-  this->clear();
-  
+
   GpsData gps_data;
   gpsGlobalRead(gps_data);
-  
+
   float vbat;
   vbatGlobalRead(vbat);
   float teng;
   tengGlobalRead(teng);
-  
+
   int line_trigger;
   gpsTriggerGlobalRead(line_trigger);
 
-  display_->setCursor(0,0);
-  this->print("GPS:");
+  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:");
+
+    display_->setCursor(7, 0);
+    this->print("LAPS:");
+
+    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(4, 0);
   if (gps_data.num_fix_ != 0) {
     this->print("Y");
   } else {
     this->print("N");
   }
 
-  display_->setCursor(7,0);
-  this->print("TRIG:");
-  switch (line_trigger)
-  {
-  case 0:
-    this->print("I");
-    break;
-  
-  case 1:
-    this->print("A");
-    break;
+  display_->setCursor(12, 0);
+  if (num_laps < 10)
+    this->print('0');
+  this->print(num_laps, 10);
 
-  case 2:
-    this->print("T");
-    break;
+  display_->setCursor(4, 1);
+  if (inj_ctr < 100) {
+    this->print('0');
+    if (inj_ctr < 10) {
+      this->print('0');
+    }
+  }
+  this->print(inj_ctr, 10);
 
-  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("V:");
+  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(10,3);
-  this->print("T:");
+  display_->setCursor(12, 3);
   this->print(teng, 1);
-  
+
   return 0;
 }
 
 int Lcd::renderMsgGpsFix() {
-  this->clear();
-  display_->setCursor(6, 1);
-  this->print("GPS INFO");
-  display_->setCursor(7, 2);
-  this->print("FIX OK");
+  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() {
-  this->clear();
-  display_->setCursor(6, 1);
-  this->print("GPS INFO");
-  display_->setCursor(4, 2);
-  this->print("LINE TRIGGER");
+  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() {
-  this->clear();
-  display_->setCursor(6, 0);
-  this->print("GPS INFO");
-  display_->setCursor(3, 1);
-  this->print("TRACK DETECTED");
+  if (!base_rendered_) {
+    this->clear();
+    display_->setCursor(6, 0);
+    this->print("GPS INFO");
+    display_->setCursor(3, 1);
+    this->print("TRACK DETECTED");
 
-  GlobalTrackData track_data;
-  trackGlobalRead(track_data);
+    GlobalTrackData track_data;
+    trackGlobalRead(track_data);
 
-  display_->setCursor((20 - strlen(track_data.root_.name_)) / 2, 2);
-  this->print(track_data.root_.name_);
+    display_->setCursor((20 - strlen(track_data.root_.name_)) / 2, 2);
+    this->print(track_data.root_.name_);
+    base_rendered_ = true;
+  }
   return 0;
 }
 
 int Lcd::renderMsgConfigNoTracks() {
-  this->clear();
-  display_->setCursor(4, 1);
-  this->print("CONFIG INFO");
-  display_->setCursor(2, 2);
-  this->print("NO TRACKS LOADED");
+  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() {
-  this->clear();
-  display_->setCursor(6, 1);
-  this->print("WARNING");
-  display_->setCursor(6, 2);
-  this->print("VBAT LOW");
+  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() {
-  this->clear();
-  display_->setCursor(6, 1);
-  this->print("WARNING");
-  display_->setCursor(2, 2);
-  this->print("ENGINE TEMP LOW");
+  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() {
-  this->clear();
-  display_->setCursor(6, 1);
-  this->print("WARNING");
-  display_->setCursor(2, 2);
-  this->print("ENGINE TEMP HIGH");
+  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) {
-  return queue_.push(task);
+int Lcd::renderMsgLapCounterStart() {
+  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),
-      logger_(nullptr),
-      screen_(screen::Blank),
-      data_screen_(screen::Blank),
-      message_screen_(screen::Blank),
-      last_render_(0),
-      frame_duration_(2000) {
+    : display_cleared_(false), logger_(nullptr), screen_(screen::Blank),
+      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),
-      logger_(logger),
-      screen_(screen::Blank),
-      data_screen_(screen::Blank),
-      message_screen_(screen::Blank),
-      last_render_(0),
-      frame_duration_(2000) {
+    : display_cleared_(false), logger_(logger), screen_(screen::Blank),
+      data_screen_(screen::Blank), message_screen_(screen::Blank),
+      last_render_(0), frame_duration_(500) {
   display_ = new LiquidCrystal_I2C(0x27, 20, 4);
 }
 
@@ -447,48 +537,65 @@ int Lcd::loop(unsigned long timeout_ms) {
     }
 
     switch (next_task.type_) {
-      case task::DisplayGpsDebug:
-        data_screen_ = screen::GpsDebug;
-        if (!message_active_) {
-          screen_ = data_screen_;
-          force_render_ = true;
-        }
-        break;
+    case task::DisplayGpsDebug:
+      base_rendered_ = false;
+      data_screen_ = screen::GpsDebug;
+      if (!message_active_) {
+        screen_ = data_screen_;
+        force_render_ = true;
+      }
+      break;
 
-      case task::DisplayDriverPrimary:
-        data_screen_ = screen::DriverPrimary;
-        if (!message_active_) {
-          screen_ = data_screen_;
-          force_render_ = true;
-        }
-        break;
+    case task::DisplayDriverPrimary:
+      base_rendered_ = false;
+      data_screen_ = screen::DriverPrimary;
+      if (!message_active_) {
+        screen_ = data_screen_;
+        force_render_ = true;
+      }
+      break;
 
-      case task::DisplayMsgGpsFix:
-        activateMessage(screen::MsgGpsFix, next_task.data_);
-        break;
+    case task::DisplayMsgGpsFix:
+      base_rendered_ = false;
+      activateMessage(screen::MsgGpsFix, next_task.data_);
+      break;
 
-      case task::DisplayMsgTrackDetectOk:
-        activateMessage(screen::MsgTrackDetectOk, next_task.data_);
-        break;
+    case task::DisplayMsgTrackDetectOk:
+      base_rendered_ = false;
+      activateMessage(screen::MsgTrackDetectOk, next_task.data_);
+      break;
 
-      case task::DisplayMsgConfigNoTracks:
-        activateMessage(screen::MsgConfigNoTracks, next_task.data_);
-        break;
-        
-      case task::DisplayMsgBatteryLow:
-        activateMessage(screen::MsgBatteryLow, next_task.data_);
-        break;
-        
-      case task::DisplayMsgEngineTempLow:
-        activateMessage(screen::MsgEngineTempLow, next_task.data_);
-        break;
-        
-      case task::DisplayMsgEngineTempHigh:
-        activateMessage(screen::MsgEngineTempHigh, next_task.data_);
-        break;
+    case task::DisplayMsgConfigNoTracks:
+      base_rendered_ = false;
+      activateMessage(screen::MsgConfigNoTracks, next_task.data_);
+      break;
 
-      default:
-        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:
+      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;
     }
 
     now = millis();
@@ -513,48 +620,56 @@ int Lcd::loop(unsigned long timeout_ms) {
   }
 
   switch (screen_) {
-    case screen::Blank:
-      this->clear();
-      break;
+  case screen::Blank:
+    this->clear();
+    break;
 
-    case screen::GpsDebug:
-      this->renderGpsDebug();
-      break;
+  case screen::GpsDebug:
+    this->renderGpsDebug();
+    break;
 
-    case screen::DriverPrimary:
-      this->renderDriverPrimary();
-      break;
+  case screen::DriverPrimary:
+    this->renderDriverPrimary();
+    break;
 
-    case screen::MsgGpsFix:
-      this->renderMsgGpsFix();
-      break;
+  case screen::MsgGpsFix:
+    this->renderMsgGpsFix();
+    break;
 
-    case screen::MsgGpsTrigger:
-      this->renderMsgGpsTrigger();
-      break;
+  case screen::MsgGpsTrigger:
+    this->renderMsgGpsTrigger();
+    break;
 
-    case screen::MsgTrackDetectOk:
-      this->renderMsgTrackDetectOk();
-      break;
+  case screen::MsgTrackDetectOk:
+    this->renderMsgTrackDetectOk();
+    break;
 
-    case screen::MsgConfigNoTracks:
-      this->renderMsgConfigNoTracks();
-      break;
-      
-    case screen::MsgBatteryLow:
-      this->renderMsgBatteryLow();
-      break;
-      
-    case screen::MsgEngineTempLow:
-      this->renderMsgEngineTempLow();
-      break;
-      
-    case screen::MsgEngineTempHigh:
-      this->renderMsgEngineTempHigh();
-      break;
+  case screen::MsgConfigNoTracks:
+    this->renderMsgConfigNoTracks();
+    break;
 
-    default:
-      break;
+  case screen::MsgBatteryLow:
+    this->renderMsgBatteryLow();
+    break;
+
+  case screen::MsgEngineTempLow:
+    this->renderMsgEngineTempLow();
+    break;
+
+  case screen::MsgEngineTempHigh:
+    this->renderMsgEngineTempHigh();
+    break;
+
+  case screen::MsgLapCounterStart:
+    this->renderMsgLapCounterStart();
+    break;
+
+  case screen::MsgLapCounterLapTime:
+    this->renderMsgLapCounterLapTime();
+    break;
+
+  default:
+    break;
   }
 
   last_render_ = now;

src/modules/lcd/lcd.h

@@ -29,6 +29,8 @@ enum LcdScreen : uint8_t {
   MsgBatteryLow,
   MsgEngineTempLow,
   MsgEngineTempHigh,
+  MsgLapCounterStart,
+  MsgLapCounterLapTime,
 };
 
 } // namespace screen
@@ -49,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);
@@ -74,6 +77,8 @@ private:
   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/