Filter: LowPassFilter: split into two classes for constant and variable dt

libraries/Filter/LowPassFilter.cpp

@@ -1,8 +1,6 @@
 //
 /// @file LowPassFilter.cpp
-/// @brief  A class to implement a low pass filter without losing precision even for int types
+/// @brief  A class to implement a low pass filter
-///         the downside being that it's a little slower as it internally uses a float
-///         and it consumes an extra 4 bytes of memory to hold the constant gain
 
 #ifndef HAL_DEBUG_BUILD
 #define AP_INLINE_VECTOR_OPS
@@ -12,52 +10,65 @@
 #include <AP_InternalError/AP_InternalError.h>
 
 ////////////////////////////////////////////////////////////////////////////////////////////
-// DigitalLPF
+// DigitalLPF, base class
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 template <class T>
 DigitalLPF<T>::DigitalLPF() {
   // built in initialization
-  _output = T();
+  output = T();
 }
 
 // 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) {
+T DigitalLPF<T>::_apply(const T &sample, const float &alpha) {
-    if (is_negative(cutoff_freq) || is_negative(dt)) {
+    output += (sample - output) * alpha;
-        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;
     if (!initialised) {
         initialised = true;
-        _output = sample;
+        output = sample;
     }
-    return _output;
+    return output;
 }
 
+// get latest filtered value from filter (equal to the value returned by latest call to apply method)
 template <class T>
-T DigitalLPF<T>::apply(const T &sample) {
+const T &DigitalLPF<T>::get() const {
-    _output += (sample - _output) * alpha;
+    return output;
-    if (!initialised) {
+}
+
+// Reset filter to given value
+template <class T>
+void DigitalLPF<T>::reset(const T &value) { 
+    output = value;
     initialised = true;
-        _output = sample;
-    }
-    return _output;
 }
 
+// Set reset flag such that the filter will be reset to the next value applied
 template <class T>
-void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) {
+void DigitalLPF<T>::reset() {
+    initialised = false;
+}
+
+template class DigitalLPF<float>;
+template class DigitalLPF<Vector2f>;
+template class DigitalLPF<Vector3f>;
+
+////////////////////////////////////////////////////////////////////////////////////////////
+// Low pass filter with constant time step
+////////////////////////////////////////////////////////////////////////////////////////////
+
+// constructor
+template <class T>
+LowPassFilterConstDt<T>::LowPassFilterConstDt(const float &sample_freq, const float &new_cutoff_freq)
+{
+    set_cutoff_frequency(sample_freq, new_cutoff_freq);
+}
+
+// change parameters
+template <class T>
+void LowPassFilterConstDt<T>::set_cutoff_frequency(const float &sample_freq, const float &new_cutoff_freq) {
+    cutoff_freq = new_cutoff_freq;
+
     if (sample_freq <= 0) {
         alpha = 1;
     } else {
@@ -65,81 +76,68 @@ void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) {
     }
 }
 
-// get latest filtered value from filter (equal to the value returned by latest call to apply method)
+// return the cutoff frequency
 template <class T>
-const T &DigitalLPF<T>::get() const {
+float LowPassFilterConstDt<T>::get_cutoff_freq(void) const {
-    return _output;
+    return cutoff_freq;
 }
 
+// add a new raw value to the filter, retrieve the filtered result
 template <class T>
-void DigitalLPF<T>::reset(T value) { 
+T LowPassFilterConstDt<T>::apply(const T &sample) {
-    _output = value;
+    return this->_apply(sample, alpha);
-    initialised = true;
 }
 
+template class LowPassFilterConstDt<float>;
+template class LowPassFilterConstDt<Vector2f>;
+template class LowPassFilterConstDt<Vector3f>;
+
 ////////////////////////////////////////////////////////////////////////////////////////////
-// LowPassFilter
+// Low pass filter with variable time step
 ////////////////////////////////////////////////////////////////////////////////////////////
 
-// constructors
 template <class T>
-LowPassFilter<T>::LowPassFilter() :
+LowPassFilter<T>::LowPassFilter(const float &new_cutoff_freq)
-    _cutoff_freq(0.0f) {}
-
-template <class T>
-LowPassFilter<T>::LowPassFilter(float cutoff_freq) :
-    _cutoff_freq(cutoff_freq) {}
-
-template <class T>
-LowPassFilter<T>::LowPassFilter(float sample_freq, float cutoff_freq)
 {
-    set_cutoff_frequency(sample_freq, cutoff_freq);
+    set_cutoff_frequency(new_cutoff_freq);
 }
 
 // change parameters
 template <class T>
-void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
+void LowPassFilter<T>::set_cutoff_frequency(const float &new_cutoff_freq) {
-    _cutoff_freq = cutoff_freq;
+    cutoff_freq = new_cutoff_freq;
-}
-
-template <class T>
-void LowPassFilter<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) {
-    _cutoff_freq = cutoff_freq;
-    _filter.compute_alpha(sample_freq, cutoff_freq);
 }
 
 // return the cutoff frequency
 template <class T>
-float LowPassFilter<T>::get_cutoff_freq(void) const {
+float LowPassFilter<T>::get_cutoff_freq() const {
-    return _cutoff_freq;
+    return cutoff_freq;
 }
 
+// add a new raw value to the filter, retrieve the filtered result
 template <class T>
-T LowPassFilter<T>::apply(T sample, float dt) {
+T LowPassFilter<T>::apply(const T &sample, const float &dt) {
-    return _filter.apply(sample, _cutoff_freq, dt);
+    if (is_negative(cutoff_freq) || is_negative(dt)) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+        this->reset(sample);
+        return this->get();
     }
-
+    if (is_zero(cutoff_freq)) {
-template <class T>
+        this->reset(sample);
-T LowPassFilter<T>::apply(T sample) {
+        return this->get();
-    return _filter.apply(sample);
     }
-
+    if (is_zero(dt)) {
-template <class T>
+        return this->get();
-const T &LowPassFilter<T>::get() const {
-    return _filter.get();
     }
-
+    const float rc = 1.0f/(M_2PI*cutoff_freq);
-template <class T>
+    const float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
-void LowPassFilter<T>::reset(T value) {
+    return this->_apply(sample, alpha);
-    _filter.reset(value);
 }
 
 /* 
  * Make an instances
  * Otherwise we have to move the constructor implementations to the header file :P
  */
-template class LowPassFilter<int>;
-template class LowPassFilter<long>;
 template class LowPassFilter<float>;
 template class LowPassFilter<Vector2f>;
 template class LowPassFilter<Vector3f>;

libraries/Filter/LowPassFilter.h

@@ -15,14 +15,12 @@
 
 //
 /// @file	LowPassFilter.h
-/// @brief	A class to implement a low pass filter without losing precision even for int types
+/// @brief	A class to implement a low pass filter.
-///         the downside being that it's a little slower as it internally uses a float
-///         and it consumes an extra 4 bytes of memory to hold the constant gain
 
 /*
-  Note that this filter can be used in 2 ways:
+  Two classes are provided:
 
-   1) providing dt on every sample, and calling apply like this:
+   LowPassFilter: providing dt on every sample, and calling apply like this:
 
       // call once
       filter.set_cutoff_frequency(frequency_hz);
@@ -30,7 +28,7 @@
       // then on each sample
       output = filter.apply(sample, dt);
 
-   2) providing a sample freq and cutoff_freq once at start
+   LowPassFilterConstDt: providing a sample freq and cutoff_freq once at start
 
       // call once
       filter.set_cutoff_frequency(sample_freq, frequency_hz);
@@ -45,79 +43,89 @@
 #pragma once
 
 #include <AP_Math/AP_Math.h>
-#include "FilterClass.h"
 
 // DigitalLPF implements the filter math
 template <class T>
 class DigitalLPF {
 public:
+
+    // constructor
     DigitalLPF();
-    // add a new raw value to the filter, retrieve the filtered result
-    T apply(const T &sample, float cutoff_freq, float dt);
-    T apply(const T &sample);
 
     CLASS_NO_COPY(DigitalLPF);
 
-    void compute_alpha(float sample_freq, float cutoff_freq);
-    
     // get latest filtered value from filter (equal to the value returned by latest call to apply method)
     const T &get() const;
-    void reset(T value);
+
-    void reset() {
+    // Reset filter to given value
-        initialised = false;
+    void reset(const T &value);
-    }
+
+    // Set reset flag such that the filter will be reset to the next value applied
+    void reset();
+
+protected:
+    // add a new raw value to the filter, retrieve the filtered result
+    T _apply(const T &sample, const float &alpha);
 
 private:
-    T _output;
+    T output;
-    float alpha = 1.0f;
     bool initialised;
 };
 
-// LPF base class
+// Low pass filter with constant time step
 template <class T>
-class LowPassFilter {
+class LowPassFilterConstDt : public DigitalLPF<T> {
 public:
-    LowPassFilter();
+
-    LowPassFilter(float cutoff_freq);
+    // constructors
-    LowPassFilter(float sample_freq, float cutoff_freq);
+    LowPassFilterConstDt() {};
+    LowPassFilterConstDt(const float &sample_freq, const float &cutoff_freq);
+
+    CLASS_NO_COPY(LowPassFilterConstDt);
+
+    // change parameters
+    void set_cutoff_frequency(const float &sample_freq, const float &cutoff_freq);
+
+    // return the cutoff frequency
+    float get_cutoff_freq() const;
+
+    // add a new raw value to the filter, retrieve the filtered result
+    T apply(const T &sample);
+
+private:
+    float cutoff_freq;
+    float alpha;
+};
+
+typedef LowPassFilterConstDt<float>    LowPassFilterConstDtFloat;
+typedef LowPassFilterConstDt<Vector2f> LowPassFilterConstDtVector2f;
+typedef LowPassFilterConstDt<Vector3f> LowPassFilterConstDtVector3f;
+
+// Low pass filter with variable time step
+template <class T>
+class LowPassFilter : public DigitalLPF<T> {
+public:
+
+    // constructors
+    LowPassFilter() {};
+    LowPassFilter(const float &cutoff_freq);
 
     CLASS_NO_COPY(LowPassFilter);
 
     // change parameters
-    void set_cutoff_frequency(float cutoff_freq);
+    void set_cutoff_frequency(const float &cutoff_freq);
-    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
 
     // return the cutoff frequency
-    float get_cutoff_freq(void) const;
+    float get_cutoff_freq() const;
-    T apply(T sample, float dt);
-    T apply(T sample);
-    const T &get() const;
-    void reset(T value);
-    void reset(void) { _filter.reset(); }
 
-protected:
+    // add a new raw value to the filter, retrieve the filtered result
-    float _cutoff_freq;
+    T apply(const T &sample, const float &dt);
 
 private:
-    DigitalLPF<T> _filter;
+    float cutoff_freq;
 };
 
-// Uncomment this, if you decide to remove the instantiations in the implementation file
-/*
-template <class T>
-LowPassFilter<T>::LowPassFilter() : _cutoff_freq(0.0f) { 
-  
-}
-// constructor
-template <class T>
-LowPassFilter<T>::LowPassFilter(float cutoff_freq) : _cutoff_freq(cutoff_freq) { 
-  
-}
-*/
-
 // typedefs for compatibility
-typedef LowPassFilter<int>      LowPassFilterInt;
-typedef LowPassFilter<long>     LowPassFilterLong;
 typedef LowPassFilter<float>    LowPassFilterFloat;
 typedef LowPassFilter<Vector2f> LowPassFilterVector2f;
 typedef LowPassFilter<Vector3f> LowPassFilterVector3f;