fix half freq issue, rm comments, add one more fail check to tests

Src/common/FFT.cpp

@@ -14,7 +14,7 @@ bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) {
         return false;
     }
     n_axes = num_axes;
-    rfft_spec = fft::init_rfft(_hanning_window.data(), _fft_input_buffer.data(),
+    rfft_spec = rfft::init_rfft(_hanning_window.data(), _fft_input_buffer.data(),
                           _fft_output_buffer.data(), &window_size);
     size = window_size;
     _sample_rate_hz = sample_rate_hz;
@@ -25,7 +25,7 @@ bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) {
 
 void FFT::update(float *input) {
     real_t conv_input[n_axes];
-    fft::convert_float_to_real_t(input, conv_input, n_axes);
+    rfft::convert_float_to_real_t(input, conv_input, n_axes);
     for (uint8_t axis = 0; axis < n_axes; axis++) {
         uint16_t &buffer_index = _fft_buffer_index[axis];
 
@@ -34,17 +34,15 @@ void FFT::update(float *input) {
             data_buffer[axis][buffer_index] = conv_input[axis] / 2;
             buffer_index++;
             _fft_updated[axis] = false;
-
             continue;
         }
 
-        fft::apply_hanning_window(data_buffer[axis].data(), _fft_input_buffer.data(),
+        rfft::apply_hanning_window(data_buffer[axis].data(), _fft_input_buffer.data(),
                                 _hanning_window.data(), size);
-        fft::rfft_one_cycle(rfft_spec, _fft_input_buffer.data(), _fft_output_buffer.data());
+        rfft::rfft_one_cycle(rfft_spec, _fft_input_buffer.data(), _fft_output_buffer.data());
         find_peaks(axis);
 
-        // reset
-        // shift buffer (3/4 overlap)
+        // shift buffer (3/4 overlap) as sliding window for input data
         const int overlap_start = size / 4;
         memmove(&data_buffer[axis][0], &data_buffer[axis][overlap_start],
                 sizeof(real_t) * overlap_start * 3);
@@ -55,14 +53,14 @@ void FFT::update(float *input) {
 
 void FFT::find_peaks(uint8_t axis) {
     float fft_output_buffer_float[size];
-    fft::convert_real_t_to_float(_fft_output_buffer.data(), fft_output_buffer_float, size);
+    rfft::convert_real_t_to_float(_fft_output_buffer.data(), fft_output_buffer_float, size);
 
     // sum total energy across all used buckets for SNR
     float bin_mag_sum = 0;
     // calculate magnitudes for each fft bin
     for (uint16_t fft_index = 0; fft_index < size/2; fft_index ++) {
-        auto real = fft::get_imag_by_index(fft_output_buffer_float, fft_index);
-        auto imag = fft::get_imag_by_index(fft_output_buffer_float, fft_index);
+        auto real = rfft::get_real_by_index(fft_output_buffer_float, fft_index);
+        auto imag = rfft::get_imag_by_index(fft_output_buffer_float, fft_index);
         const float fft_magnitude = sqrtf(real * real + imag * imag);
         _peak_magnitudes_all[fft_index] = fft_magnitude;
         bin_mag_sum += fft_magnitude;
@@ -203,8 +201,8 @@ float FFT::_estimate_peak_freq(float fft[], int peak_index) {
     float real[3];
     float imag[3];
     for (int i = -1; i < 2; i++) {
-        real[i + 1] = fft::get_real_by_index(fft, peak_index + i);
-        imag[i + 1] = fft::get_imag_by_index(fft, peak_index + i);
+        real[i + 1] = rfft::get_real_by_index(fft, peak_index + i);
+        imag[i + 1] = rfft::get_imag_by_index(fft, peak_index + i);
     }
 
     static constexpr int k = 1;

Src/modules/imu/imu.cpp

@@ -17,9 +17,6 @@ void ImuModule::init() {
     bool imu_initialized = imu.initialize();
     mode = Module::Mode::STANDBY;
     initialized = imu_initialized;
-    // for (int i = 0; i < 2; i++) {
-    //     fft[i].init(512, 3, 512);
-    // }
     fft_accel.init(512, 3, 512);
     fft_accel.fft_min_freq = 20;
     fft_gyro.init(512, 3, 512);
@@ -85,15 +82,16 @@ void ImuModule::spin_once() {
 }
 
 void ImuModule::get_vibration(std::array<float, 3> data) {
-    if (bitmask & static_cast<std::underlying_type_t<Bitmask>>(Bitmask::ENABLE_VIB_ESTIM)) {
-        float diff_magnitude = 0.0f;
-        for (uint8_t i = 0; i < 3; i++) {
-            diff_magnitude += std::pow(data[i] - pub.msg.accelerometer_latest[i], 2);
-        }
-        vibration = 0.99f * vibration + 0.01f * std::sqrt(diff_magnitude);
-        pub.msg.integration_interval = vibration;
+    if (!(bitmask & static_cast<std::underlying_type_t<Bitmask>>(Bitmask::ENABLE_VIB_ESTIM))) {
         return;
     }
+    float diff_magnitude = 0.0f;
+    for (uint8_t i = 0; i < 3; i++) {
+        diff_magnitude += std::pow(data[i] - pub.msg.accelerometer_latest[i], 2);
+    }
+    vibration = 0.99f * vibration + 0.01f * std::sqrt(diff_magnitude);
+    pub.msg.integration_interval = vibration;
+    return;
 }
 
 void ImuModule::update_accel_fft() {

Src/modules/imu/imu.hpp

@@ -2,6 +2,7 @@
  * This program is free software under the GNU General Public License v3.
  * See <https://www.gnu.org/licenses/> for details.
  * Author: Dmitry Ponomarev <[email protected]>
+ * Author: Anastasiia Stepanova <[email protected]>
  */
 
 #ifndef SRC_MODULES_IMU_HPP_

Src/platform/stm32g0b1/rfft.hpp

@@ -1,3 +1,10 @@
+/*
+ * Copyright (C) 2024 Anastasiia Stepanova <[email protected]>
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
 #ifndef SRC_PLATFORM_STM32_MATH_RFFT_HPP_
 #define SRC_PLATFORM_STM32_MATH_RFFT_HPP_
 
@@ -6,13 +13,13 @@
 typedef q15_t real_t;
 #define M_2PI           6.28318530717958647692
 
-namespace fft {
+namespace rfft {
     /*
     The function specifies arm_rfft_instance_q15 from CMSIS-DSP library based on the window size.
     @param window_size: The size of the input array.
     @param hanning_window: Pointer to the Hanning window container.
-    @param in: used fo incompatability with ubuntu version
-    @param out: used fo incompatability with ubuntu version
+    @param in: used for compatability with ubuntu version
+    @param out: used for compatability with ubuntu version
     @param N: The size of the Hanning window.
     @return: The plan for the r2c transform.
     */
@@ -50,13 +57,6 @@ namespace fft {
         _rfft_q15.ifftFlagR = 0;
         _rfft_q15.bitReverseFlagR = 1;
 
-        // *in = new real_t[N];
-        // *out = new real_t[N * 2];
-        // *hanning_window = new real_t[N];
-
-        // *in = (real_t*)calloc(*N, sizeof(real_t));
-        // *out = (real_t*)calloc(*N * 2, sizeof(real_t));
-        // *hanning_window = (real_t*)calloc(*N, sizeof(real_t));
         for (int n = 0; n < *N; n++) {
             const float hanning_value = 0.5f * (1.f - cos(M_2PI * n / (*N - 1)));
             hanning_window[n] = hanning_value;
@@ -113,6 +113,6 @@ namespace fft {
     inline T get_imag_by_index(T* in, int index) {
         return in[index + 1];
     }
-}  // namespace fft
+}  // namespace rfft
 
 #endif  // SRC_PLATFORM_STM32_MATH_RFFT_HPP_

Src/platform/ubuntu/rfft.hpp

@@ -1,5 +1,10 @@
-/* These definitions need to be changed depending on the floating-point precision */
-#pragma once
+/*
+ * Copyright (C) 2024 Anastasiia Stepanova <[email protected]>
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+ */
+
 #ifndef SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_
 #define SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_
 
@@ -10,16 +15,21 @@ typedef double real_t;
 #include <complex>
 
 #define M_2PI           6.28318530717958647692
-namespace fft {
+namespace rfft {
+    /*
+    The function specifies fftw_plan from the FFTW3 library.
+    @param window_size: The size of the input array.
+    @param hanning_window: Pointer to the Hanning window container.
+    @param in: input data buffer
+    @param out: rfft output data buffer
+    @param N: The size of the Hanning window.
+    @return: The plan for the r2c transform.
+    */
     inline fftw_plan init_rfft(real_t* hanning_window, real_t* in, real_t* out, uint16_t *N) {
-        // *hanning_window = fftw_alloc_real(*N);
         for (int n = 0; n < *N; n++) {
             const float hanning_value = 0.5f * (1.f - cos(M_2PI * n / (*N - 1)));
             hanning_window[n] = hanning_value;
         }
-        // Allocate input and output arrays
-        // *in = (real_t*) fftw_alloc_real(sizeof(real_t)* (*N));
-        // *out = (real_t*) fftw_alloc_real(sizeof(real_t)* 2 * *N);
         // Create plan
         return fftw_plan_r2r_1d(*N, in, out, FFTW_R2HC, FFTW_ESTIMATE);
     }
@@ -76,6 +86,6 @@ namespace fft {
             out[n] = (real_t)in[n];
         }
     }
-}  // namespace fft
+}  // namespace rfft
 
 #endif  // SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_

Tests/common/test_fft.cpp

@@ -60,72 +60,6 @@ struct InitParamMultiSignalWithRes {
     bool result;
 };
 
-InitParamOneSignalWithRes OneSignalTestParams[7] = {
-    // 0
-    {{InitFFTParamType{      .sample_rate_hz = 100,  .n_axes   = 1,   .window_size  = 50},
-      InitOneSignParamType{  .sample_rate_hz = 100,  .freq_hz  = 3,   .amplitude    = 10}},
-      true},
-    // 1
-    {{InitFFTParamType{      .sample_rate_hz = 1000, .n_axes   = 1,   .window_size  = 512},
-      InitOneSignParamType{  .sample_rate_hz = 1000, .freq_hz  = 100, .amplitude    = 10}},
-      true},
-    // 2
-    {{InitFFTParamType{      .sample_rate_hz = 1000, .n_axes   = 1,   .window_size  = 100},
-      InitOneSignParamType{  .sample_rate_hz = 1000, .freq_hz  = 5,   .amplitude    = 10}},
-      true},
-    // 3
-    {{InitFFTParamType{      .sample_rate_hz = 512,  .n_axes   = 1,   .window_size  = 512},
-      InitOneSignParamType{  .sample_rate_hz = 512,  .freq_hz  = 100, .amplitude    = 10}},
-      true},
-    // 4
-    {{InitFFTParamType{      .sample_rate_hz = 1024, .n_axes   = 3,   .window_size  = 512},
-      InitOneSignParamType{  .sample_rate_hz = 1024, .freq_hz  = 100, .amplitude    = 10}},
-      true},
-    // 5
-    {{InitFFTParamType{      .sample_rate_hz = 256,  .n_axes   = 3,   .window_size  = 256},
-      InitOneSignParamType{  .sample_rate_hz = 256,  .freq_hz  = 100, .amplitude    = 1}},
-      true},
-    // 6
-    {{InitFFTParamType{     .sample_rate_hz  = 300,   .n_axes   = 1,  .window_size  = 200},
-      InitOneSignParamType{ .sample_rate_hz = 1000,  .freq_hz  = 100, .amplitude    = 1}},
-      false},
-};
-
-InitParamMultiSignalWithRes MultiSignalTestParams[8] = {
-    // 0
-    {{InitFFTParamType{          .sample_rate_hz = 24,   .n_axes    = 1,   .window_size = 24},
-      InitMultiSignalsParamType{ .sample_rate_hz = 24,   .n_signals = 2,   .max_freq    = 12}},
-      true},
-    // 1
-    {{InitFFTParamType{          .sample_rate_hz = 512, .n_axes    = 1,   .window_size = 512},
-      InitMultiSignalsParamType{ .sample_rate_hz = 512, .n_signals = 5,   .max_freq    = 256}},
-      true},
-    // 2
-    {{InitFFTParamType{          .sample_rate_hz = 1000, .n_axes    = 1,   .window_size = 100},
-      InitMultiSignalsParamType{ .sample_rate_hz = 1000, .n_signals = 4,   .max_freq    = 50}},
-      true},
-    // 3
-    {{InitFFTParamType{          .sample_rate_hz = 2000, .n_axes    = 1,   .window_size = 256},
-      InitMultiSignalsParamType{ .sample_rate_hz = 2000, .n_signals = 10,   .max_freq   = 128}},
-      true},
-    // 4
-    {{InitFFTParamType{          .sample_rate_hz = 2000, .n_axes    = 1,   .window_size = 200},
-      InitMultiSignalsParamType{ .sample_rate_hz = 2000, .n_signals = 12,   .max_freq   = 100}},
-      true},
-    // 5
-    {{InitFFTParamType{          .sample_rate_hz = 1024, .n_axes    = 1,   .window_size = 512},
-      InitMultiSignalsParamType{ .sample_rate_hz = 1024, .n_signals = 15,   .max_freq   = 256}},
-      true},
-    // 6
-    {{InitFFTParamType{          .sample_rate_hz = 256, .n_axes     = 3,   .window_size = 256},
-      InitMultiSignalsParamType{ .sample_rate_hz = 256, .n_signals  = 13,   .max_freq   = 128}},
-      true},
-    // 7
-    {{InitFFTParamType{          .sample_rate_hz = 512, .n_axes     = 3,   .window_size = 512},
-      InitMultiSignalsParamType{ .sample_rate_hz = 512, .n_signals  = 10,   .max_freq   = 256}},
-      true},
-};
-
 class SinSignalGenerator {
 public:
     SinSignalGenerator(){}
@@ -305,6 +239,37 @@ class TestFFTOneSignalParametrized : public TestFFTBase<SinSignalGenerator,
     }
 };
 
+InitParamOneSignalWithRes OneSignalTestParams[7] = {
+    // 0
+    {{InitFFTParamType{      .sample_rate_hz = 100,  .n_axes   = 1,   .window_size  = 50},
+      InitOneSignParamType{  .sample_rate_hz = 100,  .freq_hz  = 3,   .amplitude    = 10}},
+      true},
+    // 1
+    {{InitFFTParamType{      .sample_rate_hz = 1000, .n_axes   = 1,   .window_size  = 100},
+      InitOneSignParamType{  .sample_rate_hz = 1000, .freq_hz  = 5,   .amplitude    = 10}},
+      true},
+    // 2
+    {{InitFFTParamType{      .sample_rate_hz = 512,  .n_axes   = 1,   .window_size  = 512},
+      InitOneSignParamType{  .sample_rate_hz = 512,  .freq_hz  = 100, .amplitude    = 10}},
+      true},
+    // 3
+    {{InitFFTParamType{      .sample_rate_hz = 1000, .n_axes   = 1,   .window_size  = 512},
+      InitOneSignParamType{  .sample_rate_hz = 1000, .freq_hz  = 100, .amplitude    = 10}},
+      true},
+    // 4
+    {{InitFFTParamType{      .sample_rate_hz = 1024, .n_axes   = 3,   .window_size  = 512},
+      InitOneSignParamType{  .sample_rate_hz = 1024, .freq_hz  = 100, .amplitude    = 10}},
+      true},
+    // 5
+    {{InitFFTParamType{      .sample_rate_hz = 256,  .n_axes   = 3,   .window_size  = 256},
+      InitOneSignParamType{  .sample_rate_hz = 256,  .freq_hz  = 200, .amplitude    = 1}},
+      false},
+    // 6
+    {{InitFFTParamType{     .sample_rate_hz  = 100,   .n_axes   = 1,  .window_size  = 200},
+      InitOneSignParamType{ .sample_rate_hz = 1000,  .freq_hz  = 100, .amplitude    = 1}},
+      false},
+};
+
 TEST_P(TestFFTOneSignalParametrized, CheckOnWindow) {
     float input[fft_parameters.n_axes];
     for (int i = 0; i < fft_parameters.window_size + 1; i++) {
@@ -382,6 +347,41 @@ class TestFFTOnMultiSignalsParametrized : public TestFFTBase<MultiSignalsSinGene
     }
 };
 
+InitParamMultiSignalWithRes MultiSignalTestParams[8] = {
+    // 0
+    {{InitFFTParamType{          .sample_rate_hz = 24,   .n_axes    = 1,   .window_size = 24},
+      InitMultiSignalsParamType{ .sample_rate_hz = 24,   .n_signals = 2,   .max_freq    = 12}},
+      true},
+    // 1
+    {{InitFFTParamType{          .sample_rate_hz = 512, .n_axes    = 1,   .window_size = 512},
+      InitMultiSignalsParamType{ .sample_rate_hz = 512, .n_signals = 5,   .max_freq    = 256}},
+      true},
+    // 2
+    {{InitFFTParamType{          .sample_rate_hz = 1000, .n_axes    = 1,   .window_size = 100},
+      InitMultiSignalsParamType{ .sample_rate_hz = 1000, .n_signals = 4,   .max_freq    = 50}},
+      true},
+    // 3
+    {{InitFFTParamType{          .sample_rate_hz = 2000, .n_axes    = 1,   .window_size = 256},
+      InitMultiSignalsParamType{ .sample_rate_hz = 2000, .n_signals = 10,   .max_freq   = 128}},
+      true},
+    // 4
+    {{InitFFTParamType{          .sample_rate_hz = 2000, .n_axes    = 1,   .window_size = 200},
+      InitMultiSignalsParamType{ .sample_rate_hz = 2000, .n_signals = 12,   .max_freq   = 100}},
+      true},
+    // 5
+    {{InitFFTParamType{          .sample_rate_hz = 1024, .n_axes    = 1,   .window_size = 512},
+      InitMultiSignalsParamType{ .sample_rate_hz = 1024, .n_signals = 15,   .max_freq   = 256}},
+      true},
+    // 6
+    {{InitFFTParamType{          .sample_rate_hz = 256, .n_axes     = 3,   .window_size = 256},
+      InitMultiSignalsParamType{ .sample_rate_hz = 256, .n_signals  = 13,   .max_freq   = 128}},
+      true},
+    // 7
+    {{InitFFTParamType{          .sample_rate_hz = 512, .n_axes     = 3,   .window_size = 512},
+      InitMultiSignalsParamType{ .sample_rate_hz = 512, .n_signals  = 10,   .max_freq   = 256}},
+      true},
+};
+
 TEST_P(TestFFTOnMultiSignalsParametrized, CheckOnWindow) {
     for (int i = 0; i < fft_parameters.window_size + 10; i++) {
         for (int j = 0; j < fft_parameters.n_axes; j++) {