filter.cpp

Go to the documentation of this file.

#include "filter.h"
#include "esphome/core/hal.h"
#include "esphome/core/log.h"
#include "sensor.h"
#include <cmath>

namespace esphome {
namespace sensor {

static const char *const TAG = "sensor.filter";

// Filter
void Filter::input(float value) {
  ESP_LOGVV(TAG, "Filter(%p)::input(%f)", this, value);
  optional<float> out = this->new_value(value);
  if (out.has_value())
    this->output(*out);
}
void Filter::output(float value) {
  if (this->next_ == nullptr) {
    ESP_LOGVV(TAG, "Filter(%p)::output(%f) -> SENSOR", this, value);
    this->parent_->internal_send_state_to_frontend(value);
  } else {
    ESP_LOGVV(TAG, "Filter(%p)::output(%f) -> %p", this, value, this->next_);
    this->next_->input(value);
  }
}
void Filter::initialize(Sensor *parent, Filter *next) {
  ESP_LOGVV(TAG, "Filter(%p)::initialize(parent=%p next=%p)", this, parent, next);
  this->parent_ = parent;
  this->next_ = next;
}

// 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 (!std::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 {};
}

// QuantileFilter
QuantileFilter::QuantileFilter(size_t window_size, size_t send_every, size_t send_first_at, float quantile)
    : send_every_(send_every), send_at_(send_every - send_first_at), window_size_(window_size), quantile_(quantile) {}
void QuantileFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void QuantileFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
void QuantileFilter::set_quantile(float quantile) { this->quantile_ = quantile; }
optional<float> QuantileFilter::new_value(float value) {
  if (!std::isnan(value)) {
    while (this->queue_.size() >= this->window_size_) {
      this->queue_.pop_front();
    }
    this->queue_.push_back(value);
    ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f), quantile:%f", this, value, this->quantile_);
  }

  if (++this->send_at_ >= this->send_every_) {
    this->send_at_ = 0;

    float result = 0.0f;
    if (!this->queue_.empty()) {
      std::deque<float> quantile_queue = this->queue_;
      sort(quantile_queue.begin(), quantile_queue.end());

      size_t queue_size = quantile_queue.size();
      size_t position = ceilf(queue_size * this->quantile_) - 1;
      ESP_LOGVV(TAG, "QuantileFilter(%p)::position: %d/%d", this, position, queue_size);
      result = quantile_queue[position];
    }

    ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f) SENDING", this, result);
    return result;
  }
  return {};
}

// MinFilter
MinFilter::MinFilter(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 MinFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void MinFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> MinFilter::new_value(float value) {
  if (!std::isnan(value)) {
    while (this->queue_.size() >= this->window_size_) {
      this->queue_.pop_front();
    }
    this->queue_.push_back(value);
    ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f)", this, value);
  }

  if (++this->send_at_ >= this->send_every_) {
    this->send_at_ = 0;

    float min = 0.0f;
    if (!this->queue_.empty()) {
      std::deque<float>::iterator it = std::min_element(queue_.begin(), queue_.end());
      min = *it;
    }

    ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f) SENDING", this, min);
    return min;
  }
  return {};
}

// MaxFilter
MaxFilter::MaxFilter(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 MaxFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void MaxFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> MaxFilter::new_value(float value) {
  if (!std::isnan(value)) {
    while (this->queue_.size() >= this->window_size_) {
      this->queue_.pop_front();
    }
    this->queue_.push_back(value);
    ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f)", this, value);
  }

  if (++this->send_at_ >= this->send_every_) {
    this->send_at_ = 0;

    float max = 0.0f;
    if (!this->queue_.empty()) {
      std::deque<float>::iterator it = std::max_element(queue_.begin(), queue_.end());
      max = *it;
    }

    ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f) SENDING", this, max);
    return max;
  }
  return {};
}

// SlidingWindowMovingAverageFilter
SlidingWindowMovingAverageFilter::SlidingWindowMovingAverageFilter(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 SlidingWindowMovingAverageFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void SlidingWindowMovingAverageFilter::set_window_size(size_t window_size) { this->window_size_ = window_size; }
optional<float> SlidingWindowMovingAverageFilter::new_value(float value) {
  if (!std::isnan(value)) {
    if (this->queue_.size() == this->window_size_) {
      this->sum_ -= this->queue_[0];
      this->queue_.pop_front();
    }
    this->queue_.push_back(value);
    this->sum_ += value;
  }
  float average;
  if (this->queue_.empty()) {
    average = 0.0f;
  } else {
    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_ == 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;
  }
  return {};
}

// ExponentialMovingAverageFilter
ExponentialMovingAverageFilter::ExponentialMovingAverageFilter(float alpha, size_t send_every, size_t send_first_at)
    : send_every_(send_every), send_at_(send_every - send_first_at), alpha_(alpha) {}
optional<float> ExponentialMovingAverageFilter::new_value(float value) {
  if (!std::isnan(value)) {
    if (this->first_value_) {
      this->accumulator_ = value;
    } else {
      this->accumulator_ = (this->alpha_ * value) + (1.0f - this->alpha_) * this->accumulator_;
    }
    this->first_value_ = false;
  }

  float average = this->accumulator_;
  ESP_LOGVV(TAG, "ExponentialMovingAverageFilter(%p)::new_value(%f) -> %f", this, value, average);

  if (++this->send_at_ >= this->send_every_) {
    ESP_LOGVV(TAG, "ExponentialMovingAverageFilter(%p)::new_value(%f) SENDING", this, value);
    this->send_at_ = 0;
    return average;
  }
  return {};
}
void ExponentialMovingAverageFilter::set_send_every(size_t send_every) { this->send_every_ = send_every; }
void ExponentialMovingAverageFilter::set_alpha(float alpha) { this->alpha_ = alpha; }

// ThrottleAverageFilter
ThrottleAverageFilter::ThrottleAverageFilter(uint32_t time_period) : time_period_(time_period) {}

optional<float> ThrottleAverageFilter::new_value(float value) {
  ESP_LOGVV(TAG, "ThrottleAverageFilter(%p)::new_value(value=%f)", this, value);
  if (!std::isnan(value)) {
    this->sum_ += value;
    this->n_++;
  }
  return {};
}
void ThrottleAverageFilter::setup() {
  this->set_interval("throttle_average", this->time_period_, [this]() {
    ESP_LOGVV(TAG, "ThrottleAverageFilter(%p)::interval(sum=%f, n=%i)", this, this->sum_, this->n_);
    if (this->n_ == 0) {
      this->output(NAN);
    } else {
      this->output(this->sum_ / this->n_);
      this->sum_ = 0.0f;
      this->n_ = 0;
    }
  });
}
float ThrottleAverageFilter::get_setup_priority() const { return setup_priority::HARDWARE; }

// LambdaFilter
LambdaFilter::LambdaFilter(lambda_filter_t lambda_filter) : lambda_filter_(std::move(lambda_filter)) {}
const lambda_filter_t &LambdaFilter::get_lambda_filter() const { return this->lambda_filter_; }
void LambdaFilter::set_lambda_filter(const lambda_filter_t &lambda_filter) { this->lambda_filter_ = lambda_filter; }

optional<float> LambdaFilter::new_value(float value) {
  auto it = this->lambda_filter_(value);
  ESP_LOGVV(TAG, "LambdaFilter(%p)::new_value(%f) -> %f", this, value, it.value_or(INFINITY));
  return it;
}

// OffsetFilter
OffsetFilter::OffsetFilter(float offset) : offset_(offset) {}

optional<float> OffsetFilter::new_value(float value) { return value + this->offset_; }

// MultiplyFilter
MultiplyFilter::MultiplyFilter(float multiplier) : multiplier_(multiplier) {}

optional<float> MultiplyFilter::new_value(float value) { return value * this->multiplier_; }

// FilterOutValueFilter
FilterOutValueFilter::FilterOutValueFilter(float value_to_filter_out) : value_to_filter_out_(value_to_filter_out) {}

optional<float> FilterOutValueFilter::new_value(float value) {
  if (std::isnan(this->value_to_filter_out_)) {
    if (std::isnan(value)) {
      return {};
    } else {
      return value;
    }
  } else {
    int8_t accuracy = this->parent_->get_accuracy_decimals();
    float accuracy_mult = powf(10.0f, accuracy);
    float rounded_filter_out = roundf(accuracy_mult * this->value_to_filter_out_);
    float rounded_value = roundf(accuracy_mult * value);
    if (rounded_filter_out == rounded_value) {
      return {};
    } else {
      return value;
    }
  }
}

// ThrottleFilter
ThrottleFilter::ThrottleFilter(uint32_t min_time_between_inputs) : min_time_between_inputs_(min_time_between_inputs) {}
optional<float> ThrottleFilter::new_value(float value) {
  const uint32_t now = millis();
  if (this->last_input_ == 0 || now - this->last_input_ >= min_time_between_inputs_) {
    this->last_input_ = now;
    return value;
  }
  return {};
}

// DeltaFilter
DeltaFilter::DeltaFilter(float min_delta) : min_delta_(min_delta), last_value_(NAN) {}
optional<float> DeltaFilter::new_value(float value) {
  if (std::isnan(value))
    return {};
  if (std::isnan(this->last_value_)) {
    return this->last_value_ = value;
  }
  if (fabsf(value - this->last_value_) >= this->min_delta_) {
    return this->last_value_ = value;
  }
  return {};
}

// OrFilter
OrFilter::OrFilter(std::vector<Filter *> filters) : filters_(std::move(filters)), phi_(this) {}
OrFilter::PhiNode::PhiNode(OrFilter *or_parent) : or_parent_(or_parent) {}

optional<float> OrFilter::PhiNode::new_value(float value) {
  this->or_parent_->output(value);

  return {};
}
optional<float> OrFilter::new_value(float value) {
  for (Filter *filter : this->filters_)
    filter->input(value);

  return {};
}
void OrFilter::initialize(Sensor *parent, Filter *next) {
  Filter::initialize(parent, next);
  for (Filter *filter : this->filters_) {
    filter->initialize(parent, &this->phi_);
  }
  this->phi_.initialize(parent, nullptr);
}

// DebounceFilter
optional<float> DebounceFilter::new_value(float value) {
  this->set_timeout("debounce", this->time_period_, [this, value]() { this->output(value); });

  return {};
}

DebounceFilter::DebounceFilter(uint32_t time_period) : time_period_(time_period) {}
float DebounceFilter::get_setup_priority() const { return setup_priority::HARDWARE; }

// HeartbeatFilter
HeartbeatFilter::HeartbeatFilter(uint32_t time_period) : time_period_(time_period), last_input_(NAN) {}

optional<float> HeartbeatFilter::new_value(float value) {
  ESP_LOGVV(TAG, "HeartbeatFilter(%p)::new_value(value=%f)", this, value);
  this->last_input_ = value;
  this->has_value_ = true;

  return {};
}
void HeartbeatFilter::setup() {
  this->set_interval("heartbeat", this->time_period_, [this]() {
    ESP_LOGVV(TAG, "HeartbeatFilter(%p)::interval(has_value=%s, last_input=%f)", this, YESNO(this->has_value_),
              this->last_input_);
    if (!this->has_value_)
      return;

    this->output(this->last_input_);
  });
}
float HeartbeatFilter::get_setup_priority() const { return setup_priority::HARDWARE; }

optional<float> CalibrateLinearFilter::new_value(float value) { return value * this->slope_ + this->bias_; }
CalibrateLinearFilter::CalibrateLinearFilter(float slope, float bias) : slope_(slope), bias_(bias) {}

optional<float> CalibratePolynomialFilter::new_value(float value) {
  float res = 0.0f;
  float x = 1.0f;
  for (float coefficient : this->coefficients_) {
    res += x * coefficient;
    x *= value;
  }
  return res;
}

}  // namespace sensor
}  // namespace esphome