diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 3047cafd45..82ae789ba5 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -22,7 +22,6 @@ #include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters -#define HNF_MAX_HARMONICS 8 // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -61,7 +60,22 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: HMNCS // @DisplayName: Harmonic Notch Filter harmonics // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. A value of 0 disables this filter. The first harmonic refers to the base frequency. - // @Bitmask: 0:1st harmonic,1:2nd harmonic,2:3rd harmonic,3:4th hamronic,4:5th harmonic,5:6th harmonic,6:7th harmonic,7:8th harmonic + // @Bitmask: 0: 1st harmonic + // @Bitmask: 1: 2nd harmonic + // @Bitmask: 2: 3rd harmonic + // @Bitmask: 3: 4th harmonic + // @Bitmask: 4: 5th harmonic + // @Bitmask: 5: 6th harmonic + // @Bitmask: 6: 7th harmonic + // @Bitmask: 7: 8th harmonic + // @Bitmask: 8: 9th harmonic + // @Bitmask: 9: 10th harmonic + // @Bitmask: 10: 11th harmonic + // @Bitmask: 11: 12th harmonic + // @Bitmask: 12: 13th harmonic + // @Bitmask: 13: 14th harmonic + // @Bitmask: 14: 15th harmonic + // @Bitmask: 15: 16th harmonic // @User: Advanced // @RebootRequired: True AP_GROUPINFO("HMNCS", 5, HarmonicNotchFilterParams, _harmonics, 3), @@ -143,7 +157,7 @@ void HarmonicNotchFilter::init(float sample_freq_hz, float center_freq_hz, fl allocate a collection of, at most HNF_MAX_FILTERS, notch filters to be managed by this harmonic notch filter */ template -void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_notches) +void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches) { _composite_notches = MIN(composite_notches, 3); _num_harmonics = __builtin_popcount(harmonics); @@ -327,6 +341,11 @@ HarmonicNotchFilterParams::HarmonicNotchFilterParams(void) AP_Param::setup_object_defaults(this, var_info); } +void HarmonicNotchFilterParams::init() +{ + _harmonics.convert_parameter_width(AP_PARAM_INT8); +} + /* save changed parameters */ diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 064365f0b2..7de4df799d 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -19,7 +19,7 @@ #include #include "NotchFilter.h" -#define HNF_MAX_HARMONICS 8 +#define HNF_MAX_HARMONICS 16 /* a filter that manages a set of notch filters targetted at a fundamental center frequency @@ -30,7 +30,7 @@ class HarmonicNotchFilter { public: ~HarmonicNotchFilter(); // allocate a bank of notch filters for this harmonic notch filter - void allocate_filters(uint8_t num_notches, uint8_t harmonics, uint8_t composite_notches); + void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches); // expand filter bank with new filters void expand_filter_count(uint8_t num_notches); // initialize the underlying filters using the provided filter parameters @@ -56,7 +56,7 @@ private: // quality factor of each filter float _Q; // a bitmask of the harmonics to use - uint8_t _harmonics; + uint32_t _harmonics; // number of notches that make up a composite notch uint8_t _composite_notches; // number of allocated filters @@ -95,16 +95,19 @@ public: }; HarmonicNotchFilterParams(void); + + void init(); + // set the fundamental center frequency of the harmonic notch void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); } // set the bandwidth of the harmonic notch void set_bandwidth_hz(float bandwidth_hz) { _bandwidth_hz.set(bandwidth_hz); } // harmonics enabled on the harmonic notch - uint8_t harmonics(void) const { return _harmonics; } + uint32_t harmonics(void) const { return _harmonics; } // set the harmonics value - void set_harmonics(uint8_t hmncs) { _harmonics.set(hmncs); } + void set_harmonics(uint32_t hmncs) { _harmonics.set(hmncs); } // has the user set the harmonics value - void set_default_harmonics(uint8_t hmncs) { _harmonics.set_default(hmncs); } + void set_default_harmonics(uint32_t hmncs) { _harmonics.set_default(hmncs); } // reference value of the harmonic notch float reference(void) const { return _reference; } void set_reference(float ref) { _reference.set(ref); } @@ -125,7 +128,7 @@ public: private: // configured notch harmonics - AP_Int8 _harmonics; + AP_Int32 _harmonics; // notch reference value AP_Float _reference; // notch dynamic tracking mode