AP_HAL_ChibiOS: add harmonics to DSP

add vector_mean_float() to DSP
allow fft_start() to use ObjectBuffer<float> for lock-free access

libraries/AP_HAL_ChibiOS/DSP.cpp

@@ -49,10 +49,10 @@ 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 harmonics)
 {
-    DSP::FFTWindowStateARM* fft = new DSP::FFTWindowStateARM(window_size, sample_rate);
-    if (fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr) {
+    DSP::FFTWindowStateARM* fft = new DSP::FFTWindowStateARM(window_size, sample_rate, harmonics);
+    if (fft == nullptr || fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr || fft->_derivative_freq_bins == nullptr) {
         delete fft;
         return nullptr;
     }
@@ -60,28 +60,28 @@ AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample
 }
 
 // start an FFT analysis
-void DSP::fft_start(AP_HAL::DSP::FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size)
+void DSP::fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance)
 {
-    step_hanning((FFTWindowStateARM*)state, samples, buffer_index, buffer_size);
+    step_hanning((FFTWindowStateARM*)state, samples, advance);
 }
 
 // perform remaining steps of an FFT analysis
-uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff)
+uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff)
 {
     FFTWindowStateARM* fft = (FFTWindowStateARM*)state;
     step_arm_cfft_f32(fft);
     step_bitreversal(fft);
     step_stage_rfft_f32(fft);
-    step_arm_cmplx_mag_f32(fft, start_bin, end_bin, harmonics, noise_att_cutoff);
+    step_arm_cmplx_mag_f32(fft, start_bin, end_bin, noise_att_cutoff);
     return step_calc_frequencies_f32(fft, start_bin, end_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 harmonics)
+    : AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate, harmonics)
 {
-    if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for window %u for DSP",
+    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",
             unsigned(sizeof(float) * (window_size * 3 + 2)), unsigned(window_size));
         return;
     }
@@ -115,9 +115,7 @@ DSP::FFTWindowStateARM::FFTWindowStateARM(uint16_t window_size, uint16_t sample_
     }
 }
 
-DSP::FFTWindowStateARM::~FFTWindowStateARM()
-{
-}
+DSP::FFTWindowStateARM::~FFTWindowStateARM() {}
 
 extern "C" {
     void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
@@ -128,17 +126,15 @@ extern "C" {
 }
 
 // step 1: filter the incoming samples through a Hanning window
-void DSP::step_hanning(FFTWindowStateARM* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size)
+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
-    const uint16_t ring_buf_idx = MIN(buffer_size - buffer_index, fft->_window_size);
-    arm_mult_f32(&samples[buffer_index], &fft->_hanning_window[0], &fft->_freq_bins[0], ring_buf_idx);
-    if (buffer_index > 0) {
-        arm_mult_f32(&samples[0], &fft->_hanning_window[ring_buf_idx], &fft->_freq_bins[ring_buf_idx], fft->_window_size - ring_buf_idx);
-    }
+    samples.peek(&fft->_freq_bins[0], fft->_window_size); // the caller ensures we get a full buffer of samples
+    samples.advance(advance);
+    arm_mult_f32(&fft->_freq_bins[0], &fft->_hanning_window[0], &fft->_freq_bins[0], fft->_window_size);
 
     TIMER_END(_hanning_timer);
 }
@@ -212,7 +208,7 @@ void DSP::step_stage_rfft_f32(FFTWindowStateARM* fft)
 }
 
 // step 5: find the magnitudes of the complex data
-void DSP::step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff)
+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)
@@ -231,7 +227,7 @@ void DSP::step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uin
     fft->_rfft_data[fft->_window_size] = fft->_rfft_data[1]; // Nyquist for the interpolator
     fft->_rfft_data[fft->_window_size + 1] = 0;
 
-    step_cmplx_mag(fft, start_bin, end_bin, harmonics, noise_att_cutoff);
+    step_cmplx_mag(fft, start_bin, end_bin, noise_att_cutoff);
 
     TIMER_END(_arm_cmplx_mag_f32_timer);
 }
@@ -250,12 +246,12 @@ uint16_t DSP::step_calc_frequencies_f32(FFTWindowStateARM* fft, uint16_t start_b
     _output_count++;
     // outputs at approx 1hz
     if (_output_count % 400 == 0) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "FFT(us): t1:%lu,t2:%lu,t3:%lu,t4:%lu,t5:%lu,t6:%lu",
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT(us): t1:%lu,t2:%lu,t3:%lu,t4:%lu,t5:%lu,t6:%lu",
                         _hanning_timer._timer_avg, _arm_cfft_f32_timer._timer_avg, _bitreversal_timer._timer_avg, _stage_rfft_f32_timer._timer_avg, _arm_cmplx_mag_f32_timer._timer_avg, _step_calc_frequencies._timer_avg);
     }
 #endif
 
-    return fft->_max_energy_bin;
+    return fft->_peak_data[CENTER]._bin;
 }
 
 static const float PI_N = M_PI / 32.0f;

libraries/AP_HAL_ChibiOS/DSP.h

@@ -29,19 +29,18 @@
 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;
-    // start an FFT analysis
-    virtual void fft_start(FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size) override;
+    virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) 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
-    virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) override;
+    virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) override;
 
     // STM32-based FFT state
     class FFTWindowStateARM : public AP_HAL::DSP::FFTWindowState {
         friend class ChibiOS::DSP;
-
-    protected:
-        FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate);
-        ~FFTWindowStateARM();
+    public:
+        FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics);
+        virtual ~FFTWindowStateARM();
 
     private:
         // underlying CMSIS data structure for FFT analysis
@@ -57,14 +56,19 @@ protected:
     void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const override {
         arm_scale_f32(vin, scale, vout, len);
     }
+    float vector_mean_float(const float* vin, uint16_t len) const override {
+        float mean_value;
+        arm_mean_f32(vin, len, &mean_value);
+        return mean_value;
+    }
 
 private:
     // following are the six independent steps for calculating an FFT
-    void step_hanning(FFTWindowStateARM* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size);
+    void step_hanning(FFTWindowStateARM* fft, FloatBuffer& samples, uint16_t advance);
     void step_arm_cfft_f32(FFTWindowStateARM* fft);
     void step_bitreversal(FFTWindowStateARM* fft);
     void step_stage_rfft_f32(FFTWindowStateARM* fft);
-    void step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff);
+    void step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff);
     uint16_t step_calc_frequencies_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin);
     // candan's frequency interpolator
     float calculate_candans_estimator(const FFTWindowStateARM* fft, uint16_t k) const;