diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp
index 9cb7976..ff764d2 100644
--- a/lib/Speedometer/speedometer.cpp
+++ b/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];
diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h
index b0108e9..6a66c69 100644
--- a/lib/Speedometer/speedometer.h
+++ b/lib/Speedometer/speedometer.h
@@ -22,14 +22,15 @@
  * @brief The default size of numbers to be taken in account for the average.
  * 
  */
-#define BUFSIZE 10
+#define BUFSIZE 5
+#define CONVERSION_FACTOR
 
 /**
  * @brief Default value for min Millisseconds between each loop
  * @see setDelay(uint8_t val)
  */
 #define DELAY_SPEEDOMETER 30
-#define PI 3.1415926535897932384626433832795
+#define PI 3.141592
 
 /**
  * @brief A class which use a encoder to calc the speed
@@ -126,7 +127,8 @@ class Speedometer {
          * 
          * @return double speed in m/s
          */
-        double getSpeed() { return this->speed; }
+        double getSpeed() const { return this->speed; }
+        double getAvgSpeed() const;
 
         /**
          * @brief Start calibration
@@ -153,7 +155,7 @@ class Speedometer {
         void clearAvgBuf();
         void addValToBuf(int16_t val);
         void updateAvgBufSize();
-        int16_t calcAverage();
+        int16_t calcAverage() const;
 
         Counter* pulseCounter;
         Direction currentDirection = Direction::None;