From d4024d92164f719442fdb893636ce2975a63e671 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 13 Nov 2021 22:40:24 +0000 Subject: [PATCH] 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 | 19 +++++++++++++++++++ libraries/AP_HAL/DSP.cpp | 5 ++--- libraries/AP_HAL/DSP.h | 6 ++---- .../AP_HAL/examples/DSP_test/DSP_test.cpp | 4 ++-- 4 files changed, 25 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index dd782dca33..473bc8a48a 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/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 diff --git a/libraries/AP_HAL/DSP.cpp b/libraries/AP_HAL/DSP.cpp index 0c2e268666..e3bb320bdc 100644 --- a/libraries/AP_HAL/DSP.cpp +++ b/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); diff --git a/libraries/AP_HAL/DSP.h b/libraries/AP_HAL/DSP.h index 45afb2597a..92310e8836 100644 --- a/libraries/AP_HAL/DSP.h +++ b/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 diff --git a/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp b/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp index aadaf66855..92104c4040 100644 --- a/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp +++ b/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++) {