diff --git a/src/base/router.cpp b/src/base/router.cpp new file mode 100644 index 0000000..cfccb64 --- /dev/null +++ b/src/base/router.cpp @@ -0,0 +1,26 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "router.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) { + Task t{target, type, data}; + return send(t); + } +} \ No newline at end of file diff --git a/src/base/router.h b/src/base/router.h index 0664faa..7e634e1 100644 --- a/src/base/router.h +++ b/src/base/router.h @@ -2,26 +2,12 @@ // Copyright (C) 2026 Association Exergie // 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); - } + int send(const Task& task); + int send(module_id target, task_type type, uint32_t data = 0); } \ No newline at end of file diff --git a/src/data/config_store.cpp b/src/data/config_store.cpp new file mode 100644 index 0000000..9e705d4 --- /dev/null +++ b/src/data/config_store.cpp @@ -0,0 +1,14 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later +#include "data/config_store.h" + + volatile vehicle_config config_global = {}; + + void config_global_read(vehicle_config& out) { + copy_from_volatile(out, config_global); + } + + void track_global_write(const vehicle_config& in) { + copy_to_volatile(config_global, in); + } \ No newline at end of file diff --git a/src/data/config_store.h b/src/data/config_store.h new file mode 100644 index 0000000..e346526 --- /dev/null +++ b/src/data/config_store.h @@ -0,0 +1,12 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later +#pragma once + +#include "custom_types.h" +#include + +extern volatile vehicle_config config_global; + +void config_global_read(vehicle_config& out); +void config_global_write(const vehicle_config& in); \ No newline at end of file diff --git a/src/data/track_store.cpp b/src/data/track_store.cpp index 2683811..c1f473c 100644 --- a/src/data/track_store.cpp +++ b/src/data/track_store.cpp @@ -4,6 +4,7 @@ #include "data/track_store.h" volatile track_data track_data_global = {}; +volatile track_data track_data_temp_global = {}; void track_global_read(track_data& out) { copy_from_volatile(out, track_data_global); diff --git a/src/data/track_store.h b/src/data/track_store.h index 51f7c99..146fce2 100644 --- a/src/data/track_store.h +++ b/src/data/track_store.h @@ -7,7 +7,11 @@ #include extern volatile track_data track_data_global; +extern volatile track_data track_data_temp_global; void track_global_read(track_data& out); int track_global_read(unsigned short idx, track_data& out); -void track_global_write(const track_data& in); \ No newline at end of file +void track_global_write(const track_data& in); + +void track_temp_global_read(track_data& out); +void track_temp_global_write(const track_data& in); \ No newline at end of file diff --git a/src/modules/cmd/cmd.cpp b/src/modules/cmd/cmd.cpp new file mode 100644 index 0000000..37952b8 --- /dev/null +++ b/src/modules/cmd/cmd.cpp @@ -0,0 +1,200 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "cmd.h" + +#include + +char *cmd::trim_arg(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::split_args(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] = trim_arg(token_start); + argc++; + + if (separator == '\0') { + break; + } + + token_start = p + 1; + } + + p++; + } + + return argc; +} + +cmd::command_id cmd::parse_command_name(const char *input) { + if (input == nullptr) { + return CMD_UNKNOWN; + } + + if (strcmp(input, "REBOOT") == 0) { + return CMD_REBOOT; + } + + if (strcmp(input, "DUMPCFG") == 0) { + return CMD_DUMPCFG; + } + + return CMD_UNKNOWN; +} + +int cmd::dispatch_command(command_id command, unsigned short argc, char *argv[]) { + switch (command) { + case CMD_REBOOT: +#ifdef INFO + if (_logger != nullptr) { + _logger->info("Rebooting"); + } +#endif + delay(200); + wdt_enable(WDTO_15MS); + while (true) { + } + + case CMD_DUMPCFG: +#ifdef INFO + if (_logger != nullptr) { + _logger->dump_config(); + } +#endif + return 0; + + case CMD_UNKNOWN: + default: +#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::try_parse() { +#ifdef DEBUG + if (_logger != nullptr) { + _logger->debug("Attempting to parse command"); + } +#endif + + char *argvp[_max_args]; + unsigned short argc = split_args(_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; + } + + command_id command = parse_command_name(argvp[0]); + return dispatch_command(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, 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 start = millis(); + + while (_data_stream->available()) { + if ((unsigned long)(millis() - start) >= timeout_ms) { + return 1; + } + + char c = _data_stream->read(); + + if (c == '<') { + _buf_open = true; + _idx = 0; + continue; + } + + if (!_buf_open) { + continue; + } + + if (c == '>') { + _buffer[_idx] = '\0'; + _buf_open = false; + return this->try_parse(); + } + + if (_idx >= sizeof(_buffer) - 1) { + _buf_open = false; + _idx = 0; +#ifdef ERROR + if (_logger != nullptr) { + _logger->error("Command parser buffer overflow"); + } +#endif + return 1; + } + + _buffer[_idx] = c; + _idx++; + } + + return 0; +} \ No newline at end of file diff --git a/src/modules/cmd/cmd.h b/src/modules/cmd/cmd.h index 568b40c..c67c753 100644 --- a/src/modules/cmd/cmd.h +++ b/src/modules/cmd/cmd.h @@ -2,101 +2,45 @@ // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later #pragma once + +#include +#include + #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 -class cmd: public module_base{ +class cmd : public module_base { private: + enum command_id { + CMD_UNKNOWN = 0, + CMD_REBOOT, + CMD_DUMPCFG + }; + 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 _queue; + + static const unsigned short _max_args = 10; + + int try_parse(); + unsigned short split_args(char *input, char *argv[], unsigned short max_args); + char *trim_arg(char *input); + command_id parse_command_name(const char *input); + int dispatch_command(command_id command, unsigned short argc, char *argv[]); + public: - int push(const Task& task) override; + 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; -} +}; \ No newline at end of file diff --git a/src/modules/config/config.cpp b/src/modules/config/config.cpp new file mode 100644 index 0000000..116c078 --- /dev/null +++ b/src/modules/config/config.cpp @@ -0,0 +1,188 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "config.h" + +#include +#include + +int config::push(const Task &task) { + return _queue.push(task); +} + +int config::task_complete() { + _task_memory_stale = true; + _active_task = {MOD_NULL, TASK_NULL, 0}; + return 0; +} + +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() { + EEPROM.put(0, _config); + return 0; +} + +int config::write_cfg(const vehicle_config &new_config) { + EEPROM.put(0, new_config); + return 0; +} + +int config::task_config_detect_track(unsigned long timeout_ms) { + unsigned long start = millis(); + bool min_one = false; + + for (size_t i = 0; i < 8; i++) { + if (_config.track_slot_occupied[i] == true) { + min_one = true; + break; + } + } + + if (!min_one) { + this->task_complete(); + return 1; + } + + task_config_track_detect_data task_data; + + if (!_task_memory_stale) { + memcpy(&task_data, _task_memory, sizeof(task_data)); + } else { + gps_data current_gps; + gps_global_read(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++; + + track_data temp; + int res = this->get_track(task_data.last_checked, temp); + if (res == 0) { + double delta_lat = temp.pt_a.lat - task_data.gps_lat; + double delta_lng = (temp.pt_a.lng - task_data.gps_lng) * task_data.cos; + double 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) { + this->load_track(task_data.smallest_idx); + this->task_complete(); + router::send(MOD_LCD, TASK_DISPLAY_MSG_TRACK_DETECT_OK, 4000); + return 0; + } + + if ((unsigned long)(millis() - start) >= timeout_ms) { + _task_memory_stale = false; + memcpy(_task_memory, &task_data, sizeof(task_data)); + return 1; + } + } +} + +int config::handle_active_task(unsigned long timeout_ms) { + switch (_active_task.type) { + case TASK_CONFIG_TRACK_DETECT: + if (!_is_track_loaded) { + return task_config_detect_track(timeout_ms); + } + break; + + default: + break; + } + 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; +} + +int config::loop(unsigned long timeout_ms) { + if (_active_task.type != TASK_NULL && _active_task.target != MOD_NULL) { + this->handle_active_task(timeout_ms); + return 0; + } + + int res = _queue.pop(_active_task); + if (res == 0) { + this->handle_active_task(timeout_ms); + } + + return 0; +} + +int config::get_track(unsigned int idx, track_data &t) { + if (idx < 1 || idx > 8) { + return 1; + } + + if (_config.track_slot_occupied[idx - 1] == false) { + return 1; + } + + EEPROM.get(idx, t); + if (t.magic != CONFIG_MAGIC) { + return 1; + } + + return 0; +} + +int config::load_track(unsigned int idx) { + if (idx < 1 || idx > 8) { + return 1; + } + + if (_config.track_slot_occupied[idx - 1] == false) { + return 1; + } + + track_data temp; + EEPROM.get(idx, temp); + if (temp.magic != CONFIG_MAGIC) { + return 1; + } + + _loaded_track = temp; + track_global_write(_loaded_track); + _is_track_loaded = true; + return 0; +} \ No newline at end of file diff --git a/src/modules/config/config.h b/src/modules/config/config.h index 55ed570..23e0722 100644 --- a/src/modules/config/config.h +++ b/src/modules/config/config.h @@ -2,6 +2,7 @@ // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later #pragma once + #include "base/module_base.h" #include "base/ring_buffer.h" #include "base/task.h" @@ -31,14 +32,15 @@ private: bool _is_track_loaded = false; ring_buffer _queue; Task _active_task = {}; - uint8_t _task_memory[64]; + uint8_t _task_memory[64] = {}; bool _task_memory_stale = true; + int read_cfg(); int write_cfg(); int write_cfg(const vehicle_config &new_config); int handle_active_task(unsigned long timeout_ms); int task_config_detect_track(unsigned long timeout_ms); - + int task_complete(); public: int push(const Task &task) override; @@ -49,148 +51,4 @@ public: int loop(unsigned long timeout_ms = 500); int get_track(unsigned int idx, track_data &t); int load_track(unsigned int idx); -}; - -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::task_config_detect_track(unsigned long timeout_ms) { - unsigned long now = millis(); - unsigned long task_timeout = now + timeout_ms; - task_config_track_detect_data task_data; - if (!_task_memory_stale) { - memcpy(&task_data, _task_memory, sizeof(task_data)); - } else { - gps_data current_gps; - gps_global_read(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++; - track_data temp; - int res = this->get_track(task_data.last_checked, temp); - if (res == 0) { - - double delta_lat = temp.pt_a.lat - task_data.gps_lat; - double delta_lng = (temp.pt_a.lng - task_data.gps_lng) * task_data.cos; - double 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) { - this->load_track(task_data.smallest_idx); - _task_memory_stale = true; - _active_task = {MOD_NULL, TASK_NULL, 0}; - router::send(MOD_LCD, TASK_DISPLAY_MSG_TRACK_DETECT_OK, 4000); - return 0; - } - } - if (millis() > task_timeout) { - _task_memory_stale = false; - memcpy(_task_memory, &task_data, sizeof(task_data)); - return 1; - } - } -} - -int config::handle_active_task(unsigned long timeout_ms) { - switch (_active_task.type) - { - case TASK_CONFIG_TRACK_DETECT: - if (!_is_track_loaded) { - return task_config_detect_track(timeout_ms); - } - break; - - default: - break; - } - 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; -} - -int config::loop(unsigned long timeout_ms) { - if (_active_task.type != TASK_NULL && _active_task.target != MOD_NULL) { - this->handle_active_task(timeout_ms); - return 0; - } - int res = _queue.pop(_active_task); - if (res == 0) { - - this->handle_active_task(timeout_ms); - } - return 0; -} - -int config::get_track(unsigned int idx, track_data &t) { - if (idx < 1 || idx > 8) { - return 1; - } - if (_config.track_slot_occupied[idx-1] == false) { - return 1; - } - EEPROM.get(idx, t); - if (t.magic != CONFIG_MAGIC) { - return 1; - } - return 0; -} - -int config::load_track(unsigned int idx) { - if (idx < 1 || idx > 8) { - return 1; - } - if (_config.track_slot_occupied[idx-1] == false) { - return 1; - } - track_data temp; - EEPROM.get(idx, temp); - if (temp.magic != CONFIG_MAGIC) { - return 1; - } - _loaded_track = temp; - track_global_write(_loaded_track); - _is_track_loaded = true; - return 0; -} +}; \ No newline at end of file diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp new file mode 100644 index 0000000..f64351c --- /dev/null +++ b/src/modules/gps/gps.cpp @@ -0,0 +1,84 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "gps.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, system_logger *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; + + while (_data_stream->available() > 0) { + if (_gps->encode(_data_stream->read())) { + gps_global_write(this->get_data()); + uint32_t current_fix_val = _gps->sentencesWithFix(); + if (_last_fix_val == 0 && current_fix_val > 0) { + router::send(MOD_LCD, TASK_DISPLAY_MSG_GPS_FIX, 2000); + router::send(MOD_CFG, TASK_CONFIG_TRACK_DETECT); + } + _last_fix_val = current_fix_val; + } + + 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(); + + output.num_fix = _gps->sentencesWithFix(); + + return output; +} + +#undef MOD \ No newline at end of file diff --git a/src/modules/gps/gps.h b/src/modules/gps/gps.h index f443759..52d95e6 100644 --- a/src/modules/gps/gps.h +++ b/src/modules/gps/gps.h @@ -2,6 +2,7 @@ // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later #pragma once + #include "custom_types.h" #include "TinyGPSPlus.h" #include "flags.h" @@ -11,96 +12,21 @@ #include "base/module_base.h" #include "data/gps_store.h" #include "base/router.h" -#define MOD "modules/gps/gps.h" -class gps : public module_base{ +class gps : public module_base { private: TinyGPSPlus *_gps; HardwareSerial *_data_stream; system_logger *_logger; ring_buffer _queue; uint32_t _last_fix_val = 0; + public: - int push(const Task& task) override; + 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()); - uint32_t current_fix_val = _gps->sentencesWithFix(); - if (_last_fix_val == 0 && current_fix_val > 0) { - router::send(MOD_LCD, TASK_DISPLAY_MSG_GPS_FIX, 2000); - router::send(MOD_CFG, TASK_CONFIG_TRACK_DETECT); - } - _last_fix_val = current_fix_val; - } - 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(); - - output.num_fix = _gps->sentencesWithFix(); - - return output; -} -#undef MOD \ No newline at end of file +}; \ No newline at end of file diff --git a/src/modules/lcd/lcd.cpp b/src/modules/lcd/lcd.cpp new file mode 100644 index 0000000..16aeed2 --- /dev/null +++ b/src/modules/lcd/lcd.cpp @@ -0,0 +1,345 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "lcd.h" + +#include +#include + +#define MOD "modules/lcd/lcd.h" + +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) { + _display->print(d, digits); + _dispaly_cleared = false; +} + +void lcd::print(unsigned long l, int base) { + _display->print(l, base); + _dispaly_cleared = false; +} + +void lcd::print(long l, int base) { + _display->print(l, base); + _dispaly_cleared = false; +} + +void lcd::print(unsigned int i, int base) { + _display->print(i, base); + _dispaly_cleared = false; +} + +void lcd::print(int i, int base) { + _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"); + } + + return 0; +} + +int lcd::render_msg_gps_fix() { + this->clear(); + _display->setCursor(6, 1); + this->print("GPS INFO"); + _display->setCursor(7, 2); + this->print("FIX OK"); + return 0; +} + +int lcd::render_msg_track_detect_ok() { + this->clear(); + _display->setCursor(6, 0); + this->print("GPS INFO"); + _display->setCursor(3, 1); + this->print("TRACK DETECTED"); + + track_data temp; + track_global_read(temp); + + _display->setCursor((20 - strlen(temp.name)) / 2, 2); + this->print(temp.name); + return 0; +} + +int lcd::push(const Task &task) { + return _queue.push(task); +} + +lcd::lcd() + : _logger(nullptr), + _screen(screen::blank), + _previous_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), + _previous_screen(screen::blank), + _last_render(0), + _frame_duration(2000), + _dispaly_cleared(false) { + _display = new LiquidCrystal_I2C(0x27, 20, 4); +} + +lcd::~lcd() { + delete _display; + _display = nullptr; +} + +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: + _previous_screen = _screen; + _screen = screen::gps_debug; + break; + + case TASK_DISPLAY_MSG_GPS_FIX: { + _previous_screen = _screen; + _screen = screen::msg_gps_fix; + unsigned long _msg_duration = next_task.data / _frame_duration; + if (_msg_duration == 0) { + _msg_duration = 1; + } + _hold_till_frame = _frame_ctr + _msg_duration; + break; + } + + case TASK_DISPLAY_MSG_TRACK_DETECT_OK: { + _previous_screen = _screen; + _screen = screen::msg_track_detect_ok; + unsigned long _msg_duration = next_task.data / _frame_duration; + if (_msg_duration == 0) { + _msg_duration = 1; + } + _hold_till_frame = _frame_ctr + _msg_duration; + break; + } + + default: + break; + } + + if (millis() > task_timeout) { + break; + } + } + + if (now < _last_render + _frame_duration) { + return 1; + } + + if (_hold_till_frame >= 0 && _frame_ctr >= (uint32_t)_hold_till_frame) { + _screen = _previous_screen; + _hold_till_frame = -1; + } + + switch (_screen) { + case screen::blank: + this->clear(); + break; + + case screen::gps_debug: + this->render_gps_debug(); + break; + + case screen::msg_gps_fix: + this->render_msg_gps_fix(); + break; + + case screen::msg_track_detect_ok: + this->render_msg_track_detect_ok(); + break; + + default: + break; + } + + _last_render = millis(); + _frame_ctr++; + return 1; +} + +#undef MOD \ No newline at end of file diff --git a/src/modules/lcd/lcd.h b/src/modules/lcd/lcd.h index a0504df..5eed9f8 100644 --- a/src/modules/lcd/lcd.h +++ b/src/modules/lcd/lcd.h @@ -2,29 +2,31 @@ // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later #pragma once + +#include +#include + #include "flags.h" #include "modules/logger/system_logger.h" #include "modules/gps/gps.h" -#include #include "base/task.h" #include "base/ring_buffer.h" #include "base/module_base.h" #include "data/gps_store.h" #include "data/track_store.h" -#define MOD "modules/lcd/lcd.h" -namespace screen -{ - +namespace screen { + typedef enum lcd_screen { - blank, + blank, gps_debug, msg_gps_fix, msg_track_detect_ok, -}; +} lcd_screen; + } // namespace screen -class lcd : public module_base{ +class lcd : public module_base { private: LiquidCrystal_I2C *_display; bool _dispaly_cleared; @@ -36,326 +38,27 @@ private: long _hold_till_frame = -1; ring_buffer _queue; uint32_t _frame_ctr = 0; + 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); - + void print(const String &msg); + void print(char c); + void print(const char c[]); + void print(double 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); + int render_gps_debug(); int render_msg_gps_fix(); int render_msg_track_detect_ok(); + public: - int push(const Task& task) override; + 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"); - } - return 0; -} - - -int lcd::render_msg_gps_fix() { - this->clear(); - _display->setCursor(6,1); - this->print("GPS INFO"); - _display->setCursor(7,2); - this->print("FIX OK"); - return 0; -} - -int lcd::render_msg_track_detect_ok() { - this->clear(); - _display->setCursor(6,0); - this->print("GPS INFO"); - _display->setCursor(3,1); - this->print("TRACK DETECTED"); - track_data temp; - track_global_read(temp); - strlen(temp.name); - _display->setCursor((20 - strlen(temp.name))/2, 2); - this->print(temp.name); - return 0; -} - -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: - _previous_screen = _screen; - _screen = screen::gps_debug; - break; - - case TASK_DISPLAY_MSG_GPS_FIX: - _previous_screen = _screen; - _screen = screen::msg_gps_fix; - unsigned long _msg_duration = next_task.data/_frame_duration; - if (_msg_duration == 0) { - _msg_duration == 1; - } - _hold_till_frame = _frame_ctr + _msg_duration; - break; - - case TASK_DISPLAY_MSG_TRACK_DETECT_OK: - _previous_screen = _screen; - _screen = screen::msg_track_detect_ok; - unsigned long _msg_duration = next_task.data/_frame_duration; - if (_msg_duration == 0) { - _msg_duration == 1; - } - _hold_till_frame = _frame_ctr + _msg_duration; - break; - - default: - break; - } - if (millis() > task_timeout) { - break; - } - - } - if (now < _last_render + _frame_duration) { - return 1; - } - - if (_hold_till_frame >= 0 && _frame_ctr >= _hold_till_frame) { - _screen = _previous_screen; - _hold_till_frame = -1; - } - switch (_screen) - { - case screen::blank: - this->clear(); - break; - - case screen::gps_debug: - this->render_gps_debug(); - break; - - case screen::msg_gps_fix: - this->render_msg_gps_fix(); - break; - - case screen::msg_track_detect_ok: - this->render_msg_track_detect_ok(); - break; - - default: - break; - } - _last_render = millis(); - _frame_ctr++; - return 1; -} - -#undef MOD \ No newline at end of file + int loop(unsigned long timeout_ms = 500); +}; \ No newline at end of file diff --git a/src/modules/logger/system_logger.cpp b/src/modules/logger/system_logger.cpp new file mode 100644 index 0000000..0eb1cf0 --- /dev/null +++ b/src/modules/logger/system_logger.cpp @@ -0,0 +1,89 @@ +// Copyright (C) 2026 Hector van der Aa +// Copyright (C) 2026 Association Exergie +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "system_logger.h" + +#include + +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); +} + +int system_logger::dump_config() { + vehicle_config temp; + config_global_read(temp); + + char buffer[64]; + + // Auto detect + snprintf(buffer, sizeof(buffer), + "\tAuto detect tracks: %d", + temp.auto_detect_track + ); + this->info(String(buffer)); + + // Track fallback + snprintf(buffer, sizeof(buffer), + "\tTrack fallback: %d", + temp.track_fallback + ); + this->info(String(buffer)); + + // Track slots (one per line) + for (size_t i = 0; i < 8; i++) { + snprintf(buffer, sizeof(buffer), + "\tTrack slot %d: %d", + i, + temp.track_slot_occupied[i] + ); + this->info(String(buffer)); + } + + return 0; +} +#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 \ No newline at end of file diff --git a/src/modules/logger/system_logger.h b/src/modules/logger/system_logger.h index fb96adf..6ef5fa8 100644 --- a/src/modules/logger/system_logger.h +++ b/src/modules/logger/system_logger.h @@ -2,11 +2,14 @@ // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later #pragma once + #include +#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 system_logger { @@ -18,6 +21,7 @@ public: ~system_logger(); #ifdef INFO int info(String message); + int dump_config(); #endif #ifdef WARN @@ -35,53 +39,4 @@ public: #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 \ No newline at end of file +}; \ No newline at end of file