aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: save a sliding window of frequency bins

Browse Source
This commit is contained in:
Andy Piper 2022-03-12 14:46:10 +00:00 committed by Peter Hall
parent 684cf996e1
commit 58b04db242
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_HAL_ChibiOS/DSP.cpp
View File

@ -50,9 +50,9 @@ extern const AP_HAL::HAL& hal;
// important as frequency resolution. Referred to as [Heinz] throughout the code.
// initialize the FFT state machine
AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate)
AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size)
{
DSP::FFTWindowStateARM* fft = new DSP::FFTWindowStateARM(window_size, sample_rate);
DSP::FFTWindowStateARM* fft = new DSP::FFTWindowStateARM(window_size, sample_rate, sliding_window_size);
if (fft == nullptr || fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr || fft->_derivative_freq_bins == nullptr) {
delete fft;
return nullptr;
@ -78,8 +78,8 @@ uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin
}
// create an instance of the FFT state machine
DSP::FFTWindowStateARM::FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate)
: AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate)
DSP::FFTWindowStateARM::FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size)
: AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate, sliding_window_size)
{
if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for window %u for DSP",
@ -130,6 +130,7 @@ extern "C" {
void DSP::step_hanning(FFTWindowStateARM* fft, FloatBuffer& samples, uint16_t advance)
{
TIMER_START(_hanning_timer);
// 5us
// apply hanning window to gyro samples and store result in _freq_bins
// hanning starts and ends with 0, could be skipped for minor speed improvement
@ -212,6 +213,7 @@ void DSP::step_stage_rfft_f32(FFTWindowStateARM* fft)
void DSP::step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff)
{
TIMER_START(_arm_cmplx_mag_f32_timer);
// 8us (BF)
// 32 - 4us F7, 5us F4, 5us H7
// 64 - 7us F7, 13us F4

4
libraries/AP_HAL_ChibiOS/DSP.h
View File

@ -29,7 +29,7 @@
class ChibiOS::DSP : public AP_HAL::DSP {
public:
// initialise an FFT instance
virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate) override;
virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size) override;
// start an FFT analysis with an ObjectBuffer
virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) override;
// perform remaining steps of an FFT analysis
@ -39,7 +39,7 @@ public:
class FFTWindowStateARM : public AP_HAL::DSP::FFTWindowState {
friend class ChibiOS::DSP;
public:
FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate);
FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate, uint8_t sliding_window_size);
virtual ~FFTWindowStateARM();
private: