From b6ecfa0db4af7bfaba9d7f7af4e696e84379bc8a Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Mon, 5 Dec 2022 16:48:51 +1030
Subject: [PATCH] Filter: Support changing update period

---
 libraries/Filter/LowPassFilter.cpp | 11 ++++++++++-
 libraries/Filter/SlewLimiter.cpp   |  4 ++++
 2 files changed, 14 insertions(+), 1 deletion(-)

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 <AP_InternalError/AP_InternalError.h>
 
 ////////////////////////////////////////////////////////////////////////////////////////////
 // DigitalLPF
@@ -23,10 +24,18 @@ DigitalLPF<T>::DigitalLPF() {
 // add a new raw value to the filter, retrieve the filtered result
 template <class T>
 T DigitalLPF<T>::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;