diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp index c19322c759..e4f741b934 100644 --- a/libraries/Filter/LowPassFilter.cpp +++ b/libraries/Filter/LowPassFilter.cpp @@ -9,6 +9,7 @@ #pragma GCC optimize("O2") #endif #include "LowPassFilter.h" +#include //////////////////////////////////////////////////////////////////////////////////////////// // DigitalLPF @@ -23,10 +24,18 @@ DigitalLPF::DigitalLPF() { // add a new raw value to the filter, retrieve the filtered result template T DigitalLPF::apply(const T &sample, float cutoff_freq, float dt) { - if (cutoff_freq <= 0.0f || dt <= 0.0f) { + if (is_negative(cutoff_freq) || is_negative(dt)) { + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); _output = sample; return _output; } + if (is_zero(cutoff_freq)) { + _output = sample; + return _output; + } + if (is_zero(dt)) { + return _output; + } float rc = 1.0f/(M_2PI*cutoff_freq); alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); _output += (sample - _output) * alpha; diff --git a/libraries/Filter/SlewLimiter.cpp b/libraries/Filter/SlewLimiter.cpp index 46005fa382..0f95760258 100644 --- a/libraries/Filter/SlewLimiter.cpp +++ b/libraries/Filter/SlewLimiter.cpp @@ -40,6 +40,10 @@ SlewLimiter::SlewLimiter(const float &_slew_rate_max, const float &_slew_rate_ta */ float SlewLimiter::modifier(float sample, float dt) { + if (!is_positive(dt)) { + return 1.0; + } + if (slew_rate_max <= 0) { _output_slew_rate = 0.0; return 1.0;