Line data    Source code

       1             : #include "AP_Filter_config.h"
       2             : 
       3             : #if AP_FILTER_ENABLED
       4             : 
       5             : #include "AP_Filter.h"
       6             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       7             : #include <AP_BoardConfig/AP_BoardConfig.h>
       8             : 
       9             : const AP_Param::GroupInfo AP_NotchFilter_params::var_info[] = {
      10             :   
      11             :     // @Param: NOTCH_FREQ
      12             :     // @DisplayName: Notch Filter center frequency
      13             :     // @Description: Notch Filter center frequency in Hz.
      14             :     // @Range: 10 495
      15             :     // @Units: Hz
      16             :     // @User: Advanced
      17             :     AP_GROUPINFO("NOTCH_FREQ", 1, AP_NotchFilter_params, _center_freq_hz, 0),
      18             : 
      19             :     // @Param: NOTCH_Q
      20             :     // @DisplayName: Notch Filter quality factor
      21             :     // @Description: Notch Filter quality factor given by the notch centre frequency divided by its bandwidth.
      22             :     // @Range: 1 10
      23             :     // @User: Advanced
      24             :     AP_GROUPINFO("NOTCH_Q", 2, AP_NotchFilter_params, _quality, 2),
      25             : 
      26             :     // @Param: NOTCH_ATT
      27             :     // @DisplayName: Notch Filter attenuation
      28             :     // @Description: Notch Filter attenuation in dB.
      29             :     // @Range: 5 50
      30             :     // @Units: dB
      31             :     // @User: Advanced
      32             :     AP_GROUPINFO("NOTCH_ATT", 3, AP_NotchFilter_params, _attenuation_dB, 40),
      33             : 
      34             :     AP_GROUPEND
      35             : };
      36             : 
      37          17 : AP_NotchFilter_params::AP_NotchFilter_params() : AP_Filter(AP_Filter::FilterType::FILTER_NOTCH)
      38             : {
      39          17 :     AP_Param::setup_object_defaults(this, var_info);
      40          17 : }
      41             : 
      42         720 : bool AP_NotchFilter_params::setup_notch_filter(NotchFilterFloat& filter, float sample_rate)
      43             : {
      44         720 :     if (is_zero(_quality) || is_zero(_center_freq_hz) || is_zero(_attenuation_dB)) {
      45           0 :         return false;
      46             :     }
      47             : 
      48         720 :     if (!is_equal(sample_rate, filter.sample_freq_hz())
      49         720 :         || !is_equal(_center_freq_hz.get(), filter.center_freq_hz())) {
      50         720 :         filter.init(sample_rate, _center_freq_hz, _center_freq_hz / _quality, _attenuation_dB);
      51             :     }
      52         720 :     return true;
      53             : }
      54             : 
      55             : #endif // AP_FILTER_ENABLED