AP_HAL: allow configuration of maximum number of notches based on MCU type

increase notch filters in SITL
remove redundant harmonics from DSP

libraries/AP_HAL/AP_HAL_Boards.h

@@ -272,3 +272,22 @@
 #ifndef HAL_WITH_MCU_MONITORING
 #define HAL_WITH_MCU_MONITORING defined(STM32H7)
 #endif
+
+#ifndef HAL_HNF_MAX_FILTERS
+// On an F7 The difference in CPU load between 1 notch and 24 notches is about 2%
+// The difference in CPU load between 1Khz backend and 2Khz backend is about 10%
+// So at 1Khz almost all notch combinations can be supported on F7 and certainly H7
+#if defined(STM32H7) || CONFIG_HAL_BOARD == HAL_BOARD_SITL
+// Enough for a double-notch per motor on an octa using three IMUs and one harmonics
+// plus one static notch with one double-notch harmonics
+#define HAL_HNF_MAX_FILTERS 54
+#elif defined(STM32F7)
+// Enough for a notch per motor on an octa using three IMUs and one harmonics
+// plus one static notch with one harmonics
+#define HAL_HNF_MAX_FILTERS 27
+#else
+// Enough for a notch per motor on an octa quad using two IMUs and one harmonic
+// plus one static notch with one harmonic
+#define HAL_HNF_MAX_FILTERS 18
+#endif
+#endif

libraries/AP_HAL/DSP.cpp

@@ -28,11 +28,10 @@ extern const AP_HAL::HAL &hal;
 #define SQRT_2_3 0.816496580927726f
 #define SQRT_6   2.449489742783178f
 
-DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics)
+DSP::FFTWindowState::FFTWindowState(uint16_t window_size, uint16_t sample_rate)
     : _window_size(window_size),
     _bin_count(window_size / 2),
-    _bin_resolution((float)sample_rate / (float)window_size),
-    _harmonics(harmonics)
+    _bin_resolution((float)sample_rate / (float)window_size)
 {
     // includes DC ad Nyquist components and needs to be large enough for intermediate steps
     _freq_bins = (float*)hal.util->malloc_type(sizeof(float) * (window_size), DSP_MEM_REGION);

libraries/AP_HAL/DSP.h

@@ -55,8 +55,6 @@ public:
         const uint16_t _bin_count;
         // size of the FFT window
         const uint16_t _window_size;
-        // number of harmonics of interest
-        const uint8_t _harmonics;
         // FFT data
         float* _freq_bins;
         // derivative real data scratch space
@@ -71,10 +69,10 @@ public:
         float _window_scale;
 
         virtual ~FFTWindowState();
-        FFTWindowState(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics);
+        FFTWindowState(uint16_t window_size, uint16_t sample_rate);
     };
     // initialise an FFT instance
-    virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) = 0;
+    virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate) = 0;
     // start an FFT analysis with an ObjectBuffer
     virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) = 0;
     // perform remaining steps of an FFT analysis

libraries/AP_HAL/examples/DSP_test/DSP_test.cpp

@@ -39,7 +39,7 @@ public:
 
 class DSPTest : public AP_HAL::DSP {
 public:
-    virtual FFTWindowState* fft_init(uint16_t w, uint16_t sample_rate, uint8_t harmonics) override { return nullptr; }
+    virtual FFTWindowState* fft_init(uint16_t w, uint16_t sample_rate) override { return nullptr; }
     virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) override {}
     virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) override { return 0; }
 protected:
@@ -65,7 +65,7 @@ void setup()
     hal.console->printf("DSP test\n");
     board_config.init();   
     serial_manager.init();
-    fft = hal.dsp->fft_init(WINDOW_SIZE, SAMPLE_RATE, 3);
+    fft = hal.dsp->fft_init(WINDOW_SIZE, SAMPLE_RATE);
     attenuation_cutoff = powf(10.0f, -attenuation_power_db / 10.0f);
 
     for(uint16_t i = 0; i < WINDOW_SIZE; i++) {