Fix battery temperature reporting invalid values in extreme temperatures

husarion_ugv_utils/include/husarion_ugv_utils/moving_average.hpp

@@ -25,40 +25,43 @@ class MovingAverage
 {
 public:
   MovingAverage(const std::size_t window_size = 5, const T initial_value = T(0))
-  : window_size_(window_size), initial_value_(initial_value), sum_(T(0))
+  : window_size_(window_size), initial_value_(initial_value)
   {
+    if (window_size_ == 0) {
+      throw std::invalid_argument("Window size must be greater than 0");
+    }
   }
 
   void Roll(const T value)
   {
-    values_.push_back(value);
-    sum_ += value;
+    buffer_.push_back(value);
 
-    if (values_.size() > window_size_) {
-      sum_ -= values_.front();
-      values_.pop_front();
+    if (buffer_.size() > window_size_) {
+      buffer_.pop_front();
     }
   }
 
-  void Reset()
-  {
-    values_.erase(values_.begin(), values_.end());
-    sum_ = T(0);
-  }
+  void Reset() { buffer_.erase(buffer_.begin(), buffer_.end()); }
 
   T GetAverage() const
   {
-    if (values_.size() == 0) {
+    if (buffer_.size() == 0) {
       return initial_value_;
     }
-    return sum_ / static_cast<T>(values_.size());
+
+    T sum = T(0);
+
+    for (const auto & value : buffer_) {
+      sum += value / static_cast<T>(buffer_.size());
+    }
+
+    return sum;
   }
 
 private:
   const std::size_t window_size_;
-  std::deque<T> values_;
+  std::deque<T> buffer_;
   const T initial_value_;
-  T sum_;
 };
 
 }  // namespace husarion_ugv_utils

husarion_ugv_utils/test/test_moving_average.cpp

@@ -59,15 +59,15 @@ TEST(TestMovingAverage, TestHighOverload)
   const std::size_t window_len = 1000;
   husarion_ugv_utils::MovingAverage<double> ma(window_len);
 
-  double sum = 0.0;
+  double avg = 0.0;
   for (std::size_t i = 1; i <= window_len * 10; i++) {
-    sum += double(i);
+    avg += double(i) / double(window_len);
     ma.Roll(double(i));
 
     // test every 1000 rolls expected average
     if (i % window_len == 0) {
-      EXPECT_EQ(sum / double(window_len), ma.GetAverage());
-      sum = 0.0;
+      ASSERT_NEAR(avg, ma.GetAverage(), std::numeric_limits<double>::epsilon());
+      avg = 0.0;
     }
   }
 }
@@ -107,6 +107,25 @@ TEST(TestMovingAverage, TestResetToInitialValue)
   EXPECT_EQ(7.0, ma.GetAverage());
 }
 
+TEST(TestMovingAverage, TestInfInjectionHandling)
+{
+  husarion_ugv_utils::MovingAverage<double> ma(4);
+  ma.Roll(1.0);
+  ma.Roll(2.0);
+  ma.Roll(3.0);
+  ma.Roll(4.0);
+  EXPECT_EQ(2.5, ma.GetAverage());
+
+  ma.Roll(std::numeric_limits<double>::infinity());
+  EXPECT_EQ(std::numeric_limits<double>::infinity(), ma.GetAverage());
+
+  ma.Roll(1.0);
+  ma.Roll(2.0);
+  ma.Roll(3.0);
+  ma.Roll(4.0);
+  EXPECT_EQ(2.5, ma.GetAverage());
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);