works on stm, add DSP lib to mini_v3 ioc commit

Src/common/FFT.cpp

@@ -8,25 +8,19 @@
 #include <cstring>
 #include <cstdio>
 #include "FFT.hpp"
-#include "common/logging.hpp"
 
-Logging logger("FFT");
 bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) {
     if (window_size > FFT_MAX_SIZE) {
         return false;
     }
     n_axes = num_axes;
     bool buffers_allocated = AllocateBuffers(window_size);
     rfft_spec = init_rfft(&_hanning_window, &_fft_input_buffer,
-                          &_fft_output_buffer, window_size);
+                          &_fft_output_buffer, &window_size);
     size = window_size;
     _sample_rate_hz = sample_rate_hz;
     _resolution_hz =  sample_rate_hz / size;
     fft_max_freq = _sample_rate_hz / 2;
-    char buffer[100];
-    snprintf(buffer, sizeof(buffer), "_resolution_hz: %d, num_axes: %d, sample_rate_hz: %d",
-           (int)( _resolution_hz * 1000), n_axes, (int)(_sample_rate_hz * 1000));
-    logger.log_info(buffer);
     return buffers_allocated;
 }
 
@@ -40,22 +34,22 @@ void FFT::update(float *input) {
             // convert int16_t -> real_t (scaling isn't relevant)
             data_buffer[axis][buffer_index] = conv_input[axis] / 2;
             buffer_index++;
+            _fft_updated[axis] = false;
+
             continue;
         }
 
         apply_hanning_window(data_buffer[axis].data(), _fft_input_buffer,
                                 _hanning_window, size);
         rfft_one_cycle(rfft_spec, _fft_input_buffer, _fft_output_buffer);
-        _fft_updated = true;
         find_peaks(axis);
 
         // reset
         // shift buffer (3/4 overlap)
         const int overlap_start = size / 4;
         memmove(&data_buffer[axis][0], &data_buffer[axis][overlap_start],
                 sizeof(real_t) * overlap_start * 3);
-        buffer_index = overlap_start * 3;
-        _fft_updated = false;
+        _fft_buffer_index[axis] = overlap_start * 3;
     }
 }
 
@@ -81,20 +75,20 @@ void FFT::find_peaks(uint8_t axis) {
         float real_f, imag_f;
         convert_real_t_to_float(&real_imag[0], &real_f, 1);
         convert_real_t_to_float(&real_imag[1], &imag_f, 1);
-        const float fft_magnitude = sqrtf(real_f * real_f);
+        const float fft_magnitude = sqrtf(real_f * real_f + imag_f * imag_f);
         _peak_magnitudes_all[fft_index] = fft_magnitude;
         bin_mag_sum += fft_magnitude;
     }
-
+    // char buffer[50];
+    // snprintf(buffer, sizeof(buffer), "bin_mag_sum: %d", (int)bin_mag_sum);
+    // logger.log_info(buffer);
     for (uint8_t i = 0; i < MAX_NUM_PEAKS; i++) {
         float largest_peak = 0;
         uint16_t largest_peak_index = 0;
 
         // Identify i'th peak bin
         for (uint16_t bin_index = 1; bin_index < size/2; bin_index++) {
-            float frhz = float(_resolution_hz);
-            float fbi = float(bin_index);
-            float freq_hz = frhz * fbi;
+            float freq_hz = _resolution_hz * bin_index;
 
             if ((_peak_magnitudes_all[bin_index] > largest_peak)
                 && (freq_hz >= fft_min_freq)
@@ -118,19 +112,25 @@ void FFT::find_peaks(uint8_t axis) {
         if (raw_peak_index[peak_new] == 0) {
             continue;
         }
+        static uint16_t indx = 0;
+        indx++;
+        (void) indx;
         float adjusted_bin = 0.5f *
                         estimate_peak_freq(fft_output_buffer_float, 2 * raw_peak_index[peak_new]);
-        if (!std::isfinite(adjusted_bin)) {
+        if (adjusted_bin > size || adjusted_bin < 0) {
             continue;
         }
 
         float freq_adjusted = _resolution_hz * adjusted_bin;
         // snr is in dB
-        const float snr = 10.f * log10f((size - 1) * peak_magnitude[peak_new] /
-                        (bin_mag_sum - peak_magnitude[peak_new]));
-
-        if (!std::isfinite(freq_adjusted)
-            || (snr < MIN_SNR)
+        float snr;
+        if (bin_mag_sum - peak_magnitude[peak_new] < 1.0e-19f) {
+            snr = 0;
+        } else {
+            snr = 10.f * log10f((size - 1) * peak_magnitude[peak_new] /
+                            (bin_mag_sum - peak_magnitude[peak_new]));
+        }
+        if ((snr < MIN_SNR)
             || (freq_adjusted < fft_min_freq)
             || (freq_adjusted > fft_max_freq)) {
                 continue;
@@ -144,6 +144,8 @@ void FFT::find_peaks(uint8_t axis) {
             if (!peak_close && peak_frequencies_prev[peak_prev] > 0) {
                 continue;
             }
+            _fft_updated[axis] = true;
+
             // keep
             peak_frequencies[axis][num_peaks_found] = freq_adjusted;
             peak_snr[axis][num_peaks_found] = snr;
@@ -156,6 +158,13 @@ void FFT::find_peaks(uint8_t axis) {
             break;
         }
     }
+    float max_snr_peak = 0;
+    for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) {
+        if (peak_snr[axis][peak_index] > max_snr_peak) {
+            max_snr_peak = peak_frequencies[axis][peak_index];
+        }
+    }
+    peak_frequencies[axis][0] = max_snr_peak;
 }
 
 static constexpr float tau(float x) {
@@ -167,34 +176,41 @@ static constexpr float tau(float x) {
 }
 
 float FFT::estimate_peak_freq(float fft[], int peak_index) {
-    if (peak_index < 2) {
-        return NAN;
+    if (peak_index < 2 || peak_index >= size) {
+        return -1;
     }
 
     // find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/)
-    float real_imag[3][2];
+    // float real_imag[3][2];
     float real[3];
     float imag[3];
     for (int i = -1; i < 2; i++) {
-        get_real_imag_by_index(fft, real_imag[i + 1], size, peak_index + i);
-        real[i + 1] = real_imag[i + 1][0];
-        imag[i + 1] = real_imag[i + 1][1];
+        real[i + 1] = get_real_by_index(fft, peak_index + i);
+        imag[i + 1] = get_imag_by_index(fft, peak_index + i);
     }
 
     static constexpr int k = 1;
 
     const float divider = (real[k] * real[k] + imag[k] * imag[k]);
-
+    if (divider < 1.0e-19f) {
+        return -1;
+    }
     // ap = (X[k + 1].r * X[k].r + X[k+1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
     float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider;
 
     // dp = -ap / (1 – ap)
+    if (1.0f - ap < 1.0e-19f) {
+        return -1;
+    }
     float dp = -ap  / (1.f - ap);
 
     // am = (X[k - 1].r * X[k].r + X[k – 1].i * X[k].i) / (X[k].r * X[k].r + X[k].i * X[k].i)
     float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider;
 
     // dm = am / (1 – am)
+    if (1.0f - am < 1.0e-19f) {
+        return -1;
+    }
     float dm = am / (1.f - am);
 
     // d = (dp + dm) / 2 + tau(dp * dp) – tau(dm * dm)

Src/common/FFT.hpp

@@ -19,7 +19,7 @@ extern "C" {
 #define FFT_MAX_SIZE    2048
 #define MAX_NUM_PEAKS   3
 #define MIN_SNR         1.0f
-#define NUM_AXES        3
+#define MAX_NUM_AXES        3
 
 class FFT {
 public:
@@ -28,20 +28,20 @@ class FFT {
     float fft_min_freq = 0;
     float fft_max_freq;
     float _resolution_hz;
-    float peak_frequencies [NUM_AXES][MAX_NUM_PEAKS] {};
-    float peak_snr [NUM_AXES][MAX_NUM_PEAKS] {};
+    float peak_frequencies [MAX_NUM_AXES][MAX_NUM_PEAKS] {0};
+    float peak_snr [MAX_NUM_AXES][MAX_NUM_PEAKS] {0};
+    bool _fft_updated[MAX_NUM_AXES]{false};
+    uint32_t total_time;
 
 private:
     uint16_t size;
-
     real_t *_hanning_window{nullptr};
     real_t *_fft_output_buffer{nullptr};
     real_t *_fft_input_buffer{nullptr};
     std::vector<std::vector<real_t>> data_buffer;
     std::vector<float> _peak_magnitudes_all;
 
     uint16_t _fft_buffer_index[3] {};
-    bool _fft_updated{false};
     uint8_t n_axes;
     float _sample_rate_hz;
 
@@ -56,14 +56,14 @@ class FFT {
         return (data_buffer[0].data() && data_buffer[1].data() && data_buffer[2].data()
                 && _peak_magnitudes_all.data());
     }
-    // #ifdef HAL_MODULE_ENABLED
+    #ifdef HAL_MODULE_ENABLED
         // specification of arm_rfft_instance_q15
         // https://arm-software.github.io/CMSIS_5/DSP/html/group__RealFFT.html
         arm_rfft_instance_q15 rfft_spec;
-    // #else
+    #else
         // plan for the r2c transform from fftw3 library.
-        // fftw_plan rfft_spec;
-    // #endif
+        fftw_plan rfft_spec;
+    #endif
 };
 
 #ifdef __cplusplus

Src/drivers/mpu9250/mpu9250.cpp

@@ -17,7 +17,7 @@ enum class Mpu9250Resgiter : uint8_t {
 
 // Register Map for Magnetometer (AK8963)
 enum class MagnetometerResgiter : uint8_t {
-    REG_MAG_XOUT_L = 0x03,
+    REG_MAG_XOUT_L  = 0x03,
 };
 
 constexpr auto MPU9250_WHO_AM_I_ID = std::byte(0x71);   // REG_WHO_AM_I expected value

Src/drivers/mpu9250/mpu9250.hpp

@@ -34,6 +34,10 @@ class Mpu9250 {
      * @return 0 on success, negative otherwise
      */
     int8_t read_magnetometer(std::array<int16_t, 3>* mag) const;
+
+    int8_t set_accel_odr(uint8_t odr);
+
+    int8_t set_gyro_odr(uint8_t odr);
 private:
     bool initialized{false};
 };

Src/modules/imu/imu.cpp

@@ -16,10 +16,8 @@ void ImuModule::init() {
     uint16_t num_axes = 3;
     uint16_t window_size = 512;
     bool fft_initialized = fft.init(window_size, num_axes, 1000.0f/period_ms);
+    fft.fft_min_freq = 10;
     initialized = imu_initialized && fft_initialized;
-    // snprintf(buffer, sizeof(buffer), "initialized: %d %d", imu_initialized, fft_initialized);
-    // logger.log_info(buffer);
-    // logger.log_info("initialized");
 }
 
 void ImuModule::update_params() {
@@ -28,14 +26,6 @@ void ImuModule::update_params() {
     if (enabled) {
         mode = initialized ? Mode::STANDBY : Mode::INITIALIZATION;
     }
-    // static uint32_t prev_time = HAL_GetTick();
-    // if (prev_time + 1000 < HAL_GetTick()) {
-    //     prev_time = HAL_GetTick();
-    //     char buffer[90];
-    //     snprintf(buffer, sizeof(buffer), "enabled: %d %d %d", enabled, initialized,
-    //                 static_cast<int>(mode));
-    //     logger.log_info(buffer);
-    // }
 }
 
 void ImuModule::spin_once() {
@@ -48,55 +38,51 @@ void ImuModule::spin_once() {
         mag.publish();
     }
 
-    bool updated{false};
+    bool updated[2]{false, false};
 
     std::array<int16_t, 3> accel_raw = {0, 0, 0};
-    std::array<int16_t, 3> gyro_raw = {0, 0, 0};
+    std::array<int16_t, 3> gyro_raw  = {0, 0, 0};
 
-    static std::vector<float> gyro;
     if (imu.read_gyroscope(&gyro_raw) >= 0) {
-        gyro = {raw_gyro_to_rad_per_second(gyro_raw[0]),
+        float gyro[3] = {
+                raw_gyro_to_rad_per_second(gyro_raw[0]),
                 raw_gyro_to_rad_per_second(gyro_raw[1]),
                 raw_gyro_to_rad_per_second(gyro_raw[2])};
         pub.msg.rate_gyro_latest[0] = gyro[0];
         pub.msg.rate_gyro_latest[1] = gyro[1];
         pub.msg.rate_gyro_latest[2] = gyro[2];
-        // fft.update(gyro.data());
-        updated = true;
+        fft.update(gyro);
+        updated[0] = true;
     }
 
-    auto status = imu.read_accelerometer(&accel_raw);
-    if (status >= 0) {
-        static float accel[3] = {
+    if (imu.read_accelerometer(&accel_raw) >= 0) {
+        float accel[3] = {
                 raw_accel_to_meter_per_square_second(accel_raw[0]),
                 raw_accel_to_meter_per_square_second(accel_raw[1]),
                 raw_accel_to_meter_per_square_second(accel_raw[2])};
         pub.msg.accelerometer_latest[0] = accel[0];
         pub.msg.accelerometer_latest[1] = accel[1];
         pub.msg.accelerometer_latest[2] = accel[2];
-        // fft.update(accel);
-        updated = true;
-    } else {
-        updated = false;
+        updated[1] = true;
     }
 
-    if (updated) {
+    if (updated[0] && updated[1]) {
         pub.msg.timestamp = HAL_GetTick() * 1000;
         pub.publish();
     }
+
     static uint32_t prev_time = HAL_GetTick();
-    if (prev_time + 1000 < HAL_GetTick()) {
+    if (prev_time + 1000 < HAL_GetTick() || fft._fft_updated[0]) {
         prev_time = HAL_GetTick();
-        char buffer[90];
-        snprintf(buffer, sizeof(buffer), "mode: %d",
-                    static_cast<int>(mode));
-        // snprintf(buffer, sizeof(buffer), "peak freq: %d %d %d snr: %d %d %d",
-        //         (int)(fft.peak_frequencies[0][0]),
-        //         (int)(fft.peak_frequencies[1][0]),
-        //         (int)(fft.peak_frequencies[2][0]),
-        //         (int)(fft.peak_snr[0][0]),
-        //         (int)(fft.peak_snr[1][0]),
-        //         (int)(fft.peak_snr[2][0]));
+        char buffer[60];
+        snprintf(buffer, sizeof(buffer), "peaks\nfreq: %d %d %d\n snrs: %d %d %d",
+                (int)(fft.peak_frequencies[0][0]),
+                (int)(fft.peak_frequencies[1][0]),
+                (int)(fft.peak_frequencies[2][0]),
+                (int)(fft.peak_snr[0][0]),
+                (int)(fft.peak_snr[1][0]),
+                (int)(fft.peak_snr[2][0])
+                );
         logger.log_info(buffer);
     }
 }

Src/modules/imu/imu.hpp

@@ -20,7 +20,7 @@ extern "C" {
 
 class ImuModule : public Module {
 public:
-    ImuModule() : Module(400.0, Protocol::DRONECAN) {}
+    ImuModule() : Module(00.0, Protocol::DRONECAN) {}
     void init() override;
 
 protected: