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             : #ifndef HAL_DEBUG_BUILD
      17             : #define AP_INLINE_VECTOR_OPS
      18             : #pragma GCC optimize("O2")
      19             : #endif
      20             : 
      21             : #include "NotchFilter.h"
      22             : #include <AP_Logger/AP_Logger.h>
      23             : 
      24             : const static float NOTCH_MAX_SLEW       = 0.05f;
      25             : const static float NOTCH_MAX_SLEW_LOWER = 1.0f - NOTCH_MAX_SLEW;
      26             : const static float NOTCH_MAX_SLEW_UPPER = 1.0f / NOTCH_MAX_SLEW_LOWER;
      27             : 
      28             : /*
      29             :    calculate the attenuation and quality factors of the filter
      30             :  */
      31             : template <class T>
      32      271107 : void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q) {
      33      271107 :     A = powf(10, -attenuation_dB / 40.0f);
      34      271107 :     if (center_freq_hz > 0.5 * bandwidth_hz) {
      35      271108 :         const float octaves = log2f(center_freq_hz / (center_freq_hz - bandwidth_hz / 2.0f)) * 2.0f;
      36      271108 :         Q = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1.0f);
      37             :     } else {
      38           0 :         Q = 0.0;
      39             :     }
      40      271107 : }
      41             : 
      42             : /*
      43             :   initialise filter
      44             :  */
      45             : template <class T>
      46         724 : void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
      47             : {
      48             :     // check center frequency is in the allowable range
      49         724 :     initialised = false;
      50         724 :     if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq_hz)) {
      51             :         float A, Q;
      52         724 :         calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, A, Q);
      53         724 :         init_with_A_and_Q(sample_freq_hz, center_freq_hz, A, Q);
      54             :     }
      55         724 : }
      56             : 
      57             : template <class T>
      58  3846458235 : void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
      59             : {
      60             :     // don't update if no updates required
      61  3846352610 :     if (initialised &&
      62  7691425088 :         is_equal(center_freq_hz, _center_freq_hz) &&
      63 11537883337 :         is_equal(sample_freq_hz, _sample_freq_hz) &&
      64  3845071830 :         is_equal(A, _A)) {
      65  3844620197 :         return;
      66             :     }
      67             : 
      68     1838052 :     float new_center_freq = center_freq_hz;
      69             : 
      70             :     // constrain the new center frequency by a percentage of the old frequency
      71     1838052 :     if (initialised && !need_reset && !is_zero(_center_freq_hz)) {
      72      903203 :         new_center_freq = constrain_float(new_center_freq, _center_freq_hz * NOTCH_MAX_SLEW_LOWER,
      73             :                                           _center_freq_hz * NOTCH_MAX_SLEW_UPPER);
      74             :     }
      75             : 
      76     1838052 :     if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {
      77     1838044 :         float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz;
      78     1838044 :         float alpha = sinf(omega) / (2 * Q);
      79     1838044 :         b0 =  1.0 + alpha*sq(A);
      80     1838048 :         b1 = -2.0 * cosf(omega);
      81     1838048 :         b2 =  1.0 - alpha*sq(A);
      82     1838062 :         a1 = b1;
      83     1838062 :         a2 =  1.0 - alpha;
      84             : 
      85     1838062 :         const float a0_inv =  1.0/(1.0 + alpha);
      86             : 
      87             :         // Pre-multiply to save runtime calc
      88     1838062 :         b0 *= a0_inv;
      89     1838062 :         b1 *= a0_inv;
      90     1838062 :         b2 *= a0_inv;
      91     1838062 :         a1 *= a0_inv;
      92     1838062 :         a2 *= a0_inv;
      93             : 
      94     1838062 :         _center_freq_hz = new_center_freq;
      95     1838062 :         _sample_freq_hz = sample_freq_hz;
      96     1838062 :         _A = A;
      97     1838062 :         initialised = true;
      98             :     } else {
      99             :         // leave center_freq_hz at last value
     100           1 :         initialised = false;
     101             :     }
     102             : }
     103             : 
     104             : /*
     105             :   apply a new input sample, returning new output
     106             :  */
     107             : template <class T>
     108  5320058251 : T NotchFilter<T>::apply(const T &sample)
     109             : {
     110  5320058251 :     if (!initialised || need_reset) {
     111             :         // if we have not been initialised when return the input
     112             :         // sample as output and update delayed samples
     113   512064326 :         signal1 = sample;
     114   512064326 :         signal2 = sample;
     115   512064326 :         ntchsig1 = sample;
     116   512064326 :         ntchsig2 = sample;
     117   512064326 :         need_reset = false;
     118   512064326 :         return sample;
     119             :     }
     120             : 
     121  4807993925 :     T output = sample*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2;
     122             : 
     123  4807993925 :     ntchsig2 = ntchsig1;
     124  4807993925 :     ntchsig1 = sample;
     125             : 
     126  4807993925 :     signal2 = signal1;
     127  4807993925 :     signal1 = output;
     128  4807993925 :     return output;
     129             : }
     130             : 
     131             : template <class T>
     132    17281207 : void NotchFilter<T>::reset()
     133             : {
     134    17281207 :     need_reset = true;
     135    17281207 : }
     136             : 
     137             : #if HAL_LOGGING_ENABLED
     138             : // return the frequency to log for the notch
     139             : template <class T>
     140       89876 : float NotchFilter<T>::logging_frequency() const
     141             : {
     142       89876 :     return initialised ? _center_freq_hz : AP_Logger::quiet_nanf();
     143             : }
     144             : #endif
     145             : 
     146             : /*
     147             :    instantiate template classes
     148             :  */
     149             : template class NotchFilter<float>;
     150             : template class NotchFilter<Vector2f>;
     151             : template class NotchFilter<Vector3f>;