Updated telemetry to use new time_stamp feature

.clang-format

@@ -0,0 +1,2 @@
+BasedOnStyle: LLVM
+BreakBeforeBraces: Attach

platformio.ini

@@ -12,3 +12,11 @@
 platform = atmelavr
 board = megaatmega2560
 framework = arduino
+monitor_speed = 115200
+monitor_echo = yes
+monitor_eol = CRLF
+monitor_filters = send_on_enter
+lib_deps = 
+	marcoschwartz/LiquidCrystal_I2C@^1.1.4
+	mikalhart/TinyGPSPlus@^1.1.0
+	adafruit/MAX6675 library@^1.1.2

src/base/module_base.h

@@ -0,0 +1,10 @@
+// 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/task.h"
+
+class ModuleBase {
+public:
+    virtual int push(const Task& task) = 0;
+};

src/base/modules.cpp

@@ -0,0 +1,6 @@
+// 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 "base/modules.h"
+
+ModuleBase* module_registry[module::Count] = {nullptr};

src/base/modules.h

@@ -0,0 +1,9 @@
+// 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/task.h"
+
+extern ModuleBase* module_registry[module::Count];

src/base/ring_buffer.h

@@ -0,0 +1,38 @@
+// 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 <inttypes.h>
+
+template<typename T, uint8_t SIZE>
+class RingBuffer {
+    private:
+        T buffer_[SIZE];
+        volatile uint8_t head_ = 0;
+        volatile uint8_t tail_ = 0;
+        volatile uint8_t count_ = 0;
+    public:
+        int push(const T& item) {
+            if (count_ == SIZE) {
+                return 1;
+            }
+            
+            buffer_[head_] = item;
+            head_ = (head_++) % SIZE;
+            count_++;
+            return 0;
+        }
+        int pop(T& item) {
+            if (count_ == 0) {
+                return 1;
+            }
+            
+            item = buffer_[tail_];
+            tail_ = (tail_++) % SIZE;
+            count_--;
+            return 0;
+        }
+        bool isEmpty() const { return count_ == 0; }
+        bool isFull() const { return count_ == SIZE; }
+        uint8_t size() const { return count_; }
+};

src/base/router.cpp

@@ -0,0 +1,42 @@
+// 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 "router.h"
+
+namespace router {
+int send(const Task &task) {
+  if (task.target_ == module::All) {
+    int ret = 0;
+    for (size_t index = 0; index < module::Count; index++) {
+
+      ModuleBase *module_ptr = module_registry[index];
+
+      if (module_ptr == nullptr) {
+        continue;
+      }
+
+      if (module_ptr->push(task) != 0) {
+        ret = 1;
+      }
+    }
+    return ret;
+  }
+  if (task.target_ >= module::Count) {
+    return 1;
+  }
+
+  ModuleBase *module_ptr = module_registry[task.target_];
+
+  if (module_ptr == nullptr) {
+    return 1;
+  }
+
+  return module_ptr->push(task);
+}
+
+int send(module::Id target, task::Type type, uint32_t data) {
+  Task task{target, type, data};
+  return send(task);
+}
+} // namespace router

src/base/router.h

@@ -0,0 +1,13 @@
+// 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/task.h"
+#include "base/module_base.h"
+#include "base/modules.h"
+
+namespace router {
+    int send(const Task& task);
+    int send(module::Id target, task::Type type, uint32_t data = 0);
+}

src/base/task.h

@@ -0,0 +1,67 @@
+// 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 <inttypes.h>
+#include <Arduino.h>
+
+namespace module {
+
+enum Id : uint8_t {
+    Cmd,
+    Config,
+    Gps,
+    Lcd,
+    Battery,
+    Thermocouple,
+    Telemetry,
+    LapCounter,
+    InjectionCounter,
+    Count,
+    Null,
+    All,
+};
+
+} // namespace module
+
+namespace task {
+
+enum Type : uint8_t {
+    Null,
+    DisplayGpsDebug,
+    DisplayDriverPrimary,
+    DisplayMsgGpsFix,
+    DisplayMsgTrackDetectOk,
+    DisplayMsgConfigNoTracks,
+    DisplayMsgBatteryLow,
+    DisplayMsgEngineTempLow,
+    DisplayMsgEngineTempHigh,
+    DisplayMsgLapCounterStart,
+    DisplayMsgLapCounterLapTime,
+    ConfigTrackDetect,
+    ConfigWriteTempTrack,
+    ConfigTrackDelete,
+    ConfigReset,
+    ConfigVbatCalSet,
+    ConfigVbatSetLow,
+    ConfigTengSetLow,
+    ConfigTengSetHigh,
+    BatteryCal,
+    AllConfigUpdated,
+    AllTrackLoaded,
+    AllStartLineTriggered,
+    AllGpsFixOk,
+};
+
+} // namespace task
+
+struct Task {
+    Task(module::Id target = module::Null,
+         task::Type type = task::Null,
+         uint32_t data = 0)
+        : target_(target), type_(type), data_(data) {}
+
+    module::Id target_;
+    task::Type type_;
+    uint32_t data_;
+};

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

@@ -0,0 +1,91 @@
+// 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 <inttypes.h>
+#include <string.h>
+
+#define CONFIG_MAGIC 0xBEEF
+
+struct VehicleConfig {
+    uint16_t magic_ = CONFIG_MAGIC;
+    bool auto_detect_track_ = true;
+    uint8_t track_fallback_ = 0;
+    bool track_slot_occupied_[8] = {false};
+    float vbat_calibration_ = 0;
+    float vbat_low_ = 0;
+    float teng_low_ = 0;
+    float teng_high_ = 0;
+};
+
+struct LatLng {
+    float lat_;
+    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_;
+    char name_[21];
+    LatLng point_a_;
+    LatLng point_b_;
+};
+
+struct GlobalTrackData {
+    bool loaded_ = false;
+    Vec2 center_;
+    float circle_radius_sq_;
+    TrackData root_;
+};
+
+struct GpsSubData {
+    uint32_t age_;
+    bool valid_;
+    float value_;
+};
+
+struct GpsData {
+    GpsSubData altitude_;
+    GpsSubData lat_;
+    GpsSubData lng_;
+    GpsSubData speed_;
+    GpsSubData course_;
+    uint32_t time_;
+    uint32_t time_write_time_;
+    uint32_t num_fix_;
+};
+
+template<typename T>
+inline void copyFromVolatile(T& dst, const volatile T& src) {
+    memcpy(&dst, (const void*)&src, sizeof(T));
+}
+
+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/config_store.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 "data/config_store.h"
+
+ volatile VehicleConfig config_global = {};
+
+ void configGlobalRead(VehicleConfig& out) {
+     copyFromVolatile(out, config_global);
+ }
+
+ void configGlobalWrite(const VehicleConfig& in) {
+     copyToVolatile(config_global, in);
+ }

src/data/config_store.h

@@ -0,0 +1,12 @@
+// 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 "custom_types.h"
+#include <EEPROM.h>
+
+extern volatile VehicleConfig config_global;
+
+void configGlobalRead(VehicleConfig& out);
+void configGlobalWrite(const VehicleConfig& in);

src/data/eeprom_layout.h

@@ -0,0 +1,25 @@
+// 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 <inttypes.h>
+
+namespace eeprom_layout {
+
+static const uint16_t TOTAL_BYTES = 4096;
+static const uint16_t CONFIG_ADDR = 0;
+static const uint16_t CONFIG_RESERVED_BYTES = 256;
+static const uint16_t TRACK_SLOTS = 8;
+static const uint16_t TRACK_SLOT_BYTES = 128;
+static const uint16_t TRACK_BASE_ADDR = CONFIG_ADDR + CONFIG_RESERVED_BYTES;
+static const uint16_t TRACK_END_ADDR = TRACK_BASE_ADDR + (TRACK_SLOTS * TRACK_SLOT_BYTES);
+static const uint16_t FREE_AFTER_TRACKS = TOTAL_BYTES - TRACK_END_ADDR;
+
+static_assert(TRACK_END_ADDR <= TOTAL_BYTES, "EEPROM layout exceeds physical storage");
+
+inline uint16_t trackSlotAddr(uint8_t idx) {
+  return TRACK_BASE_ADDR + ((idx - 1) * TRACK_SLOT_BYTES);
+}
+
+} // namespace eeprom_layout

src/data/general_store.cpp

@@ -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
+#include "general_store.h"
+
+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 vbatGlobalWrite(const float &in) { vbat_global = in; }
+
+void tengGlobalRead(float &out) { out = teng_global; }
+
+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; }
+
+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

@@ -0,0 +1,34 @@
+// 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 <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/gps_store.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 "gps_store.h"
+
+ volatile GpsData gps_data_global = {};
+
+ void gpsGlobalRead(GpsData& out) {
+     copyFromVolatile(out, gps_data_global);
+ }
+
+ void gpsGlobalWrite(const GpsData& in) {
+     copyToVolatile(gps_data_global, in);
+ }

src/data/gps_store.h

@@ -0,0 +1,11 @@
+// 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 "custom_types.h"
+
+extern volatile GpsData gps_data_global;
+
+void gpsGlobalRead(GpsData& out);
+void gpsGlobalWrite(const GpsData& in);

src/data/track_store.cpp

@@ -0,0 +1,39 @@
+// 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 "data/track_store.h"
+#include "data/eeprom_layout.h"
+
+ volatile GlobalTrackData track_data_global = {};
+volatile TrackData track_data_temp_global = {};
+
+ void trackGlobalRead(GlobalTrackData& out) {
+     copyFromVolatile(out, track_data_global);
+ }
+
+ int trackGlobalRead(unsigned short idx, GlobalTrackData& out) {
+     if (idx < 1 || idx > 8) {
+        return 1;
+     }
+     TrackData track_data;
+     EEPROM.get(eeprom_layout::trackSlotAddr(idx), track_data);
+     if (track_data.magic_ != CONFIG_MAGIC) {
+        return 1;
+     }
+     
+     out.loaded_ = true;
+     out.root_ = track_data;
+     return 0;
+ }
+
+ void trackGlobalWrite(const GlobalTrackData& in) {
+     copyToVolatile(track_data_global, in);
+ }
+
+ void trackTempGlobalRead(TrackData& out) {
+     copyFromVolatile(out, track_data_temp_global);
+ }
+
+ void trackTempGlobalWrite(const TrackData& in) {
+     copyToVolatile(track_data_temp_global, in);
+ }

src/data/track_store.h

@@ -0,0 +1,17 @@
+// 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 "custom_types.h"
+#include <EEPROM.h>
+
+extern volatile GlobalTrackData track_data_global;
+extern volatile TrackData track_data_temp_global;
+
+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/flags.h

@@ -0,0 +1,8 @@
+// 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
+#define INFO
+#define WARN
+#define ERROR
+#define DEBUG
+// #define DEEP_DEBUG

src/main.cpp

@@ -1,18 +1,103 @@
-#include <Arduino.h>
+// 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 "flags.h"
+
+#include "modules/cmd/cmd.h"
+#include "modules/config/config.h"
+#include "modules/gps/gps.h"
+#include "modules/lcd/lcd.h"
+#include "modules/logger/system_logger.h"
+#include <Arduino.h>
+#include <avr/wdt.h>
+#include "base/modules.h"
+#include "base/module_base.h"
+#include "base/router.h"
+#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);
+
+Lcd *display = new Lcd(logger);
+Gps *gps_module = new Gps(&Serial2, logger);
+Config *system_config = new Config(logger);
+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);
+
 
-// put function declarations here:
-int myFunction(int, int);
 
 void setup() {
-  // put your setup code here, to run once:
-  int result = myFunction(2, 3);
+  wdt_disable();
+
+  module_registry[module::Lcd] = display;
+  module_registry[module::Gps] = gps_module;
+  module_registry[module::Config] = system_config;
+  module_registry[module::Cmd] = command_handler;
+  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");
+  delay(750);
+
+  display->printMessage("Cmd Init...");
+  command_handler->init();
+  delay(750);
+  display->printMessage("Cmd Init Complete");
+  delay(750);
+
+  display->printMessage("Config Init...");
+  int result = system_config->autoInit();
+  delay(750);
+  if (result != 0) {
+    display->printMessage("Configuration Read Failed");
+  } else {
+    display->printMessage("Config Init Complete");
+  }
+  delay(750);
+
+  display->printMessage("GPS Init...");
+  gps_module->init();
+  lap_counter_modules->init();
+  delay(750);
+  display->printMessage("GPS Init Complete");
+  delay(750);
+
+  display->printMessage("Sensors Init...");
+  battery_module->init();
+  thermocouple_module->init();
+  inj_counter_module->init();
+  delay(750);
+  display->printMessage("Sensors Init Complete");
+  delay(750);
+
+  display->printMessage("Telemetry Init...");
+  telemetry_module->init();
+  delay(750);
+  display->printMessage("Telemetry Init Complete");
+  delay(750);
+  router::send(module::Lcd, task::DisplayDriverPrimary);
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+  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();
 }
-
-// put function definitions here:
-int myFunction(int x, int y) {
-  return x + y;
-}

src/modules/battery/battery.cpp

@@ -0,0 +1,85 @@
+// 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 "battery.h"
+#include "base/router.h"
+#include "data/config_store.h"
+#include "data/general_store.h"
+
+int Battery::calibrate(const Task &task) {
+  float actual_voltage;
+  memcpy(&actual_voltage, &task.data_, sizeof(float));
+  int adc_read = analogRead(VBAT_PIN);
+  float cal_factor = actual_voltage / adc_read;
+  uint32_t output_val;
+  memcpy(&output_val, &cal_factor, sizeof(uint32_t));
+  router::send(module::Config, task::ConfigVbatCalSet, output_val);
+  return 0;
+}
+
+int Battery::push(const Task &task) { return queue_.push(task); }
+
+Battery::Battery() : logger_(nullptr) {}
+
+Battery::Battery(SystemLogger *logger) : logger_(logger) {}
+
+Battery::~Battery() {}
+
+int Battery::init() {
+  pinMode(VBAT_PIN, INPUT);
+  VehicleConfig config;
+  configGlobalRead(config);
+  calibration_ = config.vbat_calibration_;
+  low_threshold_ = config.vbat_low_;
+  return 0;
+}
+
+int Battery::loop(unsigned long timeout_ms) {
+  (void)timeout_ms;
+
+  if (millis() > last_read_at_ + update_interval_) {
+    int adc_read = analogRead(VBAT_PIN);
+    vbat_ = calibration_ * adc_read;
+
+    vbatGlobalWrite(vbat_);
+    if (vbat_ < low_threshold_) {
+      if (warning_sent_at_ == 0 ||
+          millis() > warning_sent_at_ + warning_timeout_) {
+        router::send(module::Lcd, task::DisplayMsgBatteryLow, 2000);
+        warning_sent_at_ = millis();
+      }
+    }
+    last_read_at_ = millis();
+  }
+
+  Task active_task;
+  int res = queue_.pop(active_task);
+  if (res == 0) {
+    if (active_task.target_ == module::Battery) {
+
+      switch (active_task.type_) {
+      case task::BatteryCal:
+        this->calibrate(active_task);
+        break;
+
+      default:
+        break;
+      }
+    } else if (active_task.target_ == module::All) {
+      switch (active_task.type_) {
+      case task::AllConfigUpdated: {
+        VehicleConfig config;
+        configGlobalRead(config);
+        calibration_ = config.vbat_calibration_;
+        low_threshold_ = config.vbat_low_;
+        break;
+      }
+
+      default:
+        break;
+      }
+    }
+  }
+  return 0;
+}

src/modules/battery/battery.h

@@ -0,0 +1,33 @@
+// 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
+#define VBAT_PIN A3
+#include "base/module_base.h"
+#include "base/ring_buffer.h"
+#include "custom_types.h"
+#include "modules/logger/system_logger.h"
+#include <Arduino.h>
+
+class Battery : public ModuleBase {
+
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+  float vbat_ = 0;
+  float calibration_ = 0;
+  float low_threshold_ = 0;
+  unsigned long warning_sent_at_ = 0;
+  unsigned long warning_timeout_ = 10000;
+  unsigned long update_interval_ = 1000;
+  unsigned long last_read_at_ = 0;
+  int calibrate(const Task& task);
+
+public:
+  int push(const Task &task) override;
+  Battery();
+  Battery(SystemLogger *logger);
+  ~Battery();
+  int init();
+  int loop(unsigned long timeout_ms = 500);
+};

src/modules/cmd/cmd.cpp

@@ -0,0 +1,640 @@
+// 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 "cmd.h"
+
+#include <string.h>
+#include "data/track_store.h"
+#include "data/config_store.h"
+#include "data/general_store.h"
+#include "base/router.h"
+
+char *Cmd::trimArg(char *input) {
+  if (input == nullptr) {
+    return nullptr;
+  }
+
+  while (*input == ' ' || *input == '\t' || *input == '\r' || *input == '\n') {
+    input++;
+  }
+
+  if (*input == '\0') {
+    return input;
+  }
+
+  char *end = input + strlen(input) - 1;
+  while (end >= input &&
+         (*end == ' ' || *end == '\t' || *end == '\r' || *end == '\n')) {
+    *end = '\0';
+    end--;
+  }
+
+  return input;
+}
+
+unsigned short Cmd::splitArgs(char *input, char *argv[], unsigned short max_args) {
+  unsigned short argc = 0;
+  char *p = input;
+  char *token_start = input;
+
+  if (input == nullptr || argv == nullptr || max_args == 0) {
+    return 0;
+  }
+
+  if (*input == '\0') {
+    return 0;
+  }
+
+  while (argc < max_args) {
+    if (*p == ',' || *p == '\0') {
+      char separator = *p;
+      *p = '\0';
+
+      argv[argc] = trimArg(token_start);
+      argc++;
+
+      if (separator == '\0') {
+        break;
+      }
+
+      token_start = p + 1;
+    }
+
+    p++;
+  }
+
+  return argc;
+}
+
+Cmd::CommandId Cmd::parseCommandName(const char *input) {
+  if (input == nullptr) {
+    return Unknown;
+  }
+
+  if (strcmp(input, "REBOOT") == 0) {
+    return Reboot;
+  }
+
+  if (strcmp(input, "CFG_DUMP") == 0) {
+    return ConfigDump;
+  }
+
+  if (strcmp(input, "TRACK_PUT") == 0) {
+    return PutTrack;
+  }
+
+  if (strcmp(input, "TRACK_DELETE") == 0) {
+    return DeleteTrack;
+  }
+
+  if (strcmp(input, "TRACK_DUMP") == 0) {
+    return DumpTrack;
+  }
+
+  if (strcmp(input, "CFG_RESET") == 0) {
+    return ConfigReset;
+  }
+
+  if (strcmp(input, "TRACK_AUTODETECT") == 0) {
+    return TrackAutodetect;
+  }
+
+  if (strcmp(input, "DISPLAY_GPS_DEBUG") == 0) {
+    return DisplayGpsDebug;
+  }
+
+  if (strcmp(input, "DISPLAY_GPS_LINE_DEBUG") == 0) {
+    return DisplayGpsLineDebug;
+  }
+
+  if (strcmp(input, "DISPLAY_DRIVER_PRIMARY") == 0) {
+    return DisplayDriverPrimary;
+  }
+
+  if (strcmp(input, "BATTERY_CAL") == 0) {
+    return BatteryCal;
+  }
+
+  if (strcmp(input, "BATTERY_PRINT_VBAT") == 0) {
+    return BatteryPrintVbat;
+  }
+
+  if (strcmp(input, "BATTERY_SET_LOW") == 0) {
+    return BatterySetLow;
+  }
+
+  if (strcmp(input, "THERMO_SET_LOW") == 0) {
+    return ThermoSetLow;
+  }
+
+  if (strcmp(input, "THERMO_SET_HIGH") == 0) {
+    return ThermoSetHigh;
+  }
+
+  return Unknown;
+}
+
+int Cmd::parseTrackSlotId(const char *id_str, unsigned short &id_out) {
+  if (id_str == nullptr || id_str[0] == '\0') {
+    return 1;
+  }
+
+  id_out = strtoul(id_str, nullptr, 10);
+  if (id_out < 1 || id_out > 8) {
+    return 1;
+  }
+
+  return 0;
+}
+
+int Cmd::dumpTrackSlot(unsigned short id) {
+  VehicleConfig config;
+  configGlobalRead(config);
+  bool occupied = config.track_slot_occupied_[id - 1];
+
+  GlobalTrackData track_data;
+  int result = trackGlobalRead(id, track_data);
+  if (result != 0) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("Track slot " + String(id) + " has no valid track data");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  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.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
+
+  return 0;
+}
+
+int Cmd::handleRebootCommand(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("REBOOT expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Rebooting");
+  }
+#endif
+  delay(200);
+  wdt_enable(WDTO_15MS);
+  while (true) {
+  }
+  return 0;
+}
+
+int Cmd::handleDumpConfigCommand(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("DUMPCFG expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->dumpConfig();
+  }
+#endif
+  return 0;
+}
+
+int Cmd::handleTrackPutCommand(unsigned short argc, char *argv[]) {
+  if (argc != 7) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("TRACK_PUT expects 6 arguments");
+    }
+#endif
+    return 1;
+  }
+
+  TrackData new_track;
+  if (parseTrackSlotId(argv[1], new_track.id_) != 0) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error(String("ID out of range: ") + String(argv[1]));
+    }
+#endif
+    return 1;
+  }
+
+  strncpy(new_track.name_, argv[2], sizeof(new_track.name_) - 1);
+  new_track.name_[sizeof(new_track.name_) - 1] = '\0';
+
+  LatLng point_a;
+  point_a.lat_ = strtod(argv[3], nullptr);
+  point_a.lng_ = strtod(argv[4], nullptr);
+  new_track.point_a_ = point_a;
+
+  LatLng point_b;
+  point_b.lat_ = strtod(argv[5], nullptr);
+  point_b.lng_ = strtod(argv[6], nullptr);
+  new_track.point_b_ = point_b;
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Loading new track");
+    logger_->info(String("ID: ") + String(new_track.id_));
+    logger_->info(String("Name: ") + new_track.name_);
+    logger_->info(String("Point A lat: ") + String(new_track.point_a_.lat_));
+    logger_->info(String("Point A lng: ") + String(new_track.point_a_.lng_));
+    logger_->info(String("Point B lat: ") + String(new_track.point_b_.lat_));
+    logger_->info(String("Point B lng: ") + String(new_track.point_b_.lng_));
+  }
+#endif
+
+  trackTempGlobalWrite(new_track);
+  return router::send(module::Config, task::ConfigWriteTempTrack);
+}
+
+int Cmd::handleTrackDeleteCommand(unsigned short argc, char *argv[]) {
+  if (argc != 2) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("TRACK_DELETE expects 1 argument");
+    }
+#endif
+    return 1;
+  }
+
+  unsigned short id;
+  if (parseTrackSlotId(argv[1], id) != 0) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error(String("ID out of range: ") + String(argv[1]));
+    }
+#endif
+    return 1;
+  }
+
+  return router::send(module::Config, task::ConfigTrackDelete, id);
+}
+
+int Cmd::handleTrackDumpCommand(unsigned short argc, char *argv[]) {
+  if (argc != 2) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("TRACK_DUMP expects 1 argument");
+    }
+#endif
+    return 1;
+  }
+
+  unsigned short id;
+  if (parseTrackSlotId(argv[1], id) != 0) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error(String("ID out of range: ") + String(argv[1]));
+    }
+#endif
+    return 1;
+  }
+
+  return this->dumpTrackSlot(id);
+}
+
+int Cmd::handleConfigResetCommand(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("CFG_RESET expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Resetting config");
+  }
+#endif
+  return router::send(module::Config, task::ConfigReset);
+}
+
+int Cmd::handleTrackAutodetectCommand(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("TRACK_AUTODETECT expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Detecting track");
+  }
+#endif
+  return router::send(module::Config, task::ConfigTrackDetect, 1);
+}
+
+int Cmd::handleDisplayGpsDebug(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("DISPLAY_GPS_DEBUG expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Switching to GPS_DEBUG display");
+  }
+#endif
+  return router::send(module::Lcd, task::DisplayGpsDebug);
+}
+
+
+int Cmd::handleDisplayDriverPrimary(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("DISPLAY_DRIVER_PRIMARY expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Switching to DRIVER_PRIMARY display");
+  }
+#endif
+  return router::send(module::Lcd, task::DisplayDriverPrimary);
+}
+
+int Cmd::handleBatteryCal(unsigned short argc, char *argv[]) {
+  if (argc != 2) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("BATTERY_CAL expects 1 argument");
+    }
+#endif
+    return 1;
+  }
+  float vbat = strtod(argv[1], nullptr);
+  uint32_t task_data;
+  memcpy(&task_data, &vbat, sizeof(uint32_t));
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Calibrating VBAT");
+  }
+#endif
+  router::send(module::Battery, task::BatteryCal, task_data);
+  return 0;
+}
+
+int Cmd::handleBatteryPrintVbat(unsigned short argc) {
+  if (argc != 1) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("BATTERY_PRINT_VBAT expects no arguments");
+    }
+#endif
+    return 1;
+  }
+
+#ifdef INFO
+  float vbat;
+  vbatGlobalRead(vbat);
+  if (logger_ != nullptr) {
+    logger_->info("VBAT: " + String(vbat));
+  }
+#endif
+  return 0;
+}
+
+int Cmd::handleBatterySetLow(unsigned short argc, char* argv[]) {
+  if (argc != 2) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("BATTERY_SET_LOW expects 1 argument");
+    }
+#endif
+    return 1;
+  }
+  float low = strtod(argv[1], nullptr);
+  uint32_t task_data;
+  memcpy(&task_data, &low, sizeof(uint32_t));
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Setting warning level for VBAT");
+  }
+#endif
+  router::send(module::Config, task::ConfigVbatSetLow, task_data);
+  return 0;
+}
+
+int Cmd::handleThermoSetLow(unsigned short argc, char* argv[]) {
+  if (argc != 2) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("THERMO_SET_LOW expects 1 argument");
+    }
+#endif
+    return 1;
+  }
+  float low = strtod(argv[1], nullptr);
+  uint32_t task_data;
+  memcpy(&task_data, &low, sizeof(uint32_t));
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Setting low level for TENG");
+  }
+#endif
+  router::send(module::Config, task::ConfigTengSetLow, task_data);
+  return 0;
+}
+
+int Cmd::handleThermoSetHigh(unsigned short argc, char* argv[]) {
+  if (argc != 2) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("THERMO_SET_HIGH expects 1 argument");
+    }
+#endif
+    return 1;
+  }
+  float low = strtod(argv[1], nullptr);
+  uint32_t task_data;
+  memcpy(&task_data, &low, sizeof(uint32_t));
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Setting high level for TENG");
+  }
+#endif
+  router::send(module::Config, task::ConfigTengSetHigh, task_data);
+  return 0;
+}
+
+int Cmd::handleUnknownCommand(unsigned short argc, char *argv[]) {
+#ifdef ERROR
+  if (logger_ != nullptr) {
+    if (argc > 0 && argv != nullptr && argv[0] != nullptr && argv[0][0] != '\0') {
+      logger_->error(String("Unknown command: ") + String(argv[0]));
+    } else {
+      logger_->error("Unknown command");
+    }
+  }
+#endif
+  return 1;
+}
+
+int Cmd::dispatchCommand(CommandId command, unsigned short argc, char *argv[]) {
+  switch (command) {
+    case Reboot:
+      return this->handleRebootCommand(argc);
+
+    case ConfigDump:
+      return this->handleDumpConfigCommand(argc);
+
+    case PutTrack:
+      return this->handleTrackPutCommand(argc, argv);
+
+    case DeleteTrack:
+      return this->handleTrackDeleteCommand(argc, argv);
+
+    case DumpTrack:
+      return this->handleTrackDumpCommand(argc, argv);
+
+    case ConfigReset:
+      return this->handleConfigResetCommand(argc);
+      
+    case TrackAutodetect:
+      return this->handleTrackAutodetectCommand(argc);
+      
+    case DisplayGpsDebug:
+      return this->handleDisplayGpsDebug(argc);
+
+    case DisplayDriverPrimary:
+      return this->handleDisplayDriverPrimary(argc);
+      
+    case BatteryCal:
+      return this->handleBatteryCal(argc, argv);
+      
+    case BatteryPrintVbat:
+      return this->handleBatteryPrintVbat(argc);
+      
+    case BatterySetLow:
+      return this->handleBatterySetLow(argc, argv);
+
+    case ThermoSetLow:
+      return this->handleThermoSetLow(argc, argv);
+
+    case ThermoSetHigh:
+      return this->handleThermoSetHigh(argc, argv);
+
+    case Unknown:
+    default:
+      return this->handleUnknownCommand(argc, argv);
+  }
+}
+
+int Cmd::tryParse() {
+#ifdef DEBUG
+  if (logger_ != nullptr) {
+    logger_->debug("Attempting to parse command");
+  }
+#endif
+
+  char *argvp[MAX_ARGS];
+  unsigned short argc = splitArgs(buffer_, argvp, MAX_ARGS);
+
+  if (argc == 0 || argvp[0] == nullptr || argvp[0][0] == '\0') {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("Empty command");
+    }
+#endif
+    return 1;
+  }
+
+  CommandId command = parseCommandName(argvp[0]);
+  return dispatchCommand(command, argc, argvp);
+}
+
+int Cmd::push(const Task &task) {
+  return queue_.push(task);
+}
+
+Cmd::Cmd(HardwareSerial *data_stream)
+    : data_stream_(data_stream), logger_(nullptr) {}
+
+Cmd::Cmd(HardwareSerial *data_stream, SystemLogger *logger)
+    : data_stream_(data_stream), logger_(logger) {}
+
+Cmd::~Cmd() {}
+
+int Cmd::init() {
+  data_stream_->begin(baud_rate_);
+  return 0;
+}
+
+int Cmd::parseTask(unsigned long timeout_ms) {
+  unsigned long start = millis();
+
+  while (data_stream_->available()) {
+    if ((unsigned long)(millis() - start) >= timeout_ms) {
+      return 1;
+    }
+
+    char current_char = data_stream_->read();
+
+    if (current_char == '<') {
+      buffer_open_ = true;
+      index_ = 0;
+      continue;
+    }
+
+    if (!buffer_open_) {
+      continue;
+    }
+
+    if (current_char == '>') {
+      buffer_[index_] = '\0';
+      buffer_open_ = false;
+      return this->tryParse();
+    }
+
+    if (index_ >= sizeof(buffer_) - 1) {
+      buffer_open_ = false;
+      index_ = 0;
+#ifdef ERROR
+      if (logger_ != nullptr) {
+        logger_->error("Command parser buffer overflow");
+      }
+#endif
+      return 1;
+    }
+
+    buffer_[index_] = current_char;
+    index_++;
+  }
+
+  return 0;
+}

src/modules/cmd/cmd.h

@@ -0,0 +1,76 @@
+// 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 <Arduino.h>
+#include <avr/wdt.h>
+
+#include "base/module_base.h"
+#include "base/ring_buffer.h"
+#include "base/task.h"
+#include "custom_types.h"
+#include "modules/logger/system_logger.h"
+
+class Cmd : public ModuleBase {
+private:
+  enum CommandId {
+    Unknown = 0,
+    Reboot,
+    PutTrack,
+    DeleteTrack,
+    DumpTrack,
+    ConfigReset,
+    ConfigDump,
+    TrackAutodetect,
+    DisplayGpsDebug,
+    DisplayGpsLineDebug,
+    DisplayDriverPrimary,
+    BatteryCal,
+    BatteryPrintVbat,
+    BatterySetLow,
+    ThermoSetLow,
+    ThermoSetHigh,
+  };
+
+  HardwareSerial *data_stream_;
+  unsigned long baud_rate_ = 115200;
+  SystemLogger *logger_;
+  char buffer_[256];
+  unsigned int index_ = 0;
+  bool buffer_open_ = false;
+  RingBuffer<Task, 16> queue_;
+
+  static const unsigned short MAX_ARGS = 10;
+
+  int tryParse();
+  unsigned short splitArgs(char *input, char *argv[], unsigned short max_args);
+  char *trimArg(char *input);
+  CommandId parseCommandName(const char *input);
+  int dispatchCommand(CommandId command, unsigned short argc, char *argv[]);
+  int parseTrackSlotId(const char *id_str, unsigned short &id_out);
+  int dumpTrackSlot(unsigned short id);
+  int handleRebootCommand(unsigned short argc);
+  int handleDumpConfigCommand(unsigned short argc);
+  int handleTrackPutCommand(unsigned short argc, char *argv[]);
+  int handleTrackDeleteCommand(unsigned short argc, char *argv[]);
+  int handleTrackDumpCommand(unsigned short argc, char *argv[]);
+  int handleConfigResetCommand(unsigned short argc);
+  int handleTrackAutodetectCommand(unsigned short argc);
+  int handleDisplayGpsDebug(unsigned short argc);
+  int handleDisplayDriverPrimary(unsigned short argc);
+  int handleBatteryCal(unsigned short argc, char *argv[]);
+  int handleBatteryPrintVbat(unsigned short argc);
+  int handleBatterySetLow(unsigned short argc, char *argv[]);
+  int handleThermoSetLow(unsigned short argc, char *argv[]);
+  int handleThermoSetHigh(unsigned short argc, char *argv[]);
+  int handleUnknownCommand(unsigned short argc, char *argv[]);
+
+public:
+  int push(const Task &task) override;
+  Cmd(HardwareSerial *data_stream);
+  Cmd(HardwareSerial *data_stream, SystemLogger *logger);
+  ~Cmd();
+  int init();
+  int parseTask(unsigned long timeout_ms = 500);
+};

src/modules/config/config.cpp

@@ -0,0 +1,396 @@
+// 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 "config.h"
+#include "data/track_store.h"
+
+#include <math.h>
+#include <string.h>
+
+int Config::push(const Task &task) { return queue_.push(task); }
+
+int Config::taskComplete() {
+  task_memory_stale_ = true;
+  active_task_ = Task(module::Null, task::Null, 0);
+  return 0;
+}
+
+int Config::writeTrack(const TrackData &track_data) {
+  TrackData track_copy = track_data;
+  track_copy.magic_ = CONFIG_MAGIC;
+  if (track_copy.id_ < 1 || track_copy.id_ > 8) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("Cannot write track data with out of range id, aborting");
+    }
+#endif
+    return 1;
+  }
+  EEPROM.put(eeprom_layout::trackSlotAddr(track_copy.id_), track_copy);
+  config_.track_slot_occupied_[track_copy.id_ - 1] = true;
+  this->writeConfig();
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Succesfully wrote new track into slot " + String(track_copy.id_));
+  }
+#endif
+  return 0;
+}
+
+int Config::writeTrackFromTemp() {
+  TrackData track_data;
+  trackTempGlobalRead(track_data);
+  return this->writeTrack(track_data);
+}
+
+int Config::deleteTrack(unsigned short idx) {
+  if (idx < 1 || idx > 8) {
+#ifdef ERROR
+    if (logger_ != nullptr) {
+      logger_->error("Cannot delete track with out of range id, aborting");
+    }
+#endif
+    return 1;
+  }
+
+  if (config_.track_slot_occupied_[idx - 1] == false) {
+#ifdef WARN
+    if (logger_ != nullptr) {
+      logger_->warn("Requested delete on empty track slot " + String(idx));
+    }
+#endif
+    return 0;
+  }
+
+  config_.track_slot_occupied_[idx - 1] = false;
+
+  if (is_track_loaded_ && loaded_track_.id_ == idx) {
+    is_track_loaded_ = false;
+    loaded_track_ = {};
+    GlobalTrackData track;
+    track.loaded_ = false;
+    track.root_ = loaded_track_;
+    trackGlobalWrite(track);
+  }
+
+  int write_result = this->writeConfig();
+
+#ifdef INFO
+  if (logger_ != nullptr && write_result == 0) {
+    logger_->info("Succesfully deleted track slot " + String(idx));
+  }
+#endif
+  return write_result;
+}
+
+int Config::resetConfig() {
+  VehicleConfig clean_config;
+  config_ = clean_config;
+  is_track_loaded_ = false;
+  loaded_track_ = {};
+  task_memory_stale_ = true;
+  no_tracks_notice_shown_ = false;
+    GlobalTrackData track;
+    track.loaded_ = false;
+    track.root_ = loaded_track_;
+    trackGlobalWrite(track);
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Resetting configuration to factory defaults");
+  }
+#endif
+
+  return this->writeConfig();
+}
+
+int Config::writeVbatCal(float value) {
+  config_.vbat_calibration_ = value;
+  return this->writeConfig();
+}
+
+int Config::writeVbatLow(float value) {
+  config_.vbat_low_ = value;
+  return this->writeConfig();
+}
+
+int Config::writeTengLow(float value) {
+  config_.teng_low_ = value;
+  return this->writeConfig();
+}
+
+int Config::writeTengHigh(float value) {
+  config_.teng_high_ = value;
+  return this->writeConfig();
+}
+
+Config::Config() : logger_(nullptr), valid_config_(true) {}
+
+Config::Config(SystemLogger *logger) : logger_(logger), valid_config_(true) {}
+
+Config::~Config() {}
+
+int Config::readConfig() {
+  EEPROM.get(eeprom_layout::CONFIG_ADDR, config_);
+  configGlobalWrite(config_);
+  return 0;
+}
+
+int Config::writeConfig() {
+  EEPROM.put(eeprom_layout::CONFIG_ADDR, config_);
+  configGlobalWrite(config_);
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info("Config updated and saved to EEPROM");
+  }
+#endif
+  router::send(module::All, task::AllConfigUpdated);
+  return 0;
+}
+
+int Config::writeConfig(const VehicleConfig &new_config) {
+  config_ = new_config;
+  EEPROM.put(eeprom_layout::CONFIG_ADDR, new_config);
+  configGlobalWrite(new_config);
+  return 0;
+}
+
+int Config::taskConfigDetectTrack(unsigned long timeout_ms) {
+  unsigned long start = millis();
+  bool has_at_least_one_track = false;
+
+  for (size_t i = 0; i < 8; i++) {
+    if (config_.track_slot_occupied_[i] == true) {
+      has_at_least_one_track = true;
+      break;
+    }
+  }
+
+  if (!has_at_least_one_track) {
+    if (!no_tracks_notice_shown_) {
+      router::send(module::Lcd, task::DisplayMsgConfigNoTracks, 3000);
+      no_tracks_notice_shown_ = true;
+    }
+    this->taskComplete();
+    return 1;
+  }
+
+  TaskConfigTrackDetectData task_data;
+
+  if (!task_memory_stale_) {
+    memcpy(&task_data, task_memory_, sizeof(task_data));
+  } else {
+    GpsData current_gps;
+    gpsGlobalRead(current_gps);
+    task_data.gps_lat_ = current_gps.lat_.value_;
+    task_data.gps_lng_ = current_gps.lng_.value_;
+    task_data.cos_ = cos(task_data.gps_lat_ * M_PI / 180);
+  }
+
+  while (true) {
+    task_data.last_checked_++;
+
+    TrackData track_data;
+    int result = this->getTrack(task_data.last_checked_, track_data);
+    if (result == 0) {
+      float delta_lat = track_data.point_a_.lat_ - task_data.gps_lat_;
+      float delta_lng = (track_data.point_a_.lng_ - task_data.gps_lng_) * task_data.cos_;
+      float dist2 = delta_lat * delta_lat + delta_lng * delta_lng;
+
+      if (dist2 < task_data.sqdiff_ || task_data.smallest_idx_ == 0) {
+        task_data.smallest_idx_ = task_data.last_checked_;
+        task_data.sqdiff_ = dist2;
+      }
+    }
+
+    if (task_data.last_checked_ >= 8) {
+      int load_result = 1;
+      if (task_data.smallest_idx_ > 0) {
+        load_result = this->loadTrack(task_data.smallest_idx_);
+      }
+
+      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;
+      }
+
+      if (!no_tracks_notice_shown_) {
+        router::send(module::Lcd, task::DisplayMsgConfigNoTracks, 3000);
+        no_tracks_notice_shown_ = true;
+      }
+      return 1;
+    }
+
+    if ((unsigned long)(millis() - start) >= timeout_ms) {
+      task_memory_stale_ = false;
+      memcpy(task_memory_, &task_data, sizeof(task_data));
+      return 1;
+    }
+  }
+}
+
+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();
+    return 0;
+  }
+
+  case task::ConfigWriteTempTrack: {
+    int result = this->writeTrackFromTemp();
+    this->taskComplete();
+    return result;
+  }
+
+  case task::ConfigTrackDelete: {
+    int result = this->deleteTrack(active_task_.data_);
+    this->taskComplete();
+    return result;
+  }
+
+  case task::ConfigReset: {
+    int result = this->resetConfig();
+    this->taskComplete();
+    return result;
+  }
+
+  case task::ConfigVbatCalSet: {
+    float cal_value;
+    memcpy(&cal_value, &active_task_.data_, sizeof(float));
+    int result = this->writeVbatCal(cal_value);
+    this->taskComplete();
+    return result;
+  }
+
+  case task::ConfigVbatSetLow: {
+    float low_value;
+    memcpy(&low_value, &active_task_.data_, sizeof(float));
+    int result = this->writeVbatLow(low_value);
+    this->taskComplete();
+    return result;
+  }
+
+  case task::ConfigTengSetLow: {
+    float low_value;
+    memcpy(&low_value, &active_task_.data_, sizeof(float));
+    int result = this->writeTengLow(low_value);
+    this->taskComplete();
+    return result;
+  }
+
+  case task::ConfigTengSetHigh: {
+    float high_value;
+    memcpy(&high_value, &active_task_.data_, sizeof(float));
+    int result = this->writeTengHigh(high_value);
+    this->taskComplete();
+    return result;
+  }
+
+  default:
+    break;
+  }
+  return 0;
+}
+
+int Config::autoInit() {
+  this->readConfig();
+  if (config_.magic_ != CONFIG_MAGIC) {
+#ifdef WARN
+    if (logger_ != nullptr) {
+      logger_->warn("Config invalid, overwriting");
+    }
+#endif
+    VehicleConfig clean_config;
+    this->writeConfig(clean_config);
+    this->readConfig();
+    if (config_.magic_ != CONFIG_MAGIC) {
+#ifdef ERROR
+      if (logger_ != nullptr) {
+        logger_->error("Config write failed, EEPROM may be burnt");
+      }
+#endif
+      return 1;
+    }
+  }
+  valid_config_ = true;
+  return 0;
+}
+
+int Config::loop(unsigned long timeout_ms) {
+  if (active_task_.type_ != task::Null && active_task_.target_ != module::Null) {
+    this->handleActiveTask(timeout_ms);
+    return 0;
+  }
+
+  int result = queue_.pop(active_task_);
+  if (result == 0) {
+    this->handleActiveTask(timeout_ms);
+  }
+
+  return 0;
+}
+
+int Config::getTrack(unsigned int idx, TrackData &track_data) {
+  if (idx < 1 || idx > 8) {
+    return 1;
+  }
+
+  if (config_.track_slot_occupied_[idx - 1] == false) {
+    return 1;
+  }
+
+  EEPROM.get(eeprom_layout::trackSlotAddr(idx), track_data);
+  if (track_data.magic_ != CONFIG_MAGIC) {
+    return 1;
+  }
+
+  return 0;
+}
+
+int Config::loadTrack(unsigned int idx) {
+  if (idx < 1 || idx > 8) {
+    return 1;
+  }
+
+  if (config_.track_slot_occupied_[idx - 1] == false) {
+    return 1;
+  }
+
+  TrackData track_data;
+  EEPROM.get(eeprom_layout::trackSlotAddr(idx), track_data);
+  if (track_data.magic_ != CONFIG_MAGIC) {
+    return 1;
+  }
+
+  loaded_track_ = track_data;
+  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/config/config.h

@@ -0,0 +1,64 @@
+// 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 "base/task.h"
+#include "custom_types.h"
+#include "flags.h"
+#include "modules/logger/system_logger.h"
+#include <EEPROM.h>
+#include "data/gps_store.h"
+#include "data/track_store.h"
+#include "data/eeprom_layout.h"
+#include "base/router.h"
+
+struct TaskConfigTrackDetectData {
+  unsigned short last_checked_ = 0;
+  unsigned short smallest_idx_ = 0;
+  float cos_ = 0;
+  float sqdiff_ = 0;
+  float gps_lat_ = 0;
+  float gps_lng_ = 0;
+};
+
+class Config : public ModuleBase {
+private:
+  VehicleConfig config_;
+  SystemLogger *logger_;
+  bool valid_config_;
+  TrackData loaded_track_;
+  bool is_track_loaded_ = false;
+  RingBuffer<Task, 16> queue_;
+  Task active_task_ = {};
+  uint8_t task_memory_[64] = {};
+  bool task_memory_stale_ = true;
+  bool no_tracks_notice_shown_ = false;
+
+  int readConfig();
+  int writeConfig();
+  int writeConfig(const VehicleConfig &new_config);
+  int handleActiveTask(unsigned long timeout_ms);
+  int taskConfigDetectTrack(unsigned long timeout_ms);
+  int taskComplete();
+  int writeTrack(const TrackData& track_data);
+  int writeTrackFromTemp();
+  int deleteTrack(unsigned short idx);
+  int resetConfig();
+  int writeVbatCal(float value);
+  int writeVbatLow(float value);
+  int writeTengLow(float value);
+  int writeTengHigh(float value);
+
+public:
+  int push(const Task &task) override;
+  Config();
+  Config(SystemLogger *logger);
+  ~Config();
+  int autoInit();
+  int loop(unsigned long timeout_ms = 500);
+  int getTrack(unsigned int idx, TrackData &track_data);
+  int loadTrack(unsigned int idx);
+};

src/modules/gps/gps.cpp

@@ -0,0 +1,186 @@
+// 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 "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); }
+
+Gps::Gps(HardwareSerial *data_stream)
+    : gps_(nullptr), data_stream_(data_stream), logger_(nullptr) {
+  gps_ = new TinyGPSPlus();
+}
+
+Gps::Gps(HardwareSerial *data_stream, SystemLogger *logger)
+    : gps_(nullptr), data_stream_(data_stream), logger_(logger) {
+  gps_ = new TinyGPSPlus();
+}
+
+Gps::~Gps() {
+  data_stream_ = nullptr;
+  delete gps_;
+  gps_ = nullptr;
+}
+
+int Gps::init() {
+  data_stream_->begin(9600);
+  return 0;
+}
+
+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())) {
+      gpsGlobalWrite(this->getData());
+      uint32_t current_fix_value = gps_->sentencesWithFix();
+      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) {
+      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;
+}
+
+GpsData Gps::getData() {
+  GpsData output;
+
+  output.altitude_.age_ = gps_->altitude.age();
+  output.altitude_.valid_ = gps_->altitude.isValid();
+  output.altitude_.value_ = gps_->altitude.meters();
+
+  output.lat_.age_ = gps_->location.age();
+  output.lat_.valid_ = gps_->location.isValid();
+  output.lat_.value_ = gps_->location.lat();
+
+  output.lng_.age_ = gps_->location.age();
+  output.lng_.valid_ = gps_->location.isValid();
+  output.lng_.value_ = gps_->location.lng();
+
+  output.speed_.age_ = gps_->speed.age();
+  output.speed_.valid_ = gps_->speed.isValid();
+  output.speed_.value_ = gps_->speed.kmph();
+
+  output.course_.age_ = gps_->course.age();
+  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;
+}
+
+#undef MOD

src/modules/gps/gps.h

@@ -0,0 +1,57 @@
+// 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 "custom_types.h"
+#include "TinyGPSPlus.h"
+#include "flags.h"
+#include "modules/logger/system_logger.h"
+#include "base/task.h"
+#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_;
+  HardwareSerial *data_stream_;
+  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;
+  Gps(HardwareSerial *data_stream);
+  Gps(HardwareSerial *data_stream, SystemLogger *logger);
+  ~Gps();
+  int loop(unsigned long timeout_ms = 500);
+  int init();
+  GpsData getData();
+};

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

@@ -0,0 +1,680 @@
+// 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 "lcd.h"
+
+#include "data/general_store.h"
+#include "modules/gps/gps.h"
+#include <Wire.h>
+#include <string.h>
+
+#define MOD "modules/lcd/lcd.h"
+
+void Lcd::clear() {
+  if (!display_cleared_) {
+    display_->clear();
+    display_cleared_ = true;
+  }
+}
+
+void Lcd::print(const String &msg) {
+  display_->print(msg);
+  display_cleared_ = false;
+}
+
+void Lcd::print(char c) {
+  display_->print(c);
+  display_cleared_ = false;
+}
+
+void Lcd::print(const char c[]) {
+  display_->print(c);
+  display_cleared_ = false;
+}
+
+void Lcd::print(float d, int digits) {
+  display_->print(d, digits);
+  display_cleared_ = false;
+}
+
+void Lcd::print(unsigned long l, int base) {
+  display_->print(l, base);
+  display_cleared_ = false;
+}
+
+void Lcd::print(long l, int base) {
+  display_->print(l, base);
+  display_cleared_ = false;
+}
+
+void Lcd::print(unsigned int i, int base) {
+  display_->print(i, base);
+  display_cleared_ = false;
+}
+
+void Lcd::print(int i, int base) {
+  display_->print(i, base);
+  display_cleared_ = false;
+}
+
+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:
+  case task::DisplayMsgLapCounterLapTime:
+  case task::DisplayMsgLapCounterStart:
+    return true;
+
+  default:
+    return false;
+  }
+}
+
+void Lcd::activateMessage(screen::LcdScreen msg_screen,
+                          unsigned long duration_ms) {
+  if (duration_ms == 0) {
+    duration_ms = frame_duration_;
+  }
+
+  message_screen_ = msg_screen;
+  message_active_ = true;
+  message_end_ = millis() + duration_ms;
+  screen_ = message_screen_;
+  force_render_ = true;
+}
+
+void Lcd::expireMessageIfNeeded(unsigned long now) {
+  if (!message_active_) {
+    return;
+  }
+
+  if ((long)(now - message_end_) >= 0) {
+    message_active_ = false;
+    message_screen_ = screen::Blank;
+    screen_ = data_screen_;
+    force_render_ = true;
+    base_rendered_ = false;
+  }
+}
+
+screen::LcdScreen Lcd::getActiveScreen() const {
+  if (message_active_) {
+    return message_screen_;
+  }
+
+  return data_screen_;
+}
+
+int Lcd::renderGpsDebug() {
+  this->clear();
+
+  GpsData gps_data;
+  gpsGlobalRead(gps_data);
+
+  display_->setCursor(0, 0);
+  this->print("Alt: ");
+  if (gps_data.altitude_.valid_) {
+    this->print(gps_data.altitude_.value_, 5);
+  } else {
+    this->print("not valid");
+  }
+
+  display_->setCursor(0, 1);
+  this->print("Lat: ");
+  if (gps_data.lat_.valid_) {
+    this->print(gps_data.lat_.value_, 5);
+  } else {
+    this->print("not valid");
+  }
+
+  display_->setCursor(0, 2);
+  this->print("Lng: ");
+  if (gps_data.lng_.valid_) {
+    this->print(gps_data.lng_.value_, 5);
+  } else {
+    this->print("not valid");
+  }
+
+  display_->setCursor(0, 3);
+  this->print("Spd: ");
+  if (gps_data.speed_.valid_) {
+    this->print(gps_data.speed_.value_, 5);
+  } else {
+    this->print("not valid");
+  }
+
+  return 0;
+}
+
+int Lcd::renderDriverPrimary() {
+
+  GpsData gps_data;
+  gpsGlobalRead(gps_data);
+
+  float vbat;
+  vbatGlobalRead(vbat);
+  float teng;
+  tengGlobalRead(teng);
+
+  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:");
+
+    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(12, 0);
+  if (num_laps < 10)
+    this->print('0');
+  this->print(num_laps, 10);
+
+  display_->setCursor(4, 1);
+  if (inj_ctr < 100) {
+    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");
+
+    GlobalTrackData track_data;
+    trackGlobalRead(track_data);
+
+    display_->setCursor((20 - strlen(track_data.root_.name_)) / 2, 2);
+    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::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_ = 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_(500) {
+  display_ = new LiquidCrystal_I2C(0x27, 20, 4);
+}
+
+Lcd::~Lcd() {
+  delete display_;
+  display_ = nullptr;
+}
+
+int Lcd::init() {
+#ifdef DEEP_DEBUG
+  if (logger_ != nullptr) {
+    logger_->deepDebug(String(MOD) + ": init: Begin");
+  }
+#endif
+
+  display_->init();
+  Wire.setClock(400000);
+  display_->backlight();
+  this->clear();
+  display_->setCursor(0, 0);
+  force_render_ = true;
+
+#ifdef DEEP_DEBUG
+  if (logger_ != nullptr) {
+    logger_->deepDebug(String(MOD) + ": init: End");
+  }
+#endif
+
+  return 0;
+}
+
+int Lcd::printMessage(String message) {
+#ifdef DEEP_DEBUG
+  if (logger_ != nullptr) {
+    logger_->deepDebug(String(MOD) + ": printMessage: Begin");
+  }
+#endif
+
+  String original = message;
+  this->clear();
+
+  if (message.length() > 80) {
+    message = message.substring(0, 80);
+  }
+
+  String lines[4] = {"", "", "", ""};
+  int lineIndex = 0;
+
+  while (message.length() > 0 && lineIndex < 4) {
+    if (message.length() <= 20) {
+      lines[lineIndex++] = message;
+      break;
+    }
+
+    int splitIndex = message.lastIndexOf(' ', 20);
+
+    if (splitIndex == -1 || splitIndex == 0) {
+      splitIndex = 20;
+    }
+
+    lines[lineIndex++] = message.substring(0, splitIndex);
+
+    if (splitIndex < message.length() && message.charAt(splitIndex) == ' ') {
+      message = message.substring(splitIndex + 1);
+    } else {
+      message = message.substring(splitIndex);
+    }
+  }
+
+  int usedLines = 0;
+  for (int i = 0; i < 4; i++) {
+    if (lines[i].length() > 0) {
+      usedLines++;
+    }
+  }
+
+  int startRow = 0;
+  if (usedLines == 1) {
+    startRow = 1;
+  } else if (usedLines == 2) {
+    startRow = 1;
+  } else {
+    startRow = 0;
+  }
+
+  int currentRow = startRow;
+  for (int i = 0; i < 4; i++) {
+    if (lines[i].length() == 0) {
+      continue;
+    }
+
+    int col = (20 - lines[i].length()) / 2;
+    if (col < 0) {
+      col = 0;
+    }
+
+    display_->setCursor(col, currentRow++);
+    this->print(lines[i]);
+  }
+
+#ifdef INFO
+  if (logger_ != nullptr) {
+    logger_->info(original);
+  }
+#endif
+
+#ifdef DEEP_DEBUG
+  if (logger_ != nullptr) {
+    logger_->deepDebug(String(MOD) + ": printMessage: End");
+  }
+#endif
+
+  return 0;
+}
+
+int Lcd::loop(unsigned long timeout_ms) {
+  unsigned long now = millis();
+  unsigned long start = now;
+
+  expireMessageIfNeeded(now);
+
+  while (true) {
+    Task next_task;
+    bool have_task = false;
+
+    if (deferred_task_valid_) {
+      next_task = deferred_task_;
+      deferred_task_valid_ = false;
+      have_task = true;
+    } else {
+      if (queue_.pop(next_task) == 0) {
+        have_task = true;
+      }
+    }
+
+    if (!have_task) {
+      break;
+    }
+
+    if (message_active_ && isMessageTask(next_task.type_)) {
+      deferred_task_ = next_task;
+      deferred_task_valid_ = true;
+      break;
+    }
+
+    switch (next_task.type_) {
+    case task::DisplayGpsDebug:
+      base_rendered_ = false;
+      data_screen_ = screen::GpsDebug;
+      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:
+      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:
+      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();
+    expireMessageIfNeeded(now);
+
+    if ((unsigned long)(now - start) >= timeout_ms) {
+      break;
+    }
+  }
+
+  now = millis();
+  expireMessageIfNeeded(now);
+
+  screen::LcdScreen active_screen = getActiveScreen();
+  if (screen_ != active_screen) {
+    screen_ = active_screen;
+    force_render_ = true;
+  }
+
+  if (!force_render_ && (unsigned long)(now - last_render_) < frame_duration_) {
+    return 1;
+  }
+
+  switch (screen_) {
+  case screen::Blank:
+    this->clear();
+    break;
+
+  case screen::GpsDebug:
+    this->renderGpsDebug();
+    break;
+
+  case screen::DriverPrimary:
+    this->renderDriverPrimary();
+    break;
+
+  case screen::MsgGpsFix:
+    this->renderMsgGpsFix();
+    break;
+
+  case screen::MsgGpsTrigger:
+    this->renderMsgGpsTrigger();
+    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::MsgLapCounterStart:
+    this->renderMsgLapCounterStart();
+    break;
+
+  case screen::MsgLapCounterLapTime:
+    this->renderMsgLapCounterLapTime();
+    break;
+
+  default:
+    break;
+  }
+
+  last_render_ = now;
+  force_render_ = false;
+  return 1;
+}
+
+#undef MOD

src/modules/lcd/lcd.h

@@ -0,0 +1,91 @@
+// 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 <Arduino.h>
+#include <LiquidCrystal_I2C.h>
+
+#include "flags.h"
+#include "modules/logger/system_logger.h"
+#include "modules/gps/gps.h"
+#include "base/task.h"
+#include "base/ring_buffer.h"
+#include "base/module_base.h"
+#include "data/gps_store.h"
+#include "data/track_store.h"
+#include "data/general_store.h"
+
+namespace screen {
+
+enum LcdScreen : uint8_t {
+  Blank,
+  GpsDebug,
+  DriverPrimary,
+  MsgGpsFix,
+  MsgGpsTrigger,
+  MsgTrackDetectOk,
+  MsgConfigNoTracks,
+  MsgBatteryLow,
+  MsgEngineTempLow,
+  MsgEngineTempHigh,
+  MsgLapCounterStart,
+  MsgLapCounterLapTime,
+};
+
+} // namespace screen
+
+class Lcd : public ModuleBase {
+private:
+  LiquidCrystal_I2C *display_;
+  bool display_cleared_;
+  SystemLogger *logger_ = nullptr;
+  screen::LcdScreen screen_;
+  screen::LcdScreen data_screen_;
+  screen::LcdScreen message_screen_;
+  unsigned long last_render_;
+  unsigned long frame_duration_;
+  unsigned long message_end_ = 0;
+  bool message_active_ = false;
+  bool force_render_ = false;
+  RingBuffer<Task, 16> queue_;
+  Task deferred_task_{};
+  bool deferred_task_valid_ = false;
+  bool base_rendered_ = false;
+
+  void clear();
+  void print(const String &msg);
+  void print(char c);
+  void print(const char c[]);
+  void print(float d, int digits = 2);
+  void print(unsigned long l, int base = 10);
+  void print(long l, int base = 10);
+  void print(unsigned int i, int base = 10);
+  void print(int i, int base = 10);
+
+  bool isMessageTask(task::Type type);
+  void activateMessage(screen::LcdScreen msg_screen, unsigned long duration_ms);
+  void expireMessageIfNeeded(unsigned long now);
+  screen::LcdScreen getActiveScreen() const;
+
+  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;
+  Lcd();
+  Lcd(SystemLogger *logger);
+  ~Lcd();
+  int init();
+  int printMessage(String message);
+  int loop(unsigned long timeout_ms = 500);
+};

src/modules/logger/system_logger.cpp

@@ -0,0 +1,93 @@
+// 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 "system_logger.h"
+#include "data/general_store.h"
+
+#include <stdio.h>
+
+SystemLogger::SystemLogger(HardwareSerial *output) { output_ = output; }
+
+SystemLogger::~SystemLogger() {}
+
+int SystemLogger::printMessage(String prefix, String message) {
+    if (output_->availableForWrite()) {
+        output_->print(millis());
+        output_->print(prefix);
+        output_->println(message);
+        return 0;
+    }
+    return 1;
+}
+
+#ifdef INFO
+int SystemLogger::info(String message) {
+    return this->printMessage(" [INFO] ", message);
+}
+
+int SystemLogger::dumpConfig() {
+    VehicleConfig config;
+    configGlobalRead(config);
+
+    char buffer[64];
+
+    // Auto detect
+    snprintf(buffer, sizeof(buffer),
+        "\tAuto detect tracks: %d",
+        config.auto_detect_track_
+    );
+    this->info(String(buffer));
+
+    // Track fallback
+    snprintf(buffer, sizeof(buffer),
+        "\tTrack fallback: %d",
+        config.track_fallback_
+    );
+    this->info(String(buffer));
+    
+    
+    this->info("\tVBAT cal factor: " + String(config.vbat_calibration_, 6));
+    this->info("\tVBAT low: " + String(config.vbat_low_, 2));
+    this->info("\tTENG low: " + String(config.teng_low_, 2));
+    this->info("\tTENG high: " + String(config.teng_high_, 2));
+
+    // Track slots (one per line)
+    for (size_t i = 0; i < 8; i++) {
+        snprintf(buffer, sizeof(buffer),
+            "\tTrack slot %d: %d",
+            i + 1,
+            config.track_slot_occupied_[i]
+        );
+        this->info(String(buffer));
+    }
+    
+    
+
+    return 0;
+}
+#endif
+
+#ifdef WARN
+int SystemLogger::warn(String message) {
+    return this->printMessage(" [WARNING] ", message);
+}
+#endif
+
+#ifdef ERROR
+int SystemLogger::error(String message) {
+    return this->printMessage(" [ERROR] ", message);
+}
+#endif
+
+#ifdef DEBUG
+int SystemLogger::debug(String message) {
+    return this->printMessage(" [DEBUG] ", message);
+}
+#endif
+
+#ifdef DEEP_DEBUG
+int SystemLogger::deepDebug(String message) {
+    return this->printMessage(" [DEEP_DEBUG] ", message);
+}
+#endif

src/modules/logger/system_logger.h

@@ -0,0 +1,41 @@
+// 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 <Arduino.h>
+#include "custom_types.h"
+#include "flags.h"
+#include "base/ring_buffer.h"
+#include "base/task.h"
+#include "base/module_base.h"
+#include "data/config_store.h"
+
+class SystemLogger {
+private:
+    HardwareSerial *output_;
+    int printMessage(String prefix, String message);
+public:
+    SystemLogger(HardwareSerial *output);
+    ~SystemLogger();
+    #ifdef INFO
+    int info(String message);
+    int dumpConfig();
+    #endif
+    
+    #ifdef WARN
+    int warn(String message);
+    #endif
+    
+    #ifdef ERROR
+    int error(String message);
+    #endif
+    
+    #ifdef DEBUG
+    int debug(String message);
+    #endif
+
+    #ifdef DEEP_DEBUG
+    int deepDebug(String message);
+    #endif
+};

src/modules/telemetry/telemetry.cpp

@@ -0,0 +1,48 @@
+// 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 "telemetry.h"
+#include "base/router.h"
+#include "data/general_store.h"
+
+int Telemetry::push(const Task &task) { return queue_.push(task); }
+
+Telemetry::Telemetry(HardwareSerial* data_stream) : logger_(nullptr), data_stream_(data_stream) {}
+
+Telemetry::Telemetry(HardwareSerial* data_stream,SystemLogger *logger) : logger_(logger), data_stream_(data_stream) {}
+
+Telemetry::~Telemetry() {}
+
+int Telemetry::init() {
+    data_stream_->begin(115200);
+    lora_header_.source_ = 0x01;
+    lora_header_.dest_ = 0x02;
+    return 0;
+}
+
+int Telemetry::loop() {
+    unsigned long now = millis();
+    if (now > last_sent_ + update_interval_) {
+        TelemetryPacket2 packet;
+        tengGlobalRead(packet.teng);
+        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_;
+        lora_header_.size_ = sizeof(packet);
+        lora_header_.crc16_ = crc16_ccitt((uint8_t*)&packet, sizeof(packet));
+        lora_header_.version_ = 2;
+        uart_header_.size_ = sizeof(packet) + sizeof(lora_header_);
+        
+        if (data_stream_->availableForWrite()) {
+            data_stream_->write((uint8_t*)&uart_header_, sizeof(uart_header_));
+            data_stream_->write((uint8_t*)&lora_header_, sizeof(lora_header_));
+            data_stream_->write((uint8_t*)&packet, sizeof(packet));
+        }
+        last_sent_ = millis();
+    }
+    return 0;
+}

src/modules/telemetry/telemetry.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"
+#include "telemetry_common/telemetry_common.h"
+#include <Arduino.h>
+#include "custom_types.h"
+#include "data/gps_store.h"
+
+class Telemetry : public ModuleBase {
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+  HardwareSerial *data_stream_;
+  unsigned long last_sent_ = 0;
+  unsigned long update_interval_ = 1000;
+  TelemetryLoRaHeader lora_header_;
+  TelemetryUARTHeader uart_header_;
+
+public:
+  int push(const Task &task) override;
+  Telemetry(HardwareSerial *data_stream);
+  Telemetry(HardwareSerial *data_stream, SystemLogger *logger);
+  ~Telemetry();
+  int init();
+  int loop();
+};

src/modules/thermocouple/thermocouple.cpp

@@ -0,0 +1,70 @@
+// 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 "thermocouple.h"
+#include "base/router.h"
+#include "data/general_store.h"
+
+int Thermocouple::push(const Task &task) { return queue_.push(task); }
+
+Thermocouple::Thermocouple() : logger_(nullptr) {}
+
+Thermocouple::Thermocouple(SystemLogger *logger) : logger_(logger) {}
+
+Thermocouple::~Thermocouple() {}
+
+int Thermocouple::init() {
+  thermocouple_ = new MAX6675(THERMO_SCK, THERMO_CS, THERMO_SO);
+  VehicleConfig config;
+  configGlobalRead(config);
+  low_ = config.teng_low_;
+  high_ = config.teng_high_;
+  return 0;
+}
+
+int Thermocouple::loop(unsigned long timeout_ms) {
+  (void)timeout_ms;
+
+  if (millis() > last_read_at_ + update_interval_) {
+    temperature_ = thermocouple_->readCelsius();
+    tengGlobalWrite(temperature_);
+    last_read_at_ = millis();
+    if (temperature_ > high_) {
+      if (warning_sent_at_ == 0 ||
+          millis() > warning_sent_at_ + warning_timeout_) {
+        router::send(module::Lcd, task::DisplayMsgEngineTempHigh, 2000);
+        warning_sent_at_ = millis();
+      }
+    } else if (temperature_ < low_) {
+      if (warning_sent_at_ == 0 ||
+          millis() > warning_sent_at_ + warning_timeout_) {
+        router::send(module::Lcd, task::DisplayMsgEngineTempLow, 2000);
+        warning_sent_at_ = millis();
+      }
+    }
+
+    Task active_task;
+    int res = queue_.pop(active_task);
+    if (res == 0) {
+      if (active_task.target_ == module::Thermocouple) {
+
+      } else if (active_task.target_ == module::All) {
+
+        switch (active_task.type_) {
+        case task::AllConfigUpdated: {
+          VehicleConfig config;
+          configGlobalRead(config);
+          low_ = config.teng_low_;
+          high_ = config.teng_high_;
+          break;
+        }
+
+        default:
+          break;
+        }
+      };
+    }
+    return 0;
+  }
+}

src/modules/thermocouple/thermocouple.h

@@ -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
+#pragma once
+#define THERMO_CS A1
+#define THERMO_SO A0
+#define THERMO_SCK A2
+#include "base/module_base.h"
+#include "base/ring_buffer.h"
+#include "custom_types.h"
+#include "modules/logger/system_logger.h"
+#include <Arduino.h>
+#include <max6675.h>
+
+class Thermocouple : public ModuleBase {
+private:
+  SystemLogger *logger_;
+  RingBuffer<Task, 16> queue_;
+  MAX6675 *thermocouple_;
+  float temperature_;
+  float low_;
+  float high_;
+  unsigned long update_interval_ = 1000;
+  unsigned long last_read_at_ = 0;
+  unsigned long warning_sent_at_ = 0;
+  unsigned long warning_timeout_ = 10000;
+
+public:
+  int push(const Task &task) override;
+  Thermocouple();
+  Thermocouple(SystemLogger *logger);
+  ~Thermocouple();
+  int init();
+  int loop(unsigned long timeout_ms = 500);
+};

src/telemetry_common

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