From 20ed0b16aeca5da637652b772f99e7a7bafa9c0a Mon Sep 17 00:00:00 2001
From: tim-fan <tim.fanselow@gmail.com>
Date: Mon, 19 Apr 2021 18:24:43 +1200
Subject: [PATCH] Use average dt values for velocity calculation (#60)

---
 include/create/create.h | 11 +++++++++++
 src/create.cpp          | 29 ++++++++++++++++++++++++-----
 2 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/include/create/create.h b/include/create/create.h
index deaa4ba..eadec1a 100644
--- a/include/create/create.h
+++ b/include/create/create.h
@@ -37,6 +37,7 @@ POSSIBILITY OF SUCH DAMAGE.
 #include <memory>
 #include <string>
 #include <unistd.h>
+#include <deque>
 
 #include "create/serial_stream.h"
 #include "create/serial_query.h"
@@ -81,6 +82,8 @@ namespace create {
       float totalRightDist;
       bool firstOnData;
       std::chrono::time_point<std::chrono::steady_clock> prevOnDataTime;
+      std::deque<float> dtHistory;
+      uint8_t dtHistoryLength;
 
       Matrix poseCovar;
 
@@ -332,6 +335,14 @@ namespace create {
        */
       bool playSong(const uint8_t& songNumber) const;
 
+      /**
+       * \brief Set dtHistoryLength parameter.
+       * Used to configure the size of the buffer for calculating average time delta (dt).
+       * between onData calls, which in turn is used for velocity calculation.
+       * \param dtHistoryLength number of historical samples to use for calculating average dt.
+       */
+      void setDtHistoryLength(const uint8_t& dtHistoryLength);
+
       /**
        * \return true if a left or right wheeldrop is detected, false otherwise.
        */
diff --git a/src/create.cpp b/src/create.cpp
index e8e359e..a4f966f 100644
--- a/src/create.cpp
+++ b/src/create.cpp
@@ -41,6 +41,7 @@ namespace create {
     poseCovar = Matrix(3, 3, 0.0);
     requestedLeftVel = 0;
     requestedRightVel = 0;
+    dtHistoryLength = 100;
     data = std::shared_ptr<Data>(new Data(model.getVersion()));
     if (model.getVersion() == V_1) {
       serial = std::make_shared<SerialQuery>(data);
@@ -167,8 +168,22 @@ namespace create {
       deltaYaw = wheelDistDiff / model.getAxleLength();
     }
 
-    measuredLeftVel = leftWheelDist / dt;
-    measuredRightVel = rightWheelDist / dt;
+    // determine average dt over window
+    dtHistory.push_front(dt);
+
+    if (dtHistory.size() > dtHistoryLength){
+      dtHistory.pop_back();
+    }
+
+    float dtHistorySum = 0;
+    for (auto it = dtHistory.cbegin(); it != dtHistory.cend(); ++it)
+    {
+      dtHistorySum += *it;
+    }
+    auto dtAverage = dtHistorySum / dtHistory.size();
+
+    measuredLeftVel = leftWheelDist / dtAverage;
+    measuredRightVel = rightWheelDist / dtAverage;
 
     // Moving straight
     if (fabs(wheelDistDiff) < util::EPS) {
@@ -183,10 +198,10 @@ namespace create {
     totalLeftDist += leftWheelDist;
     totalRightDist += rightWheelDist;
 
-    if (fabs(dt) > util::EPS) {
-      vel.x = deltaDist / dt;
+    if (fabs(dtAverage) > util::EPS) {
+      vel.x = deltaDist / dtAverage;
       vel.y = 0.0;
-      vel.yaw = deltaYaw / dt;
+      vel.yaw = deltaYaw / dtAverage;
     } else {
       vel.x = 0.0;
       vel.y = 0.0;
@@ -593,6 +608,10 @@ namespace create {
     return serial->send(cmd, 2);
   }
 
+  void Create::setDtHistoryLength(const uint8_t& dtHistoryLength) {
+    this->dtHistoryLength = dtHistoryLength;
+  }
+
   bool Create::isWheeldrop() const {
     if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
       return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;