From 58b04db242d7e107901a1018d53c6325148d1406 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 12 Mar 2022 14:46:10 +0000
Subject: [PATCH] AP_HAL_ChibiOS: save a sliding window of frequency bins

---
 libraries/AP_HAL_ChibiOS/DSP.cpp | 10 ++++++----
 libraries/AP_HAL_ChibiOS/DSP.h   |  4 ++--
 2 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/DSP.cpp b/libraries/AP_HAL_ChibiOS/DSP.cpp
index 44c73a0905..0fb0692e26 100644
--- a/libraries/AP_HAL_ChibiOS/DSP.cpp
+++ b/libraries/AP_HAL_ChibiOS/DSP.cpp
@@ -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
diff --git a/libraries/AP_HAL_ChibiOS/DSP.h b/libraries/AP_HAL_ChibiOS/DSP.h
index c5ed130b2a..8d839b5393 100644
--- a/libraries/AP_HAL_ChibiOS/DSP.h
+++ b/libraries/AP_HAL_ChibiOS/DSP.h
@@ -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: