Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
increase notch filters in SITL
remove redundant harmonics from DSP
This commit is contained in:
Andy Piper 2021-11-13 22:40:24 +00:00 committed by Andrew Tridgell
parent d44478ff59
commit d4024d9216
4 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -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

5
libraries/AP_HAL/DSP.cpp
View File

@ -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);

6
libraries/AP_HAL/DSP.h
View File

@ -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

4
libraries/AP_HAL/examples/DSP_test/DSP_test.cpp
View File

@ -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++) {