Filter: allow for expansion of dynamic filters

this allows for the number of dynamic filters on a harmonic notch
filter to expand at runtime, which allows for ESC RPMs to be populated
from other than AP_Motors, such as with lua scripts or for fwd motors
in a SLT quadplane

libraries/Filter/HarmonicNotchFilter.cpp

@@ -159,6 +159,37 @@ void HarmonicNotchFilter<T>::allocate_filters(uint8_t num_notches, uint8_t harmo
     }
 }
 
+/*
+  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)
+{
+    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;
+    }
+    /*
+      note that we rely on the semaphore in
+      AP_InertialSensor_Backend.cpp to make this thread safe
+     */
+    auto filters = new NotchFilter<T>[num_filters];
+    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;
+    delete[] _old_filters;
+}
+
 /*
   update the underlying filters' center frequency using the current attenuation and quality
   this function is cheaper than init() because A & Q do not need to be recalculated
@@ -216,6 +247,11 @@ 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) {
+        // alloc realloc of filters
+        expand_filter_count(num_centers);
+    }
+
     _num_enabled_filters = 0;
 
     // update all of the filters using the new center frequencies and existing A & Q

libraries/Filter/HarmonicNotchFilter.h

@@ -31,6 +31,8 @@ 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);
+    // expand filter bank with new filters
+    void expand_filter_count(uint8_t num_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
@@ -64,6 +66,9 @@ private:
     // number of enabled filters
     uint8_t _num_enabled_filters;
     bool _initialised;
+
+    // have we failed to expand filters?
+    bool _alloc_has_failed;
 };
 
 // Harmonic notch update mode