Speed average and display

src/base/task.h

@@ -49,6 +49,7 @@ enum Type : uint8_t {
     AllConfigUpdated,
     AllTrackLoaded,
     AllStartLineTriggered,
+    AllGpsFixOk,
 };
 
 } // namespace task

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; }

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);

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;
     }

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;
 }

src/modules/lap_counter/lap_counter.h

@@ -20,6 +20,16 @@ private:
   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();

src/modules/lcd/lcd.cpp

@@ -167,6 +167,9 @@ int Lcd::renderDriverPrimary() {
   uint16_t num_laps;
   lapCountGlobalRead(num_laps);
   
+  float average_speed;
+  speedAvgGlobalRead(average_speed);
+
   if (!base_rendered_) {
     this->clear();
     display_->setCursor(0, 0);
@@ -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);