Added NULL types for module target and task type

.clang-format

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

platformio.ini

@@ -12,3 +12,10 @@
 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

src/base/module_base.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 "base/task.h"
+
+class module_base
+{
+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"
+
+module_base* modules[MOD_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 module_base* modules[MOD_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 ring_buffer {
+    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 is_empty() const { return _count == 0;}
+        bool is_full() const { return _count == SIZE;}
+        uint8_t size() const { return _count;}
+};

src/base/router.h

@@ -0,0 +1,27 @@
+// 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) {
+       if (task.target >= MOD_COUNT) {
+        return 1;
+       } 
+       
+        module_base* mod = modules[task.target];
+        
+        if (mod == nullptr) {
+            return 1;
+        }
+        
+        return mod->push(task);
+    }
+    
+    int send(module_id target, task_type type, uint32_t data=0) {
+        Task t{target, type, data};
+        return send(t);
+    }
+}

src/base/task.h

@@ -0,0 +1,27 @@
+// 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>
+
+enum module_id : uint8_t {
+    MOD_CMD,
+    MOD_CFG,
+    MOD_GPS,
+    MOD_LCD,
+    MOD_COUNT,
+    MOD_NULL
+};
+
+enum task_type : uint8_t {
+    TASK_NULL,
+    TASK_DISPLAY_GPS_DEBUG,
+    TASK_CONFIG_TRACK_DETECT,
+};
+
+struct Task {
+    module_id target;
+    task_type type;
+    uint32_t data;
+};

src/custom_types.h

@@ -0,0 +1,68 @@
+// 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>
+
+#define CONFIG_MAGIC 0xBEEF
+struct vehicle_config{
+    uint16_t magic = CONFIG_MAGIC;
+    bool auto_detect_track = true;
+    uint8_t track_fallback = 0;
+    bool track_slot_occupied[8] = {false};
+};
+
+struct lat_lng {
+    double lat;
+    double lng;
+};
+
+struct track_data {
+    uint16_t magic = CONFIG_MAGIC;
+    int id;
+    char name[32];
+    lat_lng pt_a;
+    lat_lng pt_b;
+};
+
+struct gps_sub_data {
+    uint32_t age;
+    bool valid;
+    double value;
+};
+
+struct gps_data {
+    gps_sub_data altitude;
+    gps_sub_data lat;
+    gps_sub_data lng;
+    gps_sub_data speed;
+    gps_sub_data course;
+};
+
+inline void gps_sub_copy_from_volatile(gps_sub_data& dst, const volatile gps_sub_data& src) {
+    dst.age   = src.age;
+    dst.valid = src.valid;
+    dst.value = src.value;
+}
+
+inline void gps_sub_copy_to_volatile(volatile gps_sub_data& dst, const gps_sub_data& src) {
+    dst.age   = src.age;
+    dst.valid = src.valid;
+    dst.value = src.value;
+}
+
+inline void gps_copy_from_volatile(gps_data& dst, const volatile gps_data& src) {
+    gps_sub_copy_from_volatile(dst.altitude, src.altitude);
+    gps_sub_copy_from_volatile(dst.lat,      src.lat);
+    gps_sub_copy_from_volatile(dst.lng,      src.lng);
+    gps_sub_copy_from_volatile(dst.speed,    src.speed);
+    gps_sub_copy_from_volatile(dst.course,   src.course);
+}
+
+inline void gps_copy_to_volatile(volatile gps_data& dst, const gps_data& src) {
+    gps_sub_copy_to_volatile(dst.altitude, src.altitude);
+    gps_sub_copy_to_volatile(dst.lat,      src.lat);
+    gps_sub_copy_to_volatile(dst.lng,      src.lng);
+    gps_sub_copy_to_volatile(dst.speed,    src.speed);
+    gps_sub_copy_to_volatile(dst.course,   src.course);
+}

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 gps_data gps_data_global = {};
+
+ void gps_global_read(gps_data& out) {
+     gps_copy_from_volatile(out, gps_data_global);
+ }
+
+ void gps_global_write(const gps_data& in) {
+    gps_copy_to_volatile(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 gps_data gps_data_global;
+
+void gps_global_read(gps_data& out);
+void gps_global_write(const gps_data& 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,67 @@
-#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"
+
+
+
+system_logger *logger_output = new system_logger(&Serial);
+
+lcd *driver_display = new lcd(logger_output);
+gps *gps_module = new gps(&Serial2, logger_output);
+config *system_config = new config(logger_output);
+cmd *command_handler = new cmd(&Serial, logger_output);
 
-// put function declarations here:
-int myFunction(int, int);
 
 void setup() {
-  // put your setup code here, to run once:
+  wdt_disable();
-  int result = myFunction(2, 3);
+
+  modules[MOD_LCD] = driver_display;
+  modules[MOD_GPS] = gps_module;
+  modules[MOD_CFG] = system_config;
+  modules[MOD_CMD] = command_handler;
+
+  driver_display->init();
+  driver_display->print_message("Starting Initialization");
+  delay(1000);
+
+  driver_display->print_message("Cmd Init...");
+  command_handler->init();
+  delay(1000);
+  driver_display->print_message("Cmd Init Complete");
+  delay(1000);
+
+  driver_display->print_message("Config Init...");
+  int result = system_config->auto_init();
+  delay(1000);
+  if (result != 0) {
+    driver_display->print_message("Configuration Read Failed");
+  } else {
+    driver_display->print_message("Config Init Complete");
+  }
+  delay(1000);
+
+  driver_display->print_message("GPS Init...");
+  gps_module->init();
+  delay(1000);
+  driver_display->print_message("GPS Init Complete");
+  router::send(MOD_LCD, TASK_DISPLAY_GPS_DEBUG);
+  router::send(MOD_CFG, TASK_CONFIG_TRACK_DETECT);
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+  gps_module->loop();
-}
+  driver_display->loop();
-
+  command_handler->parse_task();
-// put function definitions here:
-int myFunction(int x, int y) {
-  return x + y;
 }

src/modules/cmd/cmd.h

@@ -0,0 +1,102 @@
+// 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 "modules/logger/system_logger.h"
+#include "base/task.h"
+#include "base/ring_buffer.h"
+#include "base/module_base.h"
+#include <avr/wdt.h>
+
+class cmd: public module_base{
+private:
+  HardwareSerial *_data_stream;
+  unsigned long _baud_rate = 115200;
+  system_logger *_logger;
+  char _buffer[256];
+  unsigned int _idx = 0;
+  bool _buf_open = false;
+  int try_parse();
+  ring_buffer<Task, 16> _queue;
+public:
+    int push(const Task& task) override;
+  cmd(HardwareSerial *data_stream);
+  cmd(HardwareSerial *data_stream, system_logger *logger);
+  ~cmd();
+  int init();
+  int parse_task(unsigned long timeout_ms = 500);
+};
+
+int cmd::try_parse() {
+#ifdef DEBUG
+  if (_logger != nullptr) {
+    _logger->debug("Attempting to parse command");
+  }
+#endif
+  if (strcmp(_buffer, "REBOOT") == 0) {
+#ifdef INFO
+    if (_logger != nullptr) {
+      _logger->info("Rebooting");
+    }
+#endif
+    delay(200);
+    wdt_enable(WDTO_15MS);
+    while (true) {
+    }
+  }
+  return 0;
+}
+
+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, system_logger *logger)
+    : _data_stream(data_stream), _logger(logger) {}
+
+cmd::~cmd() {}
+
+int cmd::init() {
+  _data_stream->begin(_baud_rate);
+  return 0;
+}
+
+int cmd::parse_task(unsigned long timeout_ms) {
+  unsigned long timeout = millis() + timeout_ms;
+  while (_data_stream->available()) {
+    char c = _data_stream->read();
+    if (c == '<') {
+      _buf_open = true;
+      _idx = 0;
+      continue;
+    }
+    if (_idx >= sizeof(_buffer) - 1) {
+      _buf_open = false;
+      _idx = 0;
+#ifdef ERROR
+      if (_logger != nullptr) {
+        _logger->error("Command parser buffer overflow");
+      }
+#endif
+      return 1;
+    }
+    if (c == '>') {
+      _buffer[_idx] = '\0';
+      _buf_open = false;
+      return this->try_parse();
+    }
+    if (_buf_open) {
+
+      _buffer[_idx] = c;
+      _idx++;
+    }
+    if (millis() > timeout) {
+      return 1;
+    }
+  }
+  return 0;
+}

src/modules/config/config.h

@@ -0,0 +1,72 @@
+// 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 "flags.h"
+#include "modules/logger/system_logger.h"
+#include "base/task.h"
+#include "base/ring_buffer.h"
+#include "base/module_base.h"
+#include <EEPROM.h>
+
+class config : public module_base{
+private:
+  vehicle_config _config;
+  system_logger *_logger;
+    bool _valid_config;
+    track_data _loaded_track;
+  ring_buffer<Task, 16> _queue;
+    int read_cfg();
+    int write_cfg();
+    int write_cfg(const vehicle_config& new_config);
+public:
+    int push(const Task& task) override;
+  config();
+  config(system_logger *logger);
+  ~config();
+  int auto_init();
+};
+
+int config::push(const Task& task) {
+    return _queue.push(task);
+}
+
+config::config() : _logger(nullptr), _valid_config(true) {}
+config::config(system_logger *logger) : _logger(logger), _valid_config(true) {}
+
+config::~config() {}
+
+int config::read_cfg() {
+    EEPROM.get(0, _config); 
+    return 0;
+}
+
+int config::write_cfg(const vehicle_config& new_config) {
+    EEPROM.put(0, new_config);
+    return 0;
+}
+
+int config::auto_init() {
+    this->read_cfg();
+    if (_config.magic != CONFIG_MAGIC) {
+        #ifdef WARN
+        if (_logger != nullptr) {
+            _logger->warn("Config invalid, overwriting");
+        }
+        #endif
+        vehicle_config clean_config;
+        this->write_cfg(clean_config);
+        this->read_cfg();
+        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;
+}

src/modules/gps/gps.h

@@ -0,0 +1,96 @@
+// 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"
+#define MOD "modules/gps/gps.h"
+
+class gps : public module_base{
+private:
+  TinyGPSPlus *_gps;
+  HardwareSerial *_data_stream;
+  system_logger *_logger;
+  ring_buffer<Task, 16> _queue;
+public:
+  int push(const Task& task) override;
+  gps(HardwareSerial *data_stream);
+  gps(HardwareSerial *data_stream, system_logger *logger);
+  ~gps();
+  int loop(unsigned long timeout_ms = 500);
+  int init();
+  gps_data get_data();
+  
+};
+
+int gps::push(const Task& task) {
+  return _queue.push(task);
+}
+
+gps::gps(HardwareSerial *data_stream)
+    : _data_stream(data_stream), _logger(nullptr) {
+  _data_stream = data_stream;
+  _gps = new TinyGPSPlus();
+}
+
+gps::gps(HardwareSerial *data_stream, system_logger *logger)
+    : _data_stream(data_stream), _logger(logger) {
+  _gps = new TinyGPSPlus();
+}
+
+gps::~gps() {
+  _data_stream = nullptr;
+  delete _gps;
+}
+
+int gps::init() {
+  _data_stream->begin(9600);
+  return 0;
+}
+
+int gps::loop(unsigned long timeout_ms) {
+
+  unsigned long timeout = millis() + timeout_ms;
+  while (_data_stream->available() > 0) {
+    if (_gps->encode(_data_stream->read())) {
+      gps_global_write(this->get_data());
+    }
+    if (millis() > timeout) {
+      return 1;
+    }
+  }
+  return 0;
+}
+
+gps_data gps::get_data() {
+  gps_data 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();
+  
+  return output;
+}
+#undef MOD

src/modules/lcd/lcd.h

@@ -0,0 +1,293 @@
+// 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 "flags.h"
+#include "modules/logger/system_logger.h"
+#include "modules/gps/gps.h"
+#include <LiquidCrystal_I2C.h>
+#include "base/task.h"
+#include "base/ring_buffer.h"
+#include "base/module_base.h"
+#include "data/gps_store.h"
+#define MOD "modules/lcd/lcd.h"
+
+namespace screen
+{
+  
+typedef enum lcd_screen {
+  blank, 
+  gps_debug,
+};
+} // namespace screen
+
+class lcd : public module_base{
+private:
+  LiquidCrystal_I2C *_display;
+  bool _dispaly_cleared;
+  system_logger *_logger = nullptr;
+  screen::lcd_screen _screen;
+  unsigned long _last_render;
+  unsigned long _frame_duration;
+  ring_buffer<Task, 16> _queue;
+  void clear();
+  void print(const String& msg);
+  void print(char);
+  void print(const char []);
+  void print(double val, int digits);
+  void print(unsigned long val, int base);
+  void print(long val, int base);
+  void print(unsigned int val, int base);
+  void print(int val, int base);
+  
+  int render_gps_debug();
+public:
+  int push(const Task& task) override;
+  lcd();
+  lcd(system_logger *logger);
+  ~lcd();
+  int init();
+  int print_message(String message);
+  int loop(unsigned long timeout_ms=500);
+};
+
+void lcd::clear() {
+  if (!_dispaly_cleared) {
+    _display->clear();
+    _dispaly_cleared = true;
+  }
+}
+
+void lcd::print(const String& msg) {
+    _display->print(msg);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(char c) {
+    _display->print(c);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(const char c[]) {
+    _display->print(c);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(double d, int digits=2) {
+    _display->print(d, digits);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(unsigned long l, int base=10) {
+    _display->print(l, base);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(long l, int base=10) {
+    _display->print(l, base);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(unsigned int i, int base=10) {
+    _display->print(i, base);
+    _dispaly_cleared = false;
+}
+
+void lcd::print(int i, int base=10) {
+    _display->print(i, base);
+    _dispaly_cleared = false;
+}
+
+int lcd::render_gps_debug() {
+  
+    this->clear();
+    gps_data data;
+    gps_global_read(data);
+
+    _display->setCursor(0,0);
+    this->print("Alt: ");
+    if (data.altitude.valid) {
+      this->print(data.altitude.value, 5);
+    } else {
+      this->print("not valid");
+    }
+
+    _display->setCursor(0,1);
+    this->print("Lat: ");
+    if (data.lat.valid) {
+      this->print(data.lat.value, 5);
+    } else {
+      this->print("not valid");
+    }
+
+    _display->setCursor(0,2);
+    this->print("Lng: ");
+    if (data.lng.valid) {
+      this->print(data.lng.value, 5);
+    } else {
+      this->print("not valid");
+    }
+
+    _display->setCursor(0,3);
+    this->print("Spd: ");
+    if (data.speed.valid) {
+      this->print(data.speed.value, 5);
+    } else {
+      this->print("not valid");
+    }
+}
+
+int lcd::push(const Task& task) {
+  return _queue.push(task);
+}
+
+lcd::lcd(): _logger(nullptr), _screen(screen::blank), _last_render(0), _frame_duration(2000), _dispaly_cleared(false) { _display = new LiquidCrystal_I2C(0x27, 20, 4); }
+
+lcd::lcd(system_logger *logger): _logger(logger), _screen(screen::blank), _last_render(0), _frame_duration(2000), _dispaly_cleared(false) {
+  _display = new LiquidCrystal_I2C(0x27, 20, 4);
+
+}
+
+lcd::~lcd() {}
+
+int lcd::init() {
+#ifdef DEEP_DEBUG
+  if (_logger != nullptr) {
+    _logger->deep_debug(String(MOD) + ": init: Begin");
+  }
+#endif
+  _display->init();
+  Wire.setClock(400000);
+  _display->backlight();
+  this->clear();
+  _display->setCursor(0, 0);
+#ifdef DEEP_DEBUG
+  if (_logger != nullptr) {
+    _logger->deep_debug(String(MOD) + ": init: End");
+  }
+#endif
+  return 0;
+}
+
+
+int lcd::print_message(String message) {
+#ifdef DEEP_DEBUG
+  if (_logger != nullptr) {
+    _logger->deep_debug(String(MOD) + ": print_message: 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->deep_debug(String(MOD) + ": print_message: End");
+  }
+#endif
+
+  return 0;
+}
+
+int lcd::loop(unsigned long timeout_ms) {
+  unsigned long now = millis();
+  unsigned long task_timeout = now + timeout_ms;
+  while (_queue.size() > 0) {
+    Task next_task;
+    int res = _queue.pop(next_task);
+    if (res != 0 ) {
+      if (millis() > task_timeout) {
+        break;
+      }
+      continue;
+    }
+    switch (next_task.type)
+    {
+    case TASK_DISPLAY_GPS_DEBUG:
+      _screen = screen::gps_debug;
+      break;
+    
+    default:
+      break;
+    }
+    if (millis() > task_timeout) {
+      break;
+    }
+
+  }
+  if (now < _last_render + _frame_duration) {
+    return 1;
+  }
+  switch (_screen)
+  {
+  case screen::blank:
+    this->clear();
+    break;
+    
+  case screen::gps_debug:
+    this->render_gps_debug();
+    break;
+  
+  default:
+    break;
+  }
+  _last_render = millis();
+  return 1;
+}
+
+#undef MOD

src/modules/logger/system_logger.h

@@ -0,0 +1,87 @@
+// 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 "flags.h"
+#include "base/ring_buffer.h"
+#include "base/task.h"
+#include "base/module_base.h"
+
+class system_logger
+{
+private:
+    HardwareSerial *_output;
+    int print_message(String pre, String message);
+public:
+    system_logger(HardwareSerial *output);
+    ~system_logger();
+    #ifdef INFO
+    int info(String message);
+    #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 deep_debug(String message);
+    #endif
+};
+
+system_logger::system_logger(HardwareSerial *output)
+{
+    _output = output;
+}
+
+system_logger::~system_logger()
+{
+}
+
+int system_logger::print_message(String pre, String message) {
+    if (_output->availableForWrite()) {
+        _output->print(millis());
+        _output->print(pre);
+        _output->println(message);
+        return 0;
+    }
+    return 1;
+}
+
+#ifdef INFO
+int system_logger::info(String message) {
+    return this->print_message(" [INFO] ", message);
+}
+#endif
+
+#ifdef WARN
+int system_logger::warn(String message) {
+    return this->print_message(" [WARNING] ", message);
+}
+#endif
+
+#ifdef ERROR
+int system_logger::error(String message) {
+    return this->print_message(" [ERROR] ", message);
+}
+#endif
+
+#ifdef DEBUG
+int system_logger::debug(String message) {
+    return this->print_message(" [DEBUG] ", message);
+}
+#endif
+
+#ifdef DEEP_DEBUG
+int system_logger::deep_debug(String message) {
+    return this->print_message(" [DEEP_DEBUG] ", message);
+}
+#endif