From 4e04ae9a3f9179cbb3eb5a80930689ed0514bc6c Mon Sep 17 00:00:00 2001 From: Hector van der Aa Date: Fri, 3 Apr 2026 17:47:21 +0200 Subject: [PATCH] Speed average and display --- src/base/task.h | 1 + src/data/general_store.cpp | 5 ++++ src/data/general_store.h | 4 ++++ src/modules/gps/gps.cpp | 1 + src/modules/lap_counter/lap_counter.cpp | 32 ++++++++++++++++++++++++- src/modules/lap_counter/lap_counter.h | 10 ++++++++ src/modules/lcd/lcd.cpp | 11 +++++++++ 7 files changed, 63 insertions(+), 1 deletion(-) diff --git a/src/base/task.h b/src/base/task.h index fe12644..096c2c4 100644 --- a/src/base/task.h +++ b/src/base/task.h @@ -49,6 +49,7 @@ enum Type : uint8_t { AllConfigUpdated, AllTrackLoaded, AllStartLineTriggered, + AllGpsFixOk, }; } // namespace task diff --git a/src/data/general_store.cpp b/src/data/general_store.cpp index 11e4f30..7af7cf7 100644 --- a/src/data/general_store.cpp +++ b/src/data/general_store.cpp @@ -8,6 +8,7 @@ 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; void vbatGlobalRead(float &out) { out = vbat_global; } @@ -28,3 +29,7 @@ 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; } diff --git a/src/data/general_store.h b/src/data/general_store.h index 803c7a8..285b769 100644 --- a/src/data/general_store.h +++ b/src/data/general_store.h @@ -9,6 +9,7 @@ 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; void vbatGlobalRead(float& out); void vbatGlobalWrite(const float& in); @@ -24,3 +25,6 @@ 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); diff --git a/src/modules/gps/gps.cpp b/src/modules/gps/gps.cpp index e3c473a..1646372 100644 --- a/src/modules/gps/gps.cpp +++ b/src/modules/gps/gps.cpp @@ -43,6 +43,7 @@ int Gps::loop(unsigned long timeout_ms) { if (last_fix_value_ == 0 && current_fix_value > 0) { router::send(module::Lcd, task::DisplayMsgGpsFix, 2000); router::send(module::Config, task::ConfigTrackDetect); + router::send(module::All, task::AllGpsFixOk); } last_fix_value_ = current_fix_value; } diff --git a/src/modules/lap_counter/lap_counter.cpp b/src/modules/lap_counter/lap_counter.cpp index e0b8b56..0264055 100644 --- a/src/modules/lap_counter/lap_counter.cpp +++ b/src/modules/lap_counter/lap_counter.cpp @@ -3,10 +3,10 @@ // SPDX-License-Identifier: GPL-3.0-or-later #include "lap_counter.h" #include "base/router.h" +#include "custom_types.h" #include "data/general_store.h" #include "data/gps_store.h" #include "data/track_store.h" -#include "custom_types.h" int LapCounter::push(const Task &task) { return queue_.push(task); } @@ -67,11 +67,41 @@ int LapCounter::loop() { 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; + + continuous_time_sum_ += dt; + if (last_average_time_ == 0) { + continuous_speed_sum_ += gps.speed_.value_ * dt; + } else { + continuous_speed_sum_ += + (gps.speed_.value_ + previous_speed_) * 0.5f * dt; + } + previous_speed_ = gps.speed_.value_; + + speedAvgGlobalWrite(continuous_speed_sum_ / continuous_time_sum_); + + last_average_time_ = now; + } + 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 347c864..c787b8b 100644 --- a/src/modules/lap_counter/lap_counter.h +++ b/src/modules/lap_counter/lap_counter.h @@ -18,6 +18,16 @@ private: 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: diff --git a/src/modules/lcd/lcd.cpp b/src/modules/lcd/lcd.cpp index 79a8cf0..b111510 100644 --- a/src/modules/lcd/lcd.cpp +++ b/src/modules/lcd/lcd.cpp @@ -166,6 +166,9 @@ int Lcd::renderDriverPrimary() { uint16_t num_laps; lapCountGlobalRead(num_laps); + + float average_speed; + speedAvgGlobalRead(average_speed); if (!base_rendered_) { this->clear(); @@ -178,6 +181,9 @@ int Lcd::renderDriverPrimary() { display_->setCursor(0, 2); this->print("SPD:"); + display_->setCursor(10, 2); + this->print("AVG:"); + display_->setCursor(0, 3); this->print("V:"); @@ -203,6 +209,11 @@ int Lcd::renderDriverPrimary() { 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);