Line data    Source code

       1             : //
       2             : /// @file LowPassFilter.cpp
       3             : /// @brief  A class to implement a low pass filter
       4             : 
       5             : #ifndef HAL_DEBUG_BUILD
       6             : #define AP_INLINE_VECTOR_OPS
       7             : #pragma GCC optimize("O2")
       8             : #endif
       9             : #include "LowPassFilter.h"
      10             : #include <AP_InternalError/AP_InternalError.h>
      11             : 
      12             : ////////////////////////////////////////////////////////////////////////////////////////////
      13             : // DigitalLPF, base class
      14             : ////////////////////////////////////////////////////////////////////////////////////////////
      15             : 
      16             : template <class T>
      17      267406 : DigitalLPF<T>::DigitalLPF() {
      18             :   // built in initialization
      19      267406 :   output = T();
      20      267406 : }
      21             : 
      22             : // add a new raw value to the filter, retrieve the filtered result
      23             : template <class T>
      24  1358251670 : T DigitalLPF<T>::_apply(const T &sample, const float &alpha) {
      25  1358251670 :     output += (sample - output) * alpha;
      26  1358252080 :     if (!initialised) {
      27       14801 :         initialised = true;
      28       14801 :         output = sample;
      29             :     }
      30  1358252080 :     return output;
      31             : }
      32             : 
      33             : // get latest filtered value from filter (equal to the value returned by latest call to apply method)
      34             : template <class T>
      35   710404246 : const T &DigitalLPF<T>::get() const {
      36   710404246 :     return output;
      37             : }
      38             : 
      39             : // Reset filter to given value
      40             : template <class T>
      41    43287421 : void DigitalLPF<T>::reset(const T &value) { 
      42    43287421 :     output = value;
      43    43287421 :     initialised = true;
      44    43287421 : }
      45             : 
      46             : // Set reset flag such that the filter will be reset to the next value applied
      47             : template <class T>
      48          16 : void DigitalLPF<T>::reset() {
      49          16 :     initialised = false;
      50          16 : }
      51             : 
      52             : template class DigitalLPF<float>;
      53             : template class DigitalLPF<Vector2f>;
      54             : template class DigitalLPF<Vector3f>;
      55             : 
      56             : ////////////////////////////////////////////////////////////////////////////////////////////
      57             : // Low pass filter with constant time step
      58             : ////////////////////////////////////////////////////////////////////////////////////////////
      59             : 
      60             : // constructor
      61             : template <class T>
      62           0 : LowPassFilterConstDt<T>::LowPassFilterConstDt(const float &sample_freq, const float &new_cutoff_freq)
      63             : {
      64           0 :     set_cutoff_frequency(sample_freq, new_cutoff_freq);
      65           0 : }
      66             : 
      67             : // change parameters
      68             : template <class T>
      69        1317 : void LowPassFilterConstDt<T>::set_cutoff_frequency(const float &sample_freq, const float &new_cutoff_freq) {
      70        1317 :     cutoff_freq = new_cutoff_freq;
      71             : 
      72        1317 :     if (sample_freq <= 0) {
      73           0 :         alpha = 1;
      74             :     } else {
      75        1317 :         alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq);
      76             :     }
      77        1317 : }
      78             : 
      79             : // return the cutoff frequency
      80             : template <class T>
      81       50319 : float LowPassFilterConstDt<T>::get_cutoff_freq(void) const {
      82       50319 :     return cutoff_freq;
      83             : }
      84             : 
      85             : // add a new raw value to the filter, retrieve the filtered result
      86             : template <class T>
      87     2506170 : T LowPassFilterConstDt<T>::apply(const T &sample) {
      88     2506170 :     return this->_apply(sample, alpha);
      89             : }
      90             : 
      91             : template class LowPassFilterConstDt<float>;
      92             : template class LowPassFilterConstDt<Vector2f>;
      93             : template class LowPassFilterConstDt<Vector3f>;
      94             : 
      95             : ////////////////////////////////////////////////////////////////////////////////////////////
      96             : // Low pass filter with variable time step
      97             : ////////////////////////////////////////////////////////////////////////////////////////////
      98             : 
      99             : template <class T>
     100       17514 : LowPassFilter<T>::LowPassFilter(const float &new_cutoff_freq)
     101             : {
     102       17514 :     set_cutoff_frequency(new_cutoff_freq);
     103       17514 : }
     104             : 
     105             : // change parameters
     106             : template <class T>
     107   369249880 : void LowPassFilter<T>::set_cutoff_frequency(const float &new_cutoff_freq) {
     108   369249880 :     cutoff_freq = new_cutoff_freq;
     109   369249880 : }
     110             : 
     111             : // return the cutoff frequency
     112             : template <class T>
     113      314943 : float LowPassFilter<T>::get_cutoff_freq() const {
     114      314943 :     return cutoff_freq;
     115             : }
     116             : 
     117             : // add a new raw value to the filter, retrieve the filtered result
     118             : template <class T>
     119  1355751465 : T LowPassFilter<T>::apply(const T &sample, const float &dt) {
     120  1355751465 :     const float alpha = calc_lowpass_alpha_dt(dt, cutoff_freq);
     121  1355748421 :     return this->_apply(sample, alpha);
     122             : }
     123             : 
     124             : /* 
     125             :  * Make an instances
     126             :  * Otherwise we have to move the constructor implementations to the header file :P
     127             :  */
     128             : template class LowPassFilter<float>;
     129             : template class LowPassFilter<Vector2f>;
     130             : template class LowPassFilter<Vector3f>;
     131             :