Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : //
      17             : /// @file       LowPassFilter.h
      18             : /// @brief      A class to implement a low pass filter.
      19             : 
      20             : /*
      21             :   Two classes are provided:
      22             : 
      23             :    LowPassFilter: providing dt on every sample, and calling apply like this:
      24             : 
      25             :       // call once
      26             :       filter.set_cutoff_frequency(frequency_hz);
      27             : 
      28             :       // then on each sample
      29             :       output = filter.apply(sample, dt);
      30             : 
      31             :    LowPassFilterConstDt: providing a sample freq and cutoff_freq once at start
      32             : 
      33             :       // call once
      34             :       filter.set_cutoff_frequency(sample_freq, frequency_hz);
      35             : 
      36             :       // then on each sample
      37             :       output = filter.apply(sample);
      38             : 
      39             :   The second approach is more CPU efficient as it doesn't have to
      40             :   recalculate alpha each time, but it assumes that dt is constant
      41             :  */
      42             : 
      43             : #pragma once
      44             : 
      45             : #include <AP_Math/AP_Math.h>
      46             : 
      47             : // DigitalLPF implements the filter math
      48             : template <class T>
      49             : class DigitalLPF {
      50             : public:
      51             : 
      52             :     // constructor
      53             :     DigitalLPF();
      54             : 
      55             :     CLASS_NO_COPY(DigitalLPF);
      56             : 
      57             :     // get latest filtered value from filter (equal to the value returned by latest call to apply method)
      58             :     const T &get() const;
      59             : 
      60             :     // Reset filter to given value
      61             :     void reset(const T &value);
      62             : 
      63             :     // Set reset flag such that the filter will be reset to the next value applied
      64             :     void reset();
      65             : 
      66             : protected:
      67             :     // add a new raw value to the filter, retrieve the filtered result
      68             :     T _apply(const T &sample, const float &alpha);
      69             : 
      70             : private:
      71             :     T output;
      72             :     bool initialised;
      73             : };
      74             : 
      75             : // Low pass filter with constant time step
      76             : template <class T>
      77             : class LowPassFilterConstDt : public DigitalLPF<T> {
      78             : public:
      79             : 
      80             :     // constructors
      81       64591 :     LowPassFilterConstDt() {};
      82             :     LowPassFilterConstDt(const float &sample_freq, const float &cutoff_freq);
      83             : 
      84             :     CLASS_NO_COPY(LowPassFilterConstDt);
      85             : 
      86             :     // change parameters
      87             :     void set_cutoff_frequency(const float &sample_freq, const float &cutoff_freq);
      88             : 
      89             :     // return the cutoff frequency
      90             :     float get_cutoff_freq() const;
      91             : 
      92             :     // add a new raw value to the filter, retrieve the filtered result
      93             :     T apply(const T &sample);
      94             : 
      95             : private:
      96             :     float cutoff_freq;
      97             :     float alpha;
      98             : };
      99             : 
     100             : typedef LowPassFilterConstDt<float>    LowPassFilterConstDtFloat;
     101             : typedef LowPassFilterConstDt<Vector2f> LowPassFilterConstDtVector2f;
     102             : typedef LowPassFilterConstDt<Vector3f> LowPassFilterConstDtVector3f;
     103             : 
     104             : // Low pass filter with variable time step
     105             : template <class T>
     106             : class LowPassFilter : public DigitalLPF<T> {
     107             : public:
     108             : 
     109             :     // constructors
     110      185301 :     LowPassFilter() {};
     111             :     LowPassFilter(const float &cutoff_freq);
     112             : 
     113             :     CLASS_NO_COPY(LowPassFilter);
     114             : 
     115             :     // change parameters
     116             :     void set_cutoff_frequency(const float &cutoff_freq);
     117             : 
     118             :     // return the cutoff frequency
     119             :     float get_cutoff_freq() const;
     120             : 
     121             :     // add a new raw value to the filter, retrieve the filtered result
     122             :     T apply(const T &sample, const float &dt);
     123             : 
     124             : private:
     125             :     float cutoff_freq;
     126             : };
     127             : 
     128             : // typedefs for compatibility
     129             : typedef LowPassFilter<float>    LowPassFilterFloat;
     130             : typedef LowPassFilter<Vector2f> LowPassFilterVector2f;
     131             : typedef LowPassFilter<Vector3f> LowPassFilterVector3f;