From a6a4fcf48a4d8e236ad542f9e1cec09afb0533cd Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Wed, 9 Oct 2024 09:24:55 +0300 Subject: [PATCH] fix half freq issue, rm comments, add one more fail check to tests --- Src/common/FFT.cpp | 22 +++--- Src/modules/imu/imu.cpp | 18 ++--- Src/modules/imu/imu.hpp | 1 + Src/platform/stm32g0b1/rfft.hpp | 22 +++--- Src/platform/ubuntu/rfft.hpp | 26 +++++-- Tests/common/test_fft.cpp | 132 ++++++++++++++++---------------- 6 files changed, 114 insertions(+), 107 deletions(-) diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index d4cb39d..f005c0f 100644 --- a/Src/common/FFT.cpp +++ b/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; diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index 6bf818c..61f6339 100644 --- a/Src/modules/imu/imu.cpp +++ b/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 data) { - if (bitmask & static_cast>(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>(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() { diff --git a/Src/modules/imu/imu.hpp b/Src/modules/imu/imu.hpp index 537efeb..f9da74d 100644 --- a/Src/modules/imu/imu.hpp +++ b/Src/modules/imu/imu.hpp @@ -2,6 +2,7 @@ * This program is free software under the GNU General Public License v3. * See for details. * Author: Dmitry Ponomarev + * Author: Anastasiia Stepanova */ #ifndef SRC_MODULES_IMU_HPP_ diff --git a/Src/platform/stm32g0b1/rfft.hpp b/Src/platform/stm32g0b1/rfft.hpp index 3bf4504..35d5dd4 100644 --- a/Src/platform/stm32g0b1/rfft.hpp +++ b/Src/platform/stm32g0b1/rfft.hpp @@ -1,3 +1,10 @@ +/* + * Copyright (C) 2024 Anastasiia Stepanova + * 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_ diff --git a/Src/platform/ubuntu/rfft.hpp b/Src/platform/ubuntu/rfft.hpp index 3ba897f..9ed840b 100644 --- a/Src/platform/ubuntu/rfft.hpp +++ b/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 + * 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 #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_ diff --git a/Tests/common/test_fft.cpp b/Tests/common/test_fft.cpp index c4cc3dc..af82d8d 100644 --- a/Tests/common/test_fft.cpp +++ b/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