diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
index 26bb798f44..f56faa70e2 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
@@ -14,6 +14,7 @@ class Util;
class Semaphore;
class GPIO;
class DigitalSource;
+class DSP;
class HALSITLCAN;
class HALSITLCANDriver;
} // namespace HALSITL
diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h
index cc21c097f4..9957cf66d2 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h
@@ -6,3 +6,4 @@
#include "UARTDriver.h"
#include "SITL_State.h"
#include "Semaphores.h"
+#include "DSP.h"
diff --git a/libraries/AP_HAL_SITL/DSP.cpp b/libraries/AP_HAL_SITL/DSP.cpp
new file mode 100644
index 0000000000..6974af108d
--- /dev/null
+++ b/libraries/AP_HAL_SITL/DSP.cpp
@@ -0,0 +1,192 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ *
+ * Code by Andy Piper
+ */
+
+#include
+
+#include "AP_HAL_SITL.h"
+#include
+#include
+#include "DSP.h"
+#include
+
+using namespace HALSITL;
+
+extern const AP_HAL::HAL& hal;
+
+// The algorithms originally came from betaflight but are now substantially modified based on theory and experiment.
+// https://holometer.fnal.gov/GH_FFT.pdf "Spectrum and spectral density estimation by the Discrete Fourier transform (DFT),
+// including a comprehensive list of window functions and some new flat-top windows." - Heinzel et. al is a great reference
+// for understanding the underlying theory although we do not use spectral density here since time resolution is equally
+// 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)
+{
+ DSP::FFTWindowStateSITL* fft = new DSP::FFTWindowStateSITL(window_size, sample_rate);
+ if (fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr) {
+ delete fft;
+ return nullptr;
+ }
+ return fft;
+}
+
+// start an FFT analysis
+void DSP::fft_start(AP_HAL::DSP::FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size)
+{
+ step_hanning((FFTWindowStateSITL*)state, samples, buffer_index, buffer_size);
+}
+
+// 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)
+{
+ FFTWindowStateSITL* fft = (FFTWindowStateSITL*)state;
+ step_fft(fft);
+ step_cmplx_mag(fft, start_bin, end_bin, harmonics, noise_att_cutoff);
+ return step_calc_frequencies(fft, start_bin, end_bin);
+}
+
+// create an instance of the FFT state machine
+DSP::FFTWindowStateSITL::FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate)
+ : AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate)
+{
+ if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr) {
+ gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate window for DSP");
+ return;
+ }
+
+ buf = new complexf[window_size];
+}
+
+DSP::FFTWindowStateSITL::~FFTWindowStateSITL()
+{
+ delete[] buf;
+}
+
+// step 1: filter the incoming samples through a Hanning window
+void DSP::step_hanning(FFTWindowStateSITL* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size)
+{
+ // 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);
+ mult_f32(&samples[buffer_index], &fft->_hanning_window[0], &fft->_freq_bins[0], ring_buf_idx);
+ if (buffer_index > 0) {
+ mult_f32(&samples[0], &fft->_hanning_window[ring_buf_idx], &fft->_freq_bins[ring_buf_idx], fft->_window_size - ring_buf_idx);
+ }
+}
+
+// step 2: performm an in-place FFT on the windowed data
+void DSP::step_fft(FFTWindowStateSITL* fft)
+{
+ for (uint16_t i = 0; i < fft->_window_size; i++) {
+ fft->buf[i] = complexf(fft->_freq_bins[i], 0);
+ }
+
+ calculate_fft(fft->buf, fft->_window_size);
+
+ for (uint16_t i = 0; i < fft->_bin_count; i++) {
+ fft->_freq_bins[i] = std::norm(fft->buf[i]);
+ }
+
+ // components at the nyquist frequency are real only
+ for (uint16_t i = 0, j = 0; i <= fft->_bin_count; i++, j += 2) {
+ fft->_rfft_data[j] = fft->buf[i].real();
+ fft->_rfft_data[j+1] = fft->buf[i].imag();
+ }
+}
+
+void DSP::mult_f32(const float* v1, const float* v2, float* vout, uint16_t len)
+{
+ for (uint16_t i = 0; i < len; i++) {
+ vout[i] = v1[i] * v2[i];
+ }
+}
+
+void DSP::vector_max_float(const float* vin, uint16_t len, float* maxValue, uint16_t* maxIndex) const
+{
+ *maxValue = vin[0];
+ *maxIndex = 0;
+ for (uint16_t i = 1; i < len; i++) {
+ if (vin[i] > *maxValue) {
+ *maxValue = vin[i];
+ *maxIndex = i;
+ }
+ }
+}
+
+void DSP::vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const
+{
+ for (uint16_t i = 0; i < len; i++) {
+ vout[i] = vin[i] * scale;
+ }
+}
+
+// simple integer log2
+static uint16_t fft_log2(uint16_t n)
+{
+ uint16_t k = n, i = 0;
+ while (k) {
+ k >>= 1;
+ i++;
+ }
+ return i - 1;
+}
+
+// calculate the in-place FFT of the input using the Cooley–Tukey algorithm
+// this is a translation of Ron Nicholson's version in http://www.nicholson.com/dsp.fft1.html
+void DSP::calculate_fft(complexf *samples, uint16_t fftlen)
+{
+ uint16_t m = fft_log2(fftlen);
+ // shuffle data using bit reversed addressing ***
+ for (uint16_t k = 0; k < fftlen; k++) {
+ // generate a bit reversed address for samples[k] ***
+ uint16_t ki = k, kr = 0;
+ for (uint16_t i=1; i<=m; i++) {
+ kr <<= 1; // left shift result kr by 1 bit
+ if (ki % 2 == 1) {
+ kr++;
+ }
+ ki >>= 1; // right shift temp ki by 1 bit
+ }
+ // swap data samples[k] to bit reversed address samples[kr]
+ if (kr > k) {
+ complexf t = samples[kr];
+ samples[kr] = samples[k];
+ samples[k] = t;
+ }
+ }
+
+ // do fft butterflys in place
+ uint16_t istep = 2;
+ while (istep <= fftlen) {// layers 2,4,8,16, ... ,n
+ uint16_t is2 = istep / 2;
+ uint16_t astep = fftlen / istep;
+ for (uint16_t km = 0; km < is2; km++) { // outer row loop
+ uint16_t a = km * astep; // twiddle angle index
+ complexf w(sinf(2 * M_PI * (a+(fftlen/4)) / fftlen), sinf(2 * M_PI * a / fftlen));
+ for (uint16_t ki = 0; ki <= (fftlen - istep); ki += istep) { // inner column loop
+ uint16_t i = km + ki;
+ uint16_t j = is2 + i;
+ complexf t = w * samples[j];
+ complexf q = samples[i];
+ samples[j] = q - t;
+ samples[i] = q + t;
+ }
+ }
+ istep <<= 1;
+ }
+}
diff --git a/libraries/AP_HAL_SITL/DSP.h b/libraries/AP_HAL_SITL/DSP.h
new file mode 100644
index 0000000000..dab8552d49
--- /dev/null
+++ b/libraries/AP_HAL_SITL/DSP.h
@@ -0,0 +1,55 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ *
+ * Code by Andy Piper
+ */
+#pragma once
+
+#include
+#include "AP_HAL_SITL.h"
+
+#include
+
+typedef std::complex complexf;
+
+// ChibiOS implementation of FFT analysis to run on STM32 processors
+class HALSITL::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;
+ // 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;
+
+ // STM32-based FFT state
+ class FFTWindowStateSITL : public AP_HAL::DSP::FFTWindowState {
+ friend class HALSITL::DSP;
+
+ protected:
+ FFTWindowStateSITL(uint16_t window_size, uint16_t sample_rate);
+ ~FFTWindowStateSITL();
+
+ private:
+ complexf* buf;
+ };
+
+private:
+ void step_hanning(FFTWindowStateSITL* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size);
+ void step_fft(FFTWindowStateSITL* fft);
+ void mult_f32(const float* v1, const float* v2, float* vout, uint16_t len);
+ void vector_max_float(const float* vin, uint16_t len, float* maxValue, uint16_t* maxIndex) const override;
+ void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const override;
+ void calculate_fft(complexf* f, uint16_t length);
+};
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
index fc2c4a7511..6881a0beb1 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -19,6 +19,7 @@
#include "GPIO.h"
#include "SITL_State.h"
#include "Util.h"
+#include "DSP.h"
#include
#include
@@ -35,6 +36,8 @@ static RCInput sitlRCInput(&sitlState);
static RCOutput sitlRCOutput(&sitlState);
static AnalogIn sitlAnalogIn(&sitlState);
static GPIO sitlGPIO(&sitlState);
+static DSP dspDriver;
+
// use the Empty HAL for hardware we don't emulate
static Empty::I2CDeviceManager i2c_mgr_instance;
@@ -73,8 +76,9 @@ HAL_SITL::HAL_SITL() :
&sitlRCOutput, /* rcoutput */
&sitlScheduler, /* scheduler */
&utilInstance, /* util */
- &emptyOpticalFlow, /* onboard optical flow */
- &emptyFlash, /* flash driver */
+ &emptyOpticalFlow, /* onboard optical flow */
+ &emptyFlash, /* flash driver */
+ &dspDriver, /* dsp driver */
nullptr), /* CAN */
_sitl_state(&sitlState)
{}