From f94e4b43752664047f4a346864c259de227b7fbc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 10 Nov 2016 11:42:58 +1100
Subject: [PATCH] Filter: allow 1P LowPassFilter to work without alpha recalc
 per sample

this makes the 1P filter optionally syntax compatible with the 2P
filter and much more CPU efficient.
---
 libraries/Filter/LowPassFilter.cpp | 31 ++++++++++++++++++++++--
 libraries/Filter/LowPassFilter.h   | 39 +++++++++++++++++++++++++-----
 2 files changed, 62 insertions(+), 8 deletions(-)

diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp
index eba28c3922..be14b7148b 100644
--- a/libraries/Filter/LowPassFilter.cpp
+++ b/libraries/Filter/LowPassFilter.cpp
@@ -24,13 +24,29 @@ T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
         _output = sample;
         return _output;
     }
-
     float rc = 1.0f/(M_2PI*cutoff_freq);
-    float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
+    alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
     _output += (sample - _output) * alpha;
     return _output;
 }
 
+template <class T>
+T DigitalLPF<T>::apply(const T &sample) {
+    _output += (sample - _output) * alpha;
+    return _output;
+}
+
+template <class T>
+void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) {
+    if (cutoff_freq <= 0.0f || sample_freq <= 0.0f) {
+        alpha = 1.0;
+    } else {
+        float dt = 1.0/sample_freq;
+        float rc = 1.0f/(M_2PI*cutoff_freq);
+        alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
+    }
+}
+
 // get latest filtered value from filter (equal to the value returned by latest call to apply method)
 template <class T>
 const T &DigitalLPF<T>::get() const {
@@ -61,6 +77,12 @@ void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
     _cutoff_freq = 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 {
@@ -72,6 +94,11 @@ T LowPassFilter<T>::apply(T sample, float dt) {
     return _filter.apply(sample, _cutoff_freq, dt);
 }
 
+template <class T>
+T LowPassFilter<T>::apply(T sample) {
+    return _filter.apply(sample);
+}
+
 template <class T>
 const T &LowPassFilter<T>::get() const {
     return _filter.get();
diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h
index dffd006380..6781e28b67 100644
--- a/libraries/Filter/LowPassFilter.h
+++ b/libraries/Filter/LowPassFilter.h
@@ -18,6 +18,30 @@
 /// @brief	A class to implement a low pass filter without losing precision even for int types
 ///         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:
+
+   1) providing dt on every sample, and calling apply like this:
+
+      // call once
+      filter.set_cutoff_frequency(frequency_hz);
+
+      // then on each sample
+      output = filter.apply(sample, dt);
+
+   2) providing a sample freq and cutoff_freq once at start
+
+      // call once
+      filter.set_cutoff_frequency(sample_freq, frequency_hz);
+
+      // then on each sample
+      output = filter.apply(sample);
+
+  The second approach is more CPU efficient as it doesn't have to
+  recalculate alpha each time, but it assumes that dt is constant
+ */
+
 #pragma once
 
 #include <AP_Math/AP_Math.h>
@@ -27,21 +51,20 @@
 template <class T>
 class DigitalLPF {
 public:
-    struct lpf_params {
-        float cutoff_freq;
-        float sample_freq;
-        float alpha;
-    };  
-
     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);
+
+    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);
 
 private:
     T _output;
+    float alpha = 1.0f;
 };
 
 // LPF base class
@@ -53,11 +76,15 @@ public:
 
     // change parameters
     void set_cutoff_frequency(float cutoff_freq);
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
     // return the cutoff frequency
     float get_cutoff_freq(void) const;
     T apply(T sample, float dt);
+    T apply(T sample);
     const T &get() const;
     void reset(T value);
+    void reset(void) { reset(T()); }
     
 protected:
     float _cutoff_freq;