mirror of https://github.com/esphome/esphome.git
477 lines
16 KiB
C++
477 lines
16 KiB
C++
#include "filter.h"
|
|
#include <cmath>
|
|
#include "esphome/core/hal.h"
|
|
#include "esphome/core/log.h"
|
|
#include "sensor.h"
|
|
|
|
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) {
|
|
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 = NAN;
|
|
if (!this->queue_.empty()) {
|
|
// Copy queue without NaN values
|
|
std::vector<float> median_queue;
|
|
for (auto v : this->queue_) {
|
|
if (!std::isnan(v)) {
|
|
median_queue.push_back(v);
|
|
}
|
|
}
|
|
|
|
sort(median_queue.begin(), median_queue.end());
|
|
|
|
size_t queue_size = median_queue.size();
|
|
if (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 %f", this, value, median);
|
|
return median;
|
|
}
|
|
return {};
|
|
}
|
|
|
|
// SkipInitialFilter
|
|
SkipInitialFilter::SkipInitialFilter(size_t num_to_ignore) : num_to_ignore_(num_to_ignore) {}
|
|
optional<float> SkipInitialFilter::new_value(float value) {
|
|
if (num_to_ignore_ > 0) {
|
|
num_to_ignore_--;
|
|
ESP_LOGV(TAG, "SkipInitialFilter(%p)::new_value(%f) SKIPPING, %u left", this, value, num_to_ignore_);
|
|
return {};
|
|
}
|
|
|
|
ESP_LOGV(TAG, "SkipInitialFilter(%p)::new_value(%f) SENDING", this, value);
|
|
return value;
|
|
}
|
|
|
|
// 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) {
|
|
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 = NAN;
|
|
if (!this->queue_.empty()) {
|
|
// Copy queue without NaN values
|
|
std::vector<float> quantile_queue;
|
|
for (auto v : this->queue_) {
|
|
if (!std::isnan(v)) {
|
|
quantile_queue.push_back(v);
|
|
}
|
|
}
|
|
|
|
sort(quantile_queue.begin(), quantile_queue.end());
|
|
|
|
size_t queue_size = quantile_queue.size();
|
|
if (queue_size) {
|
|
size_t position = ceilf(queue_size * this->quantile_) - 1;
|
|
ESP_LOGVV(TAG, "QuantileFilter(%p)::position: %d/%d", this, position + 1, queue_size);
|
|
result = quantile_queue[position];
|
|
}
|
|
}
|
|
|
|
ESP_LOGVV(TAG, "QuantileFilter(%p)::new_value(%f) SENDING %f", this, value, 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) {
|
|
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 = NAN;
|
|
for (auto v : this->queue_) {
|
|
if (!std::isnan(v)) {
|
|
min = std::isnan(min) ? v : std::min(min, v);
|
|
}
|
|
}
|
|
|
|
ESP_LOGVV(TAG, "MinFilter(%p)::new_value(%f) SENDING %f", this, value, 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) {
|
|
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 = NAN;
|
|
for (auto v : this->queue_) {
|
|
if (!std::isnan(v)) {
|
|
max = std::isnan(max) ? v : std::max(max, v);
|
|
}
|
|
}
|
|
|
|
ESP_LOGVV(TAG, "MaxFilter(%p)::new_value(%f) SENDING %f", this, value, 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) {
|
|
while (this->queue_.size() >= this->window_size_) {
|
|
this->queue_.pop_front();
|
|
}
|
|
this->queue_.push_back(value);
|
|
ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f)", this, value);
|
|
|
|
if (++this->send_at_ >= this->send_every_) {
|
|
this->send_at_ = 0;
|
|
|
|
float sum = 0;
|
|
size_t valid_count = 0;
|
|
for (auto v : this->queue_) {
|
|
if (!std::isnan(v)) {
|
|
sum += v;
|
|
valid_count++;
|
|
}
|
|
}
|
|
|
|
float average = NAN;
|
|
if (valid_count) {
|
|
average = sum / valid_count;
|
|
}
|
|
|
|
ESP_LOGVV(TAG, "SlidingWindowMovingAverageFilter(%p)::new_value(%f) SENDING %f", this, value, average);
|
|
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;
|
|
this->first_value_ = false;
|
|
} else {
|
|
this->accumulator_ = (this->alpha_ * value) + (1.0f - this->alpha_) * this->accumulator_;
|
|
}
|
|
}
|
|
|
|
const float average = std::isnan(value) ? value : 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 %f", this, value, average);
|
|
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->have_nan_ = true;
|
|
} else {
|
|
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) {
|
|
if (this->have_nan_)
|
|
this->output(NAN);
|
|
} else {
|
|
this->output(this->sum_ / this->n_);
|
|
this->sum_ = 0.0f;
|
|
this->n_ = 0;
|
|
}
|
|
this->have_nan_ = false;
|
|
});
|
|
}
|
|
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 delta, bool percentage_mode)
|
|
: delta_(delta), current_delta_(delta), percentage_mode_(percentage_mode), last_value_(NAN) {}
|
|
optional<float> DeltaFilter::new_value(float value) {
|
|
if (std::isnan(value)) {
|
|
if (std::isnan(this->last_value_)) {
|
|
return {};
|
|
} else {
|
|
if (this->percentage_mode_) {
|
|
this->current_delta_ = fabsf(value * this->delta_);
|
|
}
|
|
return this->last_value_ = value;
|
|
}
|
|
}
|
|
if (std::isnan(this->last_value_) || fabsf(value - this->last_value_) >= this->current_delta_) {
|
|
if (this->percentage_mode_) {
|
|
this->current_delta_ = fabsf(value * this->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) {
|
|
if (!this->or_parent_->has_value_) {
|
|
this->or_parent_->output(value);
|
|
this->or_parent_->has_value_ = true;
|
|
}
|
|
|
|
return {};
|
|
}
|
|
optional<float> OrFilter::new_value(float value) {
|
|
this->has_value_ = false;
|
|
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);
|
|
}
|
|
|
|
// TimeoutFilter
|
|
optional<float> TimeoutFilter::new_value(float value) {
|
|
this->set_timeout("timeout", this->time_period_, [this]() { this->output(this->value_); });
|
|
return value;
|
|
}
|
|
|
|
TimeoutFilter::TimeoutFilter(uint32_t time_period, float new_value) : time_period_(time_period), value_(new_value) {}
|
|
float TimeoutFilter::get_setup_priority() const { return setup_priority::HARDWARE; }
|
|
|
|
// 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) {
|
|
for (std::array<float, 3> f : this->linear_functions_) {
|
|
if (!std::isfinite(f[2]) || value < f[2])
|
|
return (value * f[0]) + f[1];
|
|
}
|
|
return NAN;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
ClampFilter::ClampFilter(float min, float max, bool ignore_out_of_range)
|
|
: min_(min), max_(max), ignore_out_of_range_(ignore_out_of_range) {}
|
|
optional<float> ClampFilter::new_value(float value) {
|
|
if (std::isfinite(value)) {
|
|
if (std::isfinite(this->min_) && value < this->min_) {
|
|
if (this->ignore_out_of_range_) {
|
|
return {};
|
|
} else {
|
|
return this->min_;
|
|
}
|
|
}
|
|
|
|
if (std::isfinite(this->max_) && value > this->max_) {
|
|
if (this->ignore_out_of_range_) {
|
|
return {};
|
|
} else {
|
|
return this->max_;
|
|
}
|
|
}
|
|
}
|
|
return value;
|
|
}
|
|
|
|
RoundFilter::RoundFilter(uint8_t precision) : precision_(precision) {}
|
|
optional<float> RoundFilter::new_value(float value) {
|
|
if (std::isfinite(value)) {
|
|
float accuracy_mult = powf(10.0f, this->precision_);
|
|
return roundf(accuracy_mult * value) / accuracy_mult;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
} // namespace sensor
|
|
} // namespace esphome
|