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             : #pragma once
      16             : 
      17             : /*
      18             :   notch filter with settable sample rate, center frequency, bandwidth and attenuation
      19             : 
      20             :   Design by Leonard Hall
      21             :  */
      22             : 
      23             : #include <AP_Math/AP_Math.h>
      24             : #include <cmath>
      25             : #include <inttypes.h>
      26             : #include <AP_Param/AP_Param.h>
      27             : 
      28             : 
      29             : template <class T>
      30             : class HarmonicNotchFilter;
      31             : 
      32             : template <class T>
      33             : class NotchFilter {
      34             : public:
      35             :     friend class HarmonicNotchFilter<T>;
      36             :     // set parameters
      37             :     void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
      38             :     void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q);
      39             :     T apply(const T &sample);
      40             :     void reset();
      41           0 :     float center_freq_hz() const { return _center_freq_hz; }
      42         720 :     float sample_freq_hz() const { return _sample_freq_hz; }
      43             : 
      44             :     // calculate attenuation and quality from provided center frequency and bandwidth
      45             :     static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q); 
      46             : 
      47   510241325 :     void disable(void) {
      48   510241325 :         initialised = false;
      49   510241325 :     }
      50             : 
      51             :     // return the frequency to log for the notch
      52             :     float logging_frequency(void) const;
      53             : 
      54             : protected:
      55             : 
      56             :     bool initialised, need_reset;
      57             :     float b0, b1, b2, a1, a2;
      58             :     float _center_freq_hz, _sample_freq_hz, _A;
      59             :     T ntchsig1, ntchsig2, signal2, signal1;
      60             : };
      61             : 
      62             : /*
      63             :   notch filter enable and filter parameters
      64             :  */
      65             : class NotchFilterParams {
      66             : public:
      67     3121256 :     float center_freq_hz(void) const { return _center_freq_hz; }
      68     2627680 :     float bandwidth_hz(void) const { return _bandwidth_hz; }
      69     2627588 :     float attenuation_dB(void) const { return _attenuation_dB; }
      70  1115128921 :     uint8_t enabled(void) const { return _enable; }
      71           2 :     void enable() { _enable.set(true); }
      72             :     
      73             : protected:
      74             :     AP_Int8 _enable;
      75             :     AP_Float _center_freq_hz;
      76             :     AP_Float _bandwidth_hz;
      77             :     AP_Float _attenuation_dB;
      78             : };
      79             : 
      80             : typedef NotchFilter<float> NotchFilterFloat;
      81             : typedef NotchFilter<Vector2f> NotchFilterVector2f;
      82             : typedef NotchFilter<Vector3f> NotchFilterVector3f;
      83             :