Filter: fixed uint8_t bug in total filters and expand_filter_count bug

this gets the right number of notches on quadplanes, but is still very
bad in fwd flight

libraries/Filter/HarmonicNotchFilter.cpp

@@ -189,13 +189,8 @@ void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint32_t harm
   expand the number of filters at runtime, allowing for RPM sources such as lua scripts
  */
 template <class T>
-void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
+void HarmonicNotchFilter<T>::expand_filter_count(uint16_t total_notches)
 {
-    uint8_t num_filters = _num_harmonics * num_notches * _composite_notches;
-    if (num_filters <= _num_filters) {
-        // enough already
-        return;
-    }
     if (_alloc_has_failed) {
         // we've failed to allocate before, don't try again
         return;
@@ -204,15 +199,15 @@ void HarmonicNotchFilter<T>::expand_filter_count(uint8_t num_notches)
       note that we rely on the semaphore in
       AP_InertialSensor_Backend.cpp to make this thread safe
      */
-    auto filters = new NotchFilter<T>[num_filters];
+    auto filters = new NotchFilter<T>[total_notches];
     if (filters == nullptr) {
         _alloc_has_failed = true;
         return;
     }
     memcpy(filters, _filters, sizeof(filters[0])*_num_filters);
     auto _old_filters = _filters;
     _filters = filters;
-    _num_filters = num_filters;
+    _num_filters = total_notches;
     delete[] _old_filters;
 }
 
@@ -273,15 +268,16 @@ void HarmonicNotchFilter<T>::update(uint8_t num_centers, const float center_freq
     // adjust the frequencies to be in the allowable range
     const float nyquist_limit = _sample_freq_hz * 0.48f;
 
-    if (num_centers > _num_filters) {
+    uint16_t total_notches = num_centers * _num_harmonics * _composite_notches;
+    if (total_notches > _num_filters) {
         // alloc realloc of filters
-        expand_filter_count(num_centers);
+        expand_filter_count(total_notches);
     }
 
     _num_enabled_filters = 0;
 
     // update all of the filters using the new center frequencies and existing A & Q
-    for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
+    for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) {
         const uint8_t harmonic_n = i / num_centers;
         const uint8_t center_n = i % num_centers;
         // the filters are ordered by center and then harmonic so
@@ -331,7 +327,7 @@ T HarmonicNotchFilter<T>::apply(const T &sample)
 #endif
 
     T output = sample;
-    for (uint8_t i = 0; i < _num_enabled_filters; i++) {
+    for (uint16_t i = 0; i < _num_enabled_filters; i++) {
 #if NOTCH_DEBUG_LOGGING
         if (!_filters[i].initialised) {
             ::dprintf(dfd, "------- ");
@@ -359,7 +355,7 @@ void HarmonicNotchFilter<T>::reset()
         return;
     }
 
-    for (uint8_t i = 0; i < _num_filters; i++) {
+    for (uint16_t i = 0; i < _num_filters; i++) {
         _filters[i].reset();
     }
 }

libraries/Filter/HarmonicNotchFilter.h

@@ -32,7 +32,7 @@ class HarmonicNotchFilter {
     // allocate a bank of notch filters for this harmonic notch filter
     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);
+    void expand_filter_count(uint16_t total_notches);
     // initialize the underlying filters using the provided filter parameters
     void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
     // update the underlying filters' center frequencies using center_freq_hz as the fundamental
@@ -60,11 +60,11 @@ class HarmonicNotchFilter {
     // number of notches that make up a composite notch
     uint8_t _composite_notches;
     // number of allocated filters
-    uint8_t _num_filters;
+    uint16_t _num_filters;
     // pre-calculated number of harmonics
     uint8_t _num_harmonics;
     // number of enabled filters
-    uint8_t _num_enabled_filters;
+    uint16_t _num_enabled_filters;
     bool _initialised;
 
     // have we failed to expand filters?