Implement median filter (#697)

Add median filter to sensors component

esphome/components/sensor/__init__.py

@@ -61,6 +61,7 @@ SensorPublishAction = sensor_ns.class_('SensorPublishAction', automation.Action)
 
 # Filters
 Filter = sensor_ns.class_('Filter')
+MedianFilter = sensor_ns.class_('MedianFilter', Filter)
 SlidingWindowMovingAverageFilter = sensor_ns.class_('SlidingWindowMovingAverageFilter', Filter)
 ExponentialMovingAverageFilter = sensor_ns.class_('ExponentialMovingAverageFilter', Filter)
 LambdaFilter = sensor_ns.class_('LambdaFilter', Filter)
@@ -127,6 +128,19 @@ def filter_out_filter_to_code(config, filter_id):
     yield cg.new_Pvariable(filter_id, config)
 
 
+MEDIAN_SCHEMA = cv.All(cv.Schema({
+    cv.Optional(CONF_WINDOW_SIZE, default=5): cv.positive_not_null_int,
+    cv.Optional(CONF_SEND_EVERY, default=5): cv.positive_not_null_int,
+    cv.Optional(CONF_SEND_FIRST_AT, default=1): cv.positive_not_null_int,
+}), validate_send_first_at)
+
+
+@FILTER_REGISTRY.register('median', MedianFilter, MEDIAN_SCHEMA)
+def median_filter_to_code(config, filter_id):
+    yield cg.new_Pvariable(filter_id, config[CONF_WINDOW_SIZE], config[CONF_SEND_EVERY],
+                           config[CONF_SEND_FIRST_AT])
+
+
 SLIDING_AVERAGE_SCHEMA = cv.All(cv.Schema({
     cv.Optional(CONF_WINDOW_SIZE, default=15): cv.positive_not_null_int,
     cv.Optional(CONF_SEND_EVERY, default=15): cv.positive_not_null_int,

esphome/components/sensor/filter.cpp

@@ -39,6 +39,44 @@ uint32_t Filter::calculate_remaining_interval(uint32_t input) {
   }
 }
 
+// MedianFilter
+MedianFilter::MedianFilter(size_t window_size, size_t send_every, size_t send_first_at)
+    : send_every_(send_every), send_at_(send_every - send_first_at), window_size_(window_size) {}
+void MedianFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
+void MedianFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
+optional<float> MedianFilter::new_value(float value) {
+  if (!isnan(value)) {
+    while (this->queue_.size() >= this->window_size_) {
+      this->queue_.pop_front();
+    }
+    this->queue_.push_back(value);
+    ESP_LOGVV(TAG, "MedianFilter(%p)::new_value(%f)", this, value);
+  }
+
+  if (++this->send_at_ >= this->send_every_) {
+    this->send_at_ = 0;
+
+    float median = 0.0f;
+    if (!this->queue_.empty()) {
+      std::deque<float> median_queue = this->queue_;
+      sort(median_queue.begin(), median_queue.end());
+
+      size_t queue_size = median_queue.size();
+      if (queue_size % 2) {
+        median = median_queue[queue_size / 2];
+      } else {
+        median = (median_queue[queue_size / 2] + median_queue[(queue_size / 2) - 1]) / 2.0f;
+      }
+    }
+
+    ESP_LOGVV(TAG, "MedianFilter(%p)::new_value(%f) SENDING", this, median);
+    return median;
+  }
+  return {};
+}
+
+uint32_t MedianFilter::expected_interval(uint32_t input) { return input * this->send_every_; }
+
 // SlidingWindowMovingAverageFilter
 SlidingWindowMovingAverageFilter::SlidingWindowMovingAverageFilter(size_t window_size, size_t send_every,
                                                                    size_t send_first_at)

esphome/components/sensor/filter.h

@@ -46,6 +46,36 @@ class Filter {
   Sensor *parent_{nullptr};
 };
 
+/** Simple median filter.
+ *
+ * Takes the median of the last <send_every> values and pushes it out every <send_every>.
+ */
+class MedianFilter : public Filter {
+ public:
+  /** Construct a MedianFilter.
+   *
+   * @param window_size The number of values that should be used in median calculation.
+   * @param send_every After how many sensor values should a new one be pushed out.
+   * @param send_first_at After how many values to forward the very first value. Defaults to the first value
+   *   on startup being published on the first *raw* value, so with no filter applied. Must be less than or equal to
+   *   send_every.
+   */
+  explicit MedianFilter(size_t window_size, size_t send_every, size_t send_first_at);
+
+  optional<float> new_value(float value) override;
+
+  void set_send_every(size_t send_every);
+  void set_window_size(size_t window_size);
+
+  uint32_t expected_interval(uint32_t input) override;
+
+ protected:
+  std::deque<float> queue_;
+  size_t send_every_;
+  size_t send_at_;
+  size_t window_size_;
+};
+
 /** Simple sliding window moving average filter.
  *
  * Essentially just takes takes the average of the last window_size values and pushes them out

tests/test1.yaml

@@ -191,6 +191,10 @@ sensor:
         - 100.0 -> 102.5
       - filter_out: 42.0
       - filter_out: nan
+      - median:
+          window_size: 5
+          send_every: 5
+          send_first_at: 3
       - sliding_window_moving_average:
           window_size: 15
           send_every: 15