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             : #include <AP_Math/AP_Math.h>
      18             : #include <cmath>
      19             : #include <AP_Param/AP_Param.h>
      20             : #include "NotchFilter.h"
      21             : 
      22             : #define HNF_MAX_HARMONICS 16
      23             : 
      24             : class HarmonicNotchFilterParams;
      25             : 
      26             : /*
      27             :   a filter that manages a set of notch filters targetted at a fundamental center frequency
      28             :   and multiples of that fundamental frequency
      29             :  */
      30             : template <class T>
      31             : class HarmonicNotchFilter {
      32             : public:
      33             :     ~HarmonicNotchFilter();
      34             :     // allocate a bank of notch filters for this harmonic notch filter
      35             :     void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches);
      36             :     // expand filter bank with new filters
      37             :     void expand_filter_count(uint16_t total_notches);
      38             :     // initialize the underlying filters using the provided filter parameters
      39             :     void init(float sample_freq_hz, HarmonicNotchFilterParams &params);
      40             :     // update the underlying filters' center frequencies using center_freq_hz as the fundamental
      41             :     void update(float center_freq_hz);
      42             :     // update all of the underlying center frequencies individually
      43             :     void update(uint8_t num_centers, const float center_freq_hz[]);
      44             : 
      45             :     /*
      46             :       set center frequency of one notch.
      47             :       spread_mul is a scale factor for spreading of double or triple notch
      48             :       harmonic_mul is the multiplier for harmonics, 1 is for the fundamental
      49             :     */
      50             :     void set_center_frequency(uint16_t idx, float center_freq_hz, float spread_mul, uint8_t harmonic_mul);
      51             : 
      52             :     // apply a sample to each of the underlying filters in turn
      53             :     T apply(const T &sample);
      54             :     // reset each of the underlying filters
      55             :     void reset();
      56             : 
      57             :     /*
      58             :       log notch center frequencies and first harmonic
      59             :      */
      60             :     void log_notch_centers(uint8_t instance, uint64_t now_us) const;
      61             : 
      62             : private:
      63             :     // underlying bank of notch filters
      64             :     NotchFilter<T>*  _filters;
      65             :     // sample frequency for each filter
      66             :     float _sample_freq_hz;
      67             :     // base double notch bandwidth for each filter
      68             :     float _notch_spread;
      69             :     // attenuation for each filter
      70             :     float _A;
      71             :     // quality factor of each filter
      72             :     float _Q;
      73             :     // a bitmask of the harmonics to use
      74             :     uint32_t _harmonics;
      75             :     // number of notches that make up a composite notch
      76             :     uint8_t _composite_notches;
      77             :     // number of allocated filters
      78             :     uint16_t _num_filters;
      79             :     // pre-calculated number of harmonics
      80             :     uint8_t _num_harmonics;
      81             :     // number of enabled filters
      82             :     uint16_t _num_enabled_filters;
      83             :     bool _initialised;
      84             : 
      85             :     // have we failed to expand filters?
      86             :     bool _alloc_has_failed;
      87             : 
      88             :     // calculate the number of notch filters needed for the given config,
      89             :     uint16_t notch_count(uint8_t num_sources, uint8_t num_harmonics, uint8_t composite_notches) const;
      90             : 
      91             :     // minimum frequency (from INS_HNTCH_FREQ * INS_HNTCH_FM_RAT)
      92             :     float _minimum_freq;
      93             : 
      94             :     // pointer to params object for this filter
      95             :     HarmonicNotchFilterParams *params;
      96             : };
      97             : 
      98             : // Harmonic notch update mode
      99             : enum class HarmonicNotchDynamicMode {
     100             :     Fixed           = 0,
     101             :     UpdateThrottle  = 1,
     102             :     UpdateRPM       = 2,
     103             :     UpdateBLHeli    = 3,
     104             :     UpdateGyroFFT   = 4,
     105             :     UpdateRPM2      = 5,
     106             : };
     107             : 
     108             : /*
     109             :   harmonic notch filter configuration parameters
     110             :  */
     111             : class HarmonicNotchFilterParams : public NotchFilterParams {
     112             : public:
     113             :     enum class Options {
     114             :         DoubleNotch = 1<<0,
     115             :         DynamicHarmonic = 1<<1,
     116             :         LoopRateUpdate = 1<<2,
     117             :         EnableOnAllIMUs = 1<<3,
     118             :         TripleNotch = 1<<4,
     119             :         TreatLowAsMin = 1<<5,
     120             :         QuintupleNotch = 1<<6,
     121             :     };
     122             : 
     123             :     HarmonicNotchFilterParams(void);
     124             : 
     125             :     void init();
     126             : 
     127             :     // set the fundamental center frequency of the harmonic notch
     128       27204 :     void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); }
     129             : 
     130             :     // set the bandwidth of the harmonic notch
     131       27204 :     void set_bandwidth_hz(float bandwidth_hz) { _bandwidth_hz.set(bandwidth_hz); }
     132             : 
     133             :     // set the attenuation of the harmonic notch
     134       27202 :     void set_attenuation(float attenuation_dB) { _attenuation_dB.set(attenuation_dB); }
     135             :     
     136             :     // harmonics enabled on the harmonic notch
     137         231 :     uint32_t harmonics(void) const { return _harmonics; }
     138             : 
     139             :     // set the harmonics value
     140           2 :     void set_harmonics(uint32_t hmncs) { _harmonics.set(hmncs); }
     141             : 
     142             :     // has the user set the harmonics value
     143          12 :     void set_default_harmonics(uint32_t hmncs) { _harmonics.set_default(hmncs); }
     144             : 
     145             :     // reference value of the harmonic notch
     146      493703 :     float reference(void) const { return _reference; }
     147           2 :     void set_reference(float ref) { _reference.set(ref); }
     148             : 
     149             :     // notch options
     150  4482804749 :     bool hasOption(Options option) const { return _options & uint16_t(option); }
     151             :     // notch dynamic tracking mode
     152     2786473 :     HarmonicNotchDynamicMode tracking_mode(void) const { return HarmonicNotchDynamicMode(_tracking_mode.get()); }
     153             :     static const struct AP_Param::GroupInfo var_info[];
     154             : 
     155             :     // return minimum frequency ratio for throttle notch
     156      270388 :     float freq_min_ratio(void) const {
     157      270388 :         return _freq_min_ratio;
     158             :     }
     159       27204 :     void set_freq_min_ratio(float ratio) { _freq_min_ratio.set(ratio); }
     160             : 
     161             :     // set options flags
     162       27202 :     void set_options(uint16_t options) { _options.set(options); }
     163             : 
     164             :     // save parameters
     165             :     void save_params();
     166             : 
     167             :     // return the number of composite notches given the options
     168             :     uint8_t num_composite_notches(void) const;
     169             : 
     170             : private:
     171             :     // configured notch harmonics
     172             :     AP_Int32 _harmonics;
     173             :     // notch reference value
     174             :     AP_Float _reference;
     175             :     // notch dynamic tracking mode
     176             :     AP_Int8 _tracking_mode;
     177             :     // notch options
     178             :     AP_Int16 _options;
     179             : 
     180             :     // minimum frequency ratio for throttle based notches
     181             :     AP_Float _freq_min_ratio;
     182             : };
     183             : 
     184             : typedef HarmonicNotchFilter<Vector3f> HarmonicNotchFilterVector3f;
     185             :