Line data    Source code

       1             : #ifndef HAL_DEBUG_BUILD
       2             : #define AP_INLINE_VECTOR_OPS
       3             : #pragma GCC optimize("O2")
       4             : #endif
       5             : #include "LowPassFilter2p.h"
       6             : 
       7             : 
       8             : ////////////////////////////////////////////////////////////////////////////////////////////
       9             : // DigitalBiquadFilter
      10             : ////////////////////////////////////////////////////////////////////////////////////////////
      11             : 
      12             : template <class T>
      13       52400 : DigitalBiquadFilter<T>::DigitalBiquadFilter() {
      14       52400 :   _delay_element_1 = T();
      15       52400 :   _delay_element_2 = T();
      16       52400 : }
      17             : 
      18             : template <class T>
      19   557184470 : T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &params) {
      20   557184470 :     if(!is_positive(params.cutoff_freq) || !is_positive(params.sample_freq)) {
      21       27614 :         return sample;
      22             :     }
      23             : 
      24   557157025 :     if (!initialised) {
      25        8376 :         reset(sample, params);
      26             :     }
      27             : 
      28   557157025 :     T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
      29   557157236 :     T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
      30             : 
      31   557156965 :     _delay_element_2 = _delay_element_1;
      32   557156965 :     _delay_element_1 = delay_element_0;
      33             : 
      34   557156965 :     return output;
      35             : }
      36             : 
      37             : template <class T>
      38           0 : void DigitalBiquadFilter<T>::reset() { 
      39           0 :     initialised = false;
      40           0 : }
      41             : 
      42             : template <class T>
      43        8380 : void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &params) {
      44        8380 :     _delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2));
      45        8380 :     initialised = true;
      46        8380 : }
      47             : 
      48             : template <class T>
      49    40268833 : void DigitalBiquadFilter<T>::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret) {
      50             :     // Keep well under Nyquist limit
      51    40268833 :     ret.cutoff_freq = MIN(cutoff_freq, sample_freq * 0.4);
      52    40268829 :     ret.sample_freq = sample_freq;
      53    40268829 :     if (!is_positive(ret.cutoff_freq)) {
      54             :         // zero cutoff means pass-thru
      55           0 :         return;
      56             :     }
      57             : 
      58    40268841 :     float fr = ret.sample_freq / ret.cutoff_freq;
      59    40268841 :     float ohm = tanf(M_PI/fr);
      60    40268841 :     float c = 1.0f+2.0f*cosf(M_PI/4.0f)*ohm + ohm*ohm;
      61             : 
      62    40268841 :     ret.b0 = ohm*ohm/c;
      63    40268841 :     ret.b1 = 2.0f*ret.b0;
      64    40268841 :     ret.b2 = ret.b0;
      65    40268841 :     ret.a1 = 2.0f*(ohm*ohm-1.0f)/c;
      66    40268841 :     ret.a2 = (1.0f-2.0f*cosf(M_PI/4.0f)*ohm+ohm*ohm)/c;
      67             : }
      68             : 
      69             : 
      70             : ////////////////////////////////////////////////////////////////////////////////////////////
      71             : // LowPassFilter2p
      72             : ////////////////////////////////////////////////////////////////////////////////////////////
      73             : 
      74             : template <class T>
      75       52400 : LowPassFilter2p<T>::LowPassFilter2p() { 
      76       52400 :     memset(&_params, 0, sizeof(_params) ); 
      77       52400 : }
      78             : 
      79             : // constructor
      80             : template <class T>
      81           0 : LowPassFilter2p<T>::LowPassFilter2p(float sample_freq, float cutoff_freq) {
      82             :     // set initial parameters
      83           0 :     set_cutoff_frequency(sample_freq, cutoff_freq);
      84           0 : }
      85             : 
      86             : // change parameters
      87             : template <class T>
      88    40268830 : void LowPassFilter2p<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) {
      89    40268830 :     DigitalBiquadFilter<T>::compute_params(sample_freq, cutoff_freq, _params);
      90    40268836 : }
      91             : 
      92             : // return the cutoff frequency
      93             : template <class T>
      94           0 : float LowPassFilter2p<T>::get_cutoff_freq(void) const {
      95           0 :     return _params.cutoff_freq;
      96             : }
      97             : 
      98             : template <class T>
      99           0 : float LowPassFilter2p<T>::get_sample_freq(void) const {
     100           0 :     return _params.sample_freq;
     101             : }
     102             : 
     103             : template <class T>
     104   557184500 : T LowPassFilter2p<T>::apply(const T &sample) {
     105   557184500 :     return _filter.apply(sample, _params);
     106             : }
     107             : 
     108             : template <class T>
     109           0 : void LowPassFilter2p<T>::reset(void) {
     110           0 :     return _filter.reset();
     111             : }
     112             : 
     113             : template <class T>
     114           4 : void LowPassFilter2p<T>::reset(const T &value) {
     115           4 :     return _filter.reset(value, _params);
     116             : }
     117             : 
     118             : /* 
     119             :  * Make an instances
     120             :  * Otherwise we have to move the constructor implementations to the header file :P
     121             :  */
     122             : template class LowPassFilter2p<int>;
     123             : template class LowPassFilter2p<long>;
     124             : template class LowPassFilter2p<float>;
     125             : template class LowPassFilter2p<Vector2f>;
     126             : template class LowPassFilter2p<Vector3f>;