From d8e4f5d56be9526d99d7cc88db85d7d6eee27280 Mon Sep 17 00:00:00 2001
From: Otto Winter <otto@otto-winter.com>
Date: Fri, 9 Apr 2021 10:27:18 +0200
Subject: [PATCH] Sensor Average Filter Fix Floating Pointer Error Accumulating
 (#1624)

---
 esphome/components/sensor/filter.cpp | 18 +++++++++++++-----
 esphome/components/sensor/filter.h   |  2 +-
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/esphome/components/sensor/filter.cpp b/esphome/components/sensor/filter.cpp
index 6dfb11b9c9..57ffe9b482 100644
--- a/esphome/components/sensor/filter.cpp
+++ b/esphome/components/sensor/filter.cpp
@@ -148,10 +148,10 @@ void SlidingWindowMovingAverageFilter::set_window_size(size_t window_size) { thi
 optional<float> SlidingWindowMovingAverageFilter::new_value(float value) {
   if (!isnan(value)) {
     if (this->queue_.size() == this->window_size_) {
-      this->sum_ -= this->queue_.front();
-      this->queue_.pop();
+      this->sum_ -= this->queue_[0];
+      this->queue_.pop_front();
     }
-    this->queue_.push(value);
+    this->queue_.push_back(value);
     this->sum_ += value;
   }
   float average;
@@ -161,8 +161,16 @@ optional<float> SlidingWindowMovingAverageFilter::new_value(float value) {
     average = this->sum_ / this->queue_.size();
   ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f) -> %f", this, value, average);
 
-  if (++this->send_at_ >= this->send_every_) {
-    this->send_at_ = 0;
+  if (++this->send_at_ % this->send_every_ == 0) {
+    if (this->send_at_ >= 10000) {
+      // Recalculate to prevent floating point error accumulating
+      this->sum_ = 0;
+      for (auto v : this->queue_)
+        this->sum_ += v;
+      average = this->sum_ / this->queue_.size();
+      this->send_at_ = 0;
+    }
+
     ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f) SENDING", this, value);
     return average;
   }
diff --git a/esphome/components/sensor/filter.h b/esphome/components/sensor/filter.h
index 651d2a8986..5b06d002fa 100644
--- a/esphome/components/sensor/filter.h
+++ b/esphome/components/sensor/filter.h
@@ -162,7 +162,7 @@ class SlidingWindowMovingAverageFilter : public Filter {
 
  protected:
   float sum_{0.0};
-  std::queue<float> queue_;
+  std::deque<float> queue_;
   size_t send_every_;
   size_t send_at_;
   size_t window_size_;