Filter: rework LowPassFilter

libraries/Filter/LowPassFilter.h

@@ -26,114 +26,141 @@
 #include <AP_Math.h>
 #include "FilterClass.h"
 
-// 1st parameter <T> is the type of data being filtered.
+class DigitalLPF
-template <class T>
-class LowPassFilter : public Filter<T>
 {
 public:
     // constructor
-    LowPassFilter();
+    DigitalLPF() :
+    _output(0.0f) {}
 
-    void set_cutoff_frequency(float time_step, float cutoff_freq);
+    struct lpf_params {
-    float get_cutoff_frequency() { return _cutoff_hz; }
+        float cutoff_freq;
-    void set_time_constant(float time_step, float time_constant);
+        float sample_freq;
+        float alpha;
+    };
 
-    // apply - Add a new raw value to the filter, retrieve the filtered result
+    float apply(float sample, float cutoff_freq, float dt) {
-    virtual T        apply(T sample);
+        if (cutoff_freq <= 0.0f) {
+            _output = sample;
+            return _output;
+        }
 
-    // get - returns latest filtered value from filter (equal to the value returned by latest call to apply method)
+        float rc = 1.0f/(2.0f*M_PI_F*cutoff_freq);
-    virtual T        get() const { return (T)_base_value; }
+        float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
+        _output += (sample - _output) * alpha;
+        return _output;
+    }
 
-    // reset - clear the filter - next sample added will become the new base value
+    float get() const {
-    virtual void        reset() {
+        return _output;
-        _base_value_set = false;
+    }
-    };
 
-    // reset - clear the filter and provide the new base value
+    void reset(float value) { _output = value; }
-    void        reset( T new_base_value ) {
-        _base_value = new_base_value; _base_value_set = true;
-    };
 
 private:
-    float           _alpha;             // gain value  (like 0.02) applied to each new value
+    float _output;
-    bool            _base_value_set;    // true if the base value has been set
-    float           _base_value;        // filter output
-    float           _cutoff_hz;         // cutoff frequency in hz
 };
 
-// Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size)
+class LowPassFilter
-typedef LowPassFilter<int8_t> LowPassFilterInt8;
+{
-typedef LowPassFilter<uint8_t> LowPassFilterUInt8;
+public:
-
+    LowPassFilter() :
-typedef LowPassFilter<int16_t> LowPassFilterInt16;
+    _cutoff_freq(0.0f) { }
-typedef LowPassFilter<uint16_t> LowPassFilterUInt16;
+    // constructor
+    LowPassFilter(float cutoff_freq) :
+    _cutoff_freq(cutoff_freq) { }
 
-typedef LowPassFilter<int32_t> LowPassFilterInt32;
+    // change parameters
-typedef LowPassFilter<uint32_t> LowPassFilterUInt32;
+    void set_cutoff_frequency(float cutoff_freq) {
+        _cutoff_freq = cutoff_freq;
+    }
 
-typedef LowPassFilter<float> LowPassFilterFloat;
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
 
-// Constructor    //////////////////////////////////////////////////////////////
+protected:
+    float _cutoff_freq;
+};
 
-template <class T>
+class LowPassFilterFloat : public LowPassFilter
-LowPassFilter<T>::LowPassFilter() :
+{
-    Filter<T>(),
+public:
-    _alpha(1),
+    LowPassFilterFloat() :
-    _base_value_set(false)
+    LowPassFilter() {}
-{};
 
-//    F_Cut = 1; % Hz
+    LowPassFilterFloat(float cutoff_freq):
-//RC = 1/(2*pi*F_Cut);
+    LowPassFilter(cutoff_freq) {}
-//Alpha = Ts/(Ts + RC);
 
-// Public Methods //////////////////////////////////////////////////////////////
+    float apply(float sample, float dt) {
+        return _filter.apply(sample, _cutoff_freq, dt);
+    }
 
-template <class T>
+    float get() const {
-void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq)
+        return _filter.get();
-{
-    _cutoff_hz = cutoff_freq;
-    // avoid divide by zero and allow removing filtering
-    if (cutoff_freq <= 0.0f) {
-        _alpha = 1.0f;
-        return;
     }
 
-    // calculate alpha
+    void reset(float value) {
-    float rc = 1/(2*M_PI_F*cutoff_freq);
+        _filter.reset(value);
-    _alpha = time_step / (time_step + rc);
+    }
-}
+private:
+    DigitalLPF _filter;
+};
 
-template <class T>
+class LowPassFilterVector2f : public LowPassFilter
-void LowPassFilter<T>::set_time_constant(float time_step, float time_constant)
 {
-    // avoid divide by zero
+public:
-    if ((time_constant <= 0.0f) || (time_step <= 0.0f)) {
+    LowPassFilterVector2f() :
-        _cutoff_hz = 0.0f;
+    LowPassFilter() {}
-        _alpha = 1.0f;
+
-        return;
+    LowPassFilterVector2f(float cutoff_freq) :
+    LowPassFilter(cutoff_freq) {}
+
+    Vector2f apply(const Vector2f &sample, float dt) {
+        Vector2f ret;
+        ret.x = _filter_x.apply(sample.x, _cutoff_freq, dt);
+        ret.y = _filter_y.apply(sample.y, _cutoff_freq, dt);
+        return ret;
     }
 
-    _cutoff_hz = 1/(2*M_PI_F*time_constant);
+    void reset(const Vector2f& value) {
+        _filter_x.reset(value.x);
+        _filter_y.reset(value.y);
+    }
 
-    // calculate alpha
+private:
-    _alpha = time_step / (time_constant + time_step);
+    DigitalLPF _filter_x;
-}
+    DigitalLPF _filter_y;
+};
 
-template <class T>
+class LowPassFilterVector3f : public LowPassFilter
-T LowPassFilter<T>::apply(T sample)
 {
-    // initailise _base_value if required
+public:
-    if( !_base_value_set ) {
+    LowPassFilterVector3f() :
-        _base_value = sample;
+    LowPassFilter() {}
-        _base_value_set = true;
+
+    LowPassFilterVector3f(float cutoff_freq) :
+    LowPassFilter(cutoff_freq) {}
+
+    Vector3f apply(const Vector3f &sample, float dt) {
+        Vector3f ret;
+        ret.x = _filter_x.apply(sample.x, _cutoff_freq, dt);
+        ret.y = _filter_y.apply(sample.y, _cutoff_freq, dt);
+        ret.z = _filter_z.apply(sample.z, _cutoff_freq, dt);
+        return ret;
     }
 
-    // do the filtering
+    void reset(const Vector3f& value) {
-    //_base_value = _alpha * (float)sample + (1.0 - _alpha) * _base_value;
+        _filter_x.reset(value.x);
-    _base_value = _base_value + _alpha * ((float)sample - _base_value);
+        _filter_y.reset(value.y);
+        _filter_z.reset(value.z);
+    }
 
-    // return the value.  Should be no need to check limits
+private:
-    return (T)_base_value;
+    DigitalLPF _filter_x;
-}
+    DigitalLPF _filter_y;
+    DigitalLPF _filter_z;
+};
 
 #endif // __LOW_PASS_FILTER_H__