fix speedometer buffer

lib/Speedometer/speedometer.cpp

@@ -47,12 +47,12 @@ void Speedometer::runSpeedometer() {
     uint16_t elapsedTime = time - this->lastMillisCalc;
     this->lastMillisCalc = time;
 
-    this->addValToBuf(this->pulseCounter->getValue());
+    int16_t pulse = this->pulseCounter->getValue();
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
     
-    double n = (double)this->calcAverage() / this->steps;   // Wheel revolutions in absolute time
+    double n = (double)pulse / this->steps;   // Wheel revolutions in absolute time
     double u = n / ((double)elapsedTime / 1000);           // Wheel revolutions per second
     double ms = u * (diameter * PI);                      // Speed in m/s
 
@@ -70,12 +70,7 @@ void Speedometer::runSpeedometer() {
             break;
     }
 
-    // if (this->speed)
-    //     this->printCounter++;
-    // if (this->printCounter > 10) {
-    //     this->printCounter = 0;
-    //     std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
-    // }
+    this->addValToBuf(static_cast<int16_t>(this->speed * CONVERSION_FACTOR));
 }
 
 void Speedometer::setDirection(Direction dir) {
@@ -96,6 +91,11 @@ void Speedometer::setEncFilter(uint16_t val) {
     this->pulseCounter->setFilterValue(val);
 }
 
+double Speedometer::getAvgSpeed() const {
+    int16_t avg = this->calcAverage();
+    return (float)avg / CONVERSION_FACTOR;
+}
+
 void Speedometer::calibrationMeasurementStart() {
     std::cout << "Start" << std::endl;
     this->calibrationRunning = true;
@@ -150,7 +150,7 @@ void Speedometer::updateAvgBufSize() {
     initAvgBuf();
 }
 
-int16_t Speedometer::calcAverage() {
+int16_t Speedometer::calcAverage() const {
     int16_t sum = 0;
     for (int i = 0; i < this->bufSize; i++)
         sum += this->buf[i];