From ecccadc91318e2d94306d4f60fbf7d8e4017def5 Mon Sep 17 00:00:00 2001 From: Hector van der Aa Date: Fri, 3 Apr 2026 16:20:45 +0200 Subject: [PATCH] LapCounter finished and working --- src/base/task.h | 3 + src/custom_types.cpp | 2 +- src/custom_types.h | 14 ++ src/data/general_store.cpp | 35 ++- src/data/general_store.h | 5 + src/main.cpp | 5 + src/modules/gps/gps.cpp | 5 + src/modules/lap_counter/lap_counter.cpp | 63 +++++- src/modules/lap_counter/lap_counter.h | 8 + src/modules/lcd/lcd.cpp | 276 ++++++++++++++---------- src/modules/lcd/lcd.h | 4 + 11 files changed, 283 insertions(+), 137 deletions(-) diff --git a/src/base/task.h b/src/base/task.h index 692d730..fe12644 100644 --- a/src/base/task.h +++ b/src/base/task.h @@ -35,6 +35,8 @@ enum Type : uint8_t { DisplayMsgBatteryLow, DisplayMsgEngineTempLow, DisplayMsgEngineTempHigh, + DisplayMsgLapCounterStart, + DisplayMsgLapCounterLapTime, ConfigTrackDetect, ConfigWriteTempTrack, ConfigTrackDelete, @@ -46,6 +48,7 @@ enum Type : uint8_t { BatteryCal, AllConfigUpdated, AllTrackLoaded, + AllStartLineTriggered, }; } // namespace task diff --git a/src/custom_types.cpp b/src/custom_types.cpp index d768c48..9a47cd4 100644 --- a/src/custom_types.cpp +++ b/src/custom_types.cpp @@ -68,4 +68,4 @@ float vec2SqDist(const Vec2& A, const Vec2& B) { float vec2Cross(const Vec2& B, const Vec2& M) { return B.x_ * M.y_ - B.y_ * M.x_; -} \ No newline at end of file +} diff --git a/src/custom_types.h b/src/custom_types.h index dc1f19f..458a4e3 100644 --- a/src/custom_types.h +++ b/src/custom_types.h @@ -63,6 +63,8 @@ struct GpsData { GpsSubData lng_; GpsSubData speed_; GpsSubData course_; + uint32_t time_; + uint32_t time_write_time_; uint32_t num_fix_; }; @@ -75,3 +77,15 @@ template 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; +} diff --git a/src/data/general_store.cpp b/src/data/general_store.cpp index a60e360..1c5d60a 100644 --- a/src/data/general_store.cpp +++ b/src/data/general_store.cpp @@ -3,30 +3,23 @@ // SPDX-License-Identifier: GPL-3.0-or-later #include "general_store.h" - volatile float vbat_global = 0; - volatile float teng_global = 0; - volatile int gps_trigger_global = 0; +volatile float vbat_global = 0; +volatile float teng_global = 0; +volatile int gps_trigger_global = 0; +volatile uint32_t last_lap_time_global = 0; - void vbatGlobalRead(float& out) { - out = vbat_global; - } +void vbatGlobalRead(float &out) { out = vbat_global; } - void vbatGlobalWrite(const float& in) { - vbat_global = in; - } +void vbatGlobalWrite(const float &in) { vbat_global = in; } - void tengGlobalRead(float& out) { - out = teng_global; - } +void tengGlobalRead(float &out) { out = teng_global; } - void tengGlobalWrite(const float& in) { - teng_global = in; - } +void tengGlobalWrite(const float &in) { teng_global = in; } -void gpsTriggerGlobalRead(int& out) { - out = gps_trigger_global; -} +void gpsTriggerGlobalRead(int &out) { out = gps_trigger_global; } -void gpsTriggerGlobalWrite(const int& in) { - gps_trigger_global = in; -} +void gpsTriggerGlobalWrite(const int &in) { gps_trigger_global = in; } + +void lastLapTimeGlobalRead(uint32_t &out) { out = last_lap_time_global; } + +void lastLapTimeGlobalWrite(const uint32_t &in) { last_lap_time_global = in; } diff --git a/src/data/general_store.h b/src/data/general_store.h index e5e7648..8233928 100644 --- a/src/data/general_store.h +++ b/src/data/general_store.h @@ -2,10 +2,12 @@ // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later #pragma once +#include extern volatile float vbat_global; extern volatile float teng_global; extern volatile int gps_trigger_global; +extern volatile uint32_t last_lap_time_global; void vbatGlobalRead(float& out); void vbatGlobalWrite(const float& in); @@ -15,3 +17,6 @@ void tengGlobalWrite(const float& in); void gpsTriggerGlobalRead(int& out); void gpsTriggerGlobalWrite(const int& in); + +void lastLapTimeGlobalRead(uint32_t& in); +void lastLapTimeGlobalWrite(const uint32_t& out); diff --git a/src/main.cpp b/src/main.cpp index 38efadd..f51fffd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,6 +16,7 @@ #include "modules/battery/battery.h" #include "modules/thermocouple/thermocouple.h" #include "modules/telemetry/telemetry.h" +#include "modules/lap_counter/lap_counter.h" SystemLogger *logger = new SystemLogger(&Serial); @@ -27,6 +28,7 @@ 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); @@ -40,6 +42,7 @@ void setup() { module_registry[module::Battery] = battery_module; module_registry[module::Thermocouple] = thermocouple_module; module_registry[module::Telemetry] = telemetry_module; + module_registry[module::LapCounter] = lap_counter_modules; display->init(); display->printMessage("Starting Initialization"); @@ -63,6 +66,7 @@ void setup() { display->printMessage("GPS Init..."); gps_module->init(); + lap_counter_modules->init(); delay(750); display->printMessage("GPS Init Complete"); delay(750); @@ -84,6 +88,7 @@ void setup() { void loop() { gps_module->loop(); + lap_counter_modules->loop(); display->loop(); command_handler->parseTask(); system_config->loop(); diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp index b7e584e..e3c473a 100644 --- a/src/modules/gps/gps.cpp +++ b/src/modules/gps/gps.cpp @@ -128,6 +128,7 @@ int Gps::loop(unsigned long timeout_ms) { gpsTriggerGlobalWrite(start_line_trigger_); arm_sign_ = 0; state_changed_at_ = now; + router::send(module::All, task::AllStartLineTriggered); } break; }; @@ -173,7 +174,11 @@ GpsData Gps::getData() { output.course_.valid_ = gps_->course.isValid(); output.course_.value_ = gps_->course.deg(); + output.time_ = gps_->time.value(); + output.time_write_time_ = millis() - gps_->time.age(); + output.num_fix_ = gps_->sentencesWithFix(); + gps_->time.minute(); return output; } diff --git a/src/modules/lap_counter/lap_counter.cpp b/src/modules/lap_counter/lap_counter.cpp index f9ce886..f418f84 100644 --- a/src/modules/lap_counter/lap_counter.cpp +++ b/src/modules/lap_counter/lap_counter.cpp @@ -4,6 +4,9 @@ #include "lap_counter.h" #include "base/router.h" #include "data/general_store.h" +#include "data/gps_store.h" +#include "data/track_store.h" +#include "custom_types.h" int LapCounter::push(const Task &task) { return queue_.push(task); } @@ -11,4 +14,62 @@ LapCounter::LapCounter() : logger_(nullptr) {}; LapCounter::LapCounter(SystemLogger *logger) : logger_(logger) {}; -LapCounter::~LapCounter() {} \ No newline at end of file +LapCounter::~LapCounter() {} + +int LapCounter::init() { + counting_ = false; + count_ = false; +} + +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_++; + + last_trigger_time_ = time_cs; + + lastLapTimeGlobalWrite(lap_time); + + router::send(module::Lcd, task::DisplayMsgLapCounterLapTime, 1000); + } + + break; + } + + default: + break; + } + } + } + + return 0; +} \ No newline at end of file diff --git a/src/modules/lap_counter/lap_counter.h b/src/modules/lap_counter/lap_counter.h index 9fd37ac..347c864 100644 --- a/src/modules/lap_counter/lap_counter.h +++ b/src/modules/lap_counter/lap_counter.h @@ -6,11 +6,19 @@ #include "base/ring_buffer.h" #include "custom_types.h" #include "modules/logger/system_logger.h" +#include class LapCounter : public ModuleBase { private: SystemLogger *logger_; RingBuffer 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; + public: int push(const Task &task) override; diff --git a/src/modules/lcd/lcd.cpp b/src/modules/lcd/lcd.cpp index b754c36..c2c6cc8 100644 --- a/src/modules/lcd/lcd.cpp +++ b/src/modules/lcd/lcd.cpp @@ -4,10 +4,10 @@ #include "lcd.h" +#include "data/general_store.h" +#include "modules/gps/gps.h" #include #include -#include "modules/gps/gps.h" -#include "data/general_store.h" #define MOD "modules/lcd/lcd.h" @@ -60,20 +60,23 @@ void Lcd::print(int i, int base) { bool Lcd::isMessageTask(task::Type type) { switch (type) { - case task::DisplayMsgGpsFix: - case task::DisplayMsgTrackDetectOk: - case task::DisplayMsgConfigNoTracks: - case task::DisplayMsgBatteryLow: - case task::DisplayMsgEngineTempLow: - case task::DisplayMsgEngineTempHigh: - return true; + case task::DisplayMsgGpsFix: + case task::DisplayMsgTrackDetectOk: + case task::DisplayMsgConfigNoTracks: + case task::DisplayMsgBatteryLow: + case task::DisplayMsgEngineTempLow: + case task::DisplayMsgEngineTempHigh: + case task::DisplayMsgLapCounterLapTime: + case task::DisplayMsgLapCounterStart: + return true; - default: - return false; + default: + return false; } } -void Lcd::activateMessage(screen::LcdScreen msg_screen, unsigned long duration_ms) { +void Lcd::activateMessage(screen::LcdScreen msg_screen, + unsigned long duration_ms) { if (duration_ms == 0) { duration_ms = frame_duration_; } @@ -149,19 +152,19 @@ int Lcd::renderGpsDebug() { int Lcd::renderDriverPrimary() { this->clear(); - + GpsData gps_data; gpsGlobalRead(gps_data); - + float vbat; vbatGlobalRead(vbat); float teng; tengGlobalRead(teng); - + int line_trigger; gpsTriggerGlobalRead(line_trigger); - display_->setCursor(0,0); + display_->setCursor(0, 0); this->print("GPS:"); if (gps_data.num_fix_ != 0) { this->print("Y"); @@ -169,14 +172,13 @@ int Lcd::renderDriverPrimary() { this->print("N"); } - display_->setCursor(7,0); + display_->setCursor(7, 0); this->print("TRIG:"); - switch (line_trigger) - { + switch (line_trigger) { case 0: this->print("I"); break; - + case 1: this->print("A"); break; @@ -189,23 +191,23 @@ int Lcd::renderDriverPrimary() { this->print("NULL"); break; } - - display_->setCursor(0,2); + + display_->setCursor(0, 2); this->print("SPD:"); if (gps_data.speed_.valid_) { this->print(gps_data.speed_.value_, 1); } else { this->print("NA"); } - - display_->setCursor(0,3); + + display_->setCursor(0, 3); this->print("V:"); this->print(vbat, 1); - display_->setCursor(10,3); + display_->setCursor(10, 3); this->print("T:"); this->print(teng, 1); - + return 0; } @@ -278,29 +280,60 @@ int Lcd::renderMsgEngineTempHigh() { return 0; } -int Lcd::push(const Task &task) { - return queue_.push(task); +int Lcd::renderMsgLapCounterStart() { + this->clear(); + display_->setCursor(5, 1); + this->print("LAP COUNTER"); + display_->setCursor(6, 2); + this->print("STARTED"); + return 0; } +int Lcd::renderMsgLapCounterLapTime() { + + 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); + + return 0; +} + +int Lcd::push(const Task &task) { return queue_.push(task); } + Lcd::Lcd() - : display_cleared_(false), - logger_(nullptr), - screen_(screen::Blank), - data_screen_(screen::Blank), - message_screen_(screen::Blank), - last_render_(0), - frame_duration_(2000) { + : display_cleared_(false), logger_(nullptr), screen_(screen::Blank), + data_screen_(screen::Blank), message_screen_(screen::Blank), + last_render_(0), frame_duration_(2000) { display_ = new LiquidCrystal_I2C(0x27, 20, 4); } Lcd::Lcd(SystemLogger *logger) - : display_cleared_(false), - logger_(logger), - screen_(screen::Blank), - data_screen_(screen::Blank), - message_screen_(screen::Blank), - last_render_(0), - frame_duration_(2000) { + : display_cleared_(false), logger_(logger), screen_(screen::Blank), + data_screen_(screen::Blank), message_screen_(screen::Blank), + last_render_(0), frame_duration_(2000) { display_ = new LiquidCrystal_I2C(0x27, 20, 4); } @@ -447,48 +480,55 @@ int Lcd::loop(unsigned long timeout_ms) { } switch (next_task.type_) { - case task::DisplayGpsDebug: - data_screen_ = screen::GpsDebug; - if (!message_active_) { - screen_ = data_screen_; - force_render_ = true; - } - break; + case task::DisplayGpsDebug: + data_screen_ = screen::GpsDebug; + if (!message_active_) { + screen_ = data_screen_; + force_render_ = true; + } + break; - case task::DisplayDriverPrimary: - data_screen_ = screen::DriverPrimary; - if (!message_active_) { - screen_ = data_screen_; - force_render_ = true; - } - break; + case task::DisplayDriverPrimary: + data_screen_ = screen::DriverPrimary; + if (!message_active_) { + screen_ = data_screen_; + force_render_ = true; + } + break; - case task::DisplayMsgGpsFix: - activateMessage(screen::MsgGpsFix, next_task.data_); - break; + case task::DisplayMsgGpsFix: + activateMessage(screen::MsgGpsFix, next_task.data_); + break; - case task::DisplayMsgTrackDetectOk: - activateMessage(screen::MsgTrackDetectOk, next_task.data_); - break; + case task::DisplayMsgTrackDetectOk: + activateMessage(screen::MsgTrackDetectOk, next_task.data_); + break; - case task::DisplayMsgConfigNoTracks: - activateMessage(screen::MsgConfigNoTracks, next_task.data_); - break; - - case task::DisplayMsgBatteryLow: - activateMessage(screen::MsgBatteryLow, next_task.data_); - break; - - case task::DisplayMsgEngineTempLow: - activateMessage(screen::MsgEngineTempLow, next_task.data_); - break; - - case task::DisplayMsgEngineTempHigh: - activateMessage(screen::MsgEngineTempHigh, next_task.data_); - break; + case task::DisplayMsgConfigNoTracks: + activateMessage(screen::MsgConfigNoTracks, next_task.data_); + break; - default: - break; + case task::DisplayMsgBatteryLow: + activateMessage(screen::MsgBatteryLow, next_task.data_); + break; + + case task::DisplayMsgEngineTempLow: + activateMessage(screen::MsgEngineTempLow, next_task.data_); + break; + + case task::DisplayMsgEngineTempHigh: + activateMessage(screen::MsgEngineTempHigh, next_task.data_); + break; + + case task::DisplayMsgLapCounterStart: + activateMessage(screen::MsgLapCounterStart, next_task.data_); + break; + + case task::DisplayMsgLapCounterLapTime: + activateMessage(screen::MsgLapCounterLapTime, next_task.data_); + + default: + break; } now = millis(); @@ -513,48 +553,56 @@ int Lcd::loop(unsigned long timeout_ms) { } switch (screen_) { - case screen::Blank: - this->clear(); - break; + case screen::Blank: + this->clear(); + break; - case screen::GpsDebug: - this->renderGpsDebug(); - break; + case screen::GpsDebug: + this->renderGpsDebug(); + break; - case screen::DriverPrimary: - this->renderDriverPrimary(); - break; + case screen::DriverPrimary: + this->renderDriverPrimary(); + break; - case screen::MsgGpsFix: - this->renderMsgGpsFix(); - break; + case screen::MsgGpsFix: + this->renderMsgGpsFix(); + break; - case screen::MsgGpsTrigger: - this->renderMsgGpsTrigger(); - break; + case screen::MsgGpsTrigger: + this->renderMsgGpsTrigger(); + break; - case screen::MsgTrackDetectOk: - this->renderMsgTrackDetectOk(); - break; + case screen::MsgTrackDetectOk: + this->renderMsgTrackDetectOk(); + break; - case screen::MsgConfigNoTracks: - this->renderMsgConfigNoTracks(); - break; - - case screen::MsgBatteryLow: - this->renderMsgBatteryLow(); - break; - - case screen::MsgEngineTempLow: - this->renderMsgEngineTempLow(); - break; - - case screen::MsgEngineTempHigh: - this->renderMsgEngineTempHigh(); - break; + case screen::MsgConfigNoTracks: + this->renderMsgConfigNoTracks(); + break; - default: - break; + case screen::MsgBatteryLow: + this->renderMsgBatteryLow(); + break; + + case screen::MsgEngineTempLow: + this->renderMsgEngineTempLow(); + break; + + case screen::MsgEngineTempHigh: + this->renderMsgEngineTempHigh(); + break; + + case screen::MsgLapCounterStart: + this->renderMsgLapCounterStart(); + break; + + case screen::MsgLapCounterLapTime: + this->renderMsgLapCounterLapTime(); + break; + + default: + break; } last_render_ = now; diff --git a/src/modules/lcd/lcd.h b/src/modules/lcd/lcd.h index 769f662..7617362 100644 --- a/src/modules/lcd/lcd.h +++ b/src/modules/lcd/lcd.h @@ -29,6 +29,8 @@ enum LcdScreen : uint8_t { MsgBatteryLow, MsgEngineTempLow, MsgEngineTempHigh, + MsgLapCounterStart, + MsgLapCounterLapTime, }; } // namespace screen @@ -74,6 +76,8 @@ private: int renderMsgBatteryLow(); int renderMsgEngineTempLow(); int renderMsgEngineTempHigh(); + int renderMsgLapCounterStart(); + int renderMsgLapCounterLapTime(); public: int push(const Task &task) override;