diff --git a/CMakeLists.txt b/CMakeLists.txt index 83db34e..ed74b0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,6 @@ # Copyright (C) 2023-2024 Dmitry Ponomarev # Distributed under the terms of the GPL v3 license, available in the file LICENSE. cmake_minimum_required(VERSION 3.15.3) -set(CMAKE_C_FLAGS "-g ${CMAKE_C_FLAGS} ${WARNING_FLAGS}") -set(CMAKE_CXX_FLAGS "-g ${CMAKE_CXX_FLAGS} ${WARNING_FLAGS} -Wno-volatile") # Pathes set(ROOT_DIR ${CMAKE_CURRENT_LIST_DIR}) @@ -61,15 +59,8 @@ include(${LIBPARAMS_PATH}/CMakeLists.txt) list(APPEND APPLICATION_HEADERS ${ROOT_DIR}/Src ${APPLICATION_DIR}) include(${APPLICATION_DIR}/CMakeLists.txt) -# Set compile options -# set(WARNING_FLAGS "-Wall -Wextra -Wfloat-equal -Werror -Wundef -Wshadow -Wpointer-arith -Wunreachable-code -Wstrict-overflow=5 -Wwrite-strings -Wswitch-default") -# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${WARNING_FLAGS}") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${WARNING_FLAGS} -Wno-volatile") -# set(CMAKE_CXX_STANDARD 20) # Set compile options set(WARNING_FLAGS "-Wall -Wextra -Wfloat-equal -Werror -Wundef -Wshadow -Wpointer-arith -Wunreachable-code -Wstrict-overflow=5 -Wwrite-strings -Wswitch-default") -set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -o0 ${WARNING_FLAGS}") -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -o0 ${WARNING_FLAGS} -Wno-volatile") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${WARNING_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${WARNING_FLAGS} -Wno-volatile") set(CMAKE_CXX_STANDARD 20) diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index daae553..7d77c80 100644 --- a/Src/common/FFT.cpp +++ b/Src/common/FFT.cpp @@ -14,8 +14,8 @@ bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) { return false; } n_axes = num_axes; - rfft_spec = init_rfft(_hanning_window, _fft_input_buffer, - _fft_output_buffer, &window_size); + rfft_spec = fft::init_rfft(_hanning_window.data(), _fft_input_buffer.data(), + _fft_output_buffer.data(), &window_size); size = window_size; _sample_rate_hz = sample_rate_hz; _resolution_hz = sample_rate_hz / size; @@ -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]; - convert_float_to_real_t(input, conv_input, n_axes); + fft::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]; @@ -38,9 +38,9 @@ void FFT::update(float *input) { 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::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()); find_peaks(axis); // reset @@ -50,35 +50,64 @@ void FFT::update(float *input) { sizeof(real_t) * overlap_start * 3); _fft_buffer_index[axis] = overlap_start * 3; } + find_dominant(); } void FFT::find_peaks(uint8_t axis) { - // sum total energy across all used buckets for SNR - float bin_mag_sum = 0; - uint16_t num_peaks_found = 0; - - float peak_magnitude[MAX_NUM_PEAKS] {}; - uint16_t raw_peak_index[MAX_NUM_PEAKS] {}; - - static float peak_frequencies_prev[MAX_NUM_PEAKS] {}; - for (int i = 0; i < MAX_NUM_PEAKS; i++) { - peak_frequencies_prev[i] = peak_frequencies[axis][i]; - } - float fft_output_buffer_float[size]; - convert_real_t_to_float(_fft_output_buffer, fft_output_buffer_float, size); + fft::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 ++) { real_t real_imag[2] = {0, 0}; - get_real_imag_by_index(_fft_output_buffer, real_imag, size, fft_index); + fft::get_real_imag_by_index(_fft_output_buffer.data(), real_imag, size, fft_index); 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); + fft::convert_real_t_to_float(&real_imag[0], &real_f, 1); + fft::convert_real_t_to_float(&real_imag[1], &imag_f, 1); 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; } + float peak_magnitude[MAX_NUM_PEAKS] {}; + uint16_t raw_peak_index[MAX_NUM_PEAKS] {}; + _identify_peaks_bins(peak_magnitude, raw_peak_index); + auto num_peaks_found = _estimate_peaks(peak_magnitude, raw_peak_index, + fft_output_buffer_float, bin_mag_sum, axis); + // reset values if no peaks found + _fft_updated[axis] = true; + if (num_peaks_found == 0) { + for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) { + peak_frequencies[axis][peak_new] = 0; + peak_snr[axis][peak_new] = 0; + peak_magnitudes[axis][peak_new] = 0; + } + return; + } +} + +void FFT::find_dominant() { + if (!is_updated()) { + return; + } + dominant_frequency = 0; + dominant_snr = 0; + dominant_mag = 0; + for (uint8_t axis = 0; axis < n_axes; axis++) { + for (uint8_t peak = 0; peak < MAX_NUM_PEAKS; peak++) { + if (peak_snr[axis][peak] > dominant_snr) { + dominant_snr = peak_snr[axis][peak]; + dominant_frequency = peak_frequencies[axis][peak]; + dominant_mag = peak_magnitudes[axis][peak]; + } + } + } +} + +void FFT::_identify_peaks_bins(float peak_magnitude[MAX_NUM_PEAKS], + uint16_t raw_peak_index[MAX_NUM_PEAKS]) { for (uint8_t i = 0; i < MAX_NUM_PEAKS; i++) { float largest_peak = 0; uint16_t largest_peak_index = 0; @@ -104,72 +133,59 @@ void FFT::find_peaks(uint8_t axis) { _peak_magnitudes_all[largest_peak_index + 1] = 0; } } +} +uint16_t FFT::_estimate_peaks(float* peak_magnitude, + uint16_t* raw_peak_index, + float* fft, + float magnitudes_sum, uint8_t axis) { + uint16_t num_peaks_found = 0; for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) { if (raw_peak_index[peak_new] == 0) { continue; } float adjusted_bin = 0.5f * - estimate_peak_freq(fft_output_buffer_float, 2 * raw_peak_index[peak_new]); + estimate_peak_freq(fft, 2 * raw_peak_index[peak_new]); if (adjusted_bin > size || adjusted_bin < 0) { continue; } - float freq_adjusted = _resolution_hz * adjusted_bin; + // check if we already found the peak + bool peak_close = false; + for (int peak_prev = 0; peak_prev < peak_new; peak_prev++) { + peak_close = (fabsf(freq_adjusted - peak_frequencies[axis][peak_prev]) + < (_resolution_hz * 10.0f)); + if (peak_close) { + break; + } + } + if (peak_close) { + continue; + } // snr is in dB float snr; - if (bin_mag_sum - peak_magnitude[peak_new] < 1.0e-19f) { - if (bin_mag_sum > 0) { + if (magnitudes_sum - peak_magnitude[peak_new] < 1.0e-19f) { + if (magnitudes_sum > 0) { snr = MIN_SNR; } else { snr = 0.0f; } } else { snr = 10.f * log10f((size - 1) * peak_magnitude[peak_new] / - (bin_mag_sum - peak_magnitude[peak_new])); + (magnitudes_sum - peak_magnitude[peak_new])); } + // keep if SNR satisfies the requirement and the frequency is within the range if ((snr < MIN_SNR) || (freq_adjusted < fft_min_freq) || (freq_adjusted > fft_max_freq)) { continue; } - - // only keep if we're already tracking this frequency or - // if the SNR is significant - for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) { - bool peak_close = (fabsf(freq_adjusted - peak_frequencies_prev[peak_prev]) - < (_resolution_hz * 0.25f)); - _fft_updated[axis] = true; - - // keep - peak_frequencies[axis][num_peaks_found] = freq_adjusted; - peak_snr[axis][num_peaks_found] = snr; - - // remove - if (peak_close) { - peak_frequencies_prev[peak_prev] = NAN; - } - num_peaks_found++; - break; - } - } - if (num_peaks_found == 0) { - for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) { - peak_frequencies[axis][peak_new] = 0; - peak_snr[axis][peak_new] = 0; - } - return; - } - float max_snr_peak = 0; - uint8_t max_peak_index = 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_snr[axis][peak_index]; - max_peak_index = peak_index; - } + peak_frequencies[axis][num_peaks_found] = freq_adjusted; + peak_magnitudes[axis][num_peaks_found] = peak_magnitude[peak_new]; + peak_snr[axis][num_peaks_found] = snr; + num_peaks_found++; } - peak_frequencies[axis][0] = peak_frequencies[axis][max_peak_index]; - peak_snr[axis][0] = peak_snr[axis][max_peak_index]; + return num_peaks_found; } static constexpr float tau(float x) { @@ -190,8 +206,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] = get_real_by_index(fft, peak_index + i); - imag[i + 1] = get_imag_by_index(fft, peak_index + 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); } static constexpr int k = 1; diff --git a/Src/common/FFT.hpp b/Src/common/FFT.hpp index 8d925ce..536d574 100644 --- a/Src/common/FFT.hpp +++ b/Src/common/FFT.hpp @@ -29,25 +29,45 @@ class FFT { float fft_min_freq = 0; float fft_max_freq; float _resolution_hz; - 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}; + float dominant_frequency; + float dominant_snr; + float dominant_mag; + std::array, MAX_NUM_AXES> peak_frequencies {0}; + std::array, MAX_NUM_AXES> peak_magnitudes {0}; + std::array, MAX_NUM_AXES> peak_snr {0}; uint32_t total_time; + bool is_updated() { + for (uint8_t axis = 0; axis < n_axes; axis++) { + if (!_fft_updated[axis]) { + return false; + } + } + return true; + } private: uint16_t size; - real_t _hanning_window[FFT_MAX_SIZE] {}; - real_t _fft_output_buffer[FFT_MAX_SIZE * 2] {}; - real_t _fft_input_buffer[FFT_MAX_SIZE] {}; + std::array _fft_updated{false}; + std::array _hanning_window; + std::array _fft_output_buffer; + std::array _fft_input_buffer; + std::array _peak_magnitudes_all; + std::array _fft_buffer_index; std::array, MAX_NUM_AXES> data_buffer; - std::array _peak_magnitudes_all; - uint16_t _fft_buffer_index[3] {}; uint8_t n_axes; float _sample_rate_hz; float estimate_peak_freq(float fft[], int peak_index); void find_peaks(uint8_t axis); + void find_dominant(); + // void identify_bin_peaks(uint8_t axis); + void _identify_peaks_bins(float peak_magnitude[MAX_NUM_PEAKS], + uint16_t raw_peak_index[MAX_NUM_PEAKS]); + uint16_t _estimate_peaks(float* peak_magnitude, + uint16_t* raw_peak_index, + float* fft_output_buffer_float, + float bin_mag_sum, uint8_t axis); #ifdef HAL_MODULE_ENABLED // specification of arm_rfft_instance_q15 // https://arm-software.github.io/CMSIS_5/DSP/html/group__RealFFT.html diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index 029383b..8ea4e4a 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -101,9 +101,9 @@ void ImuModule::update_accel_fft() { return; } fft_accel.update(accel.data()); - pub.msg.accelerometer_integral[0] = fft_accel.peak_frequencies[0][0]; - pub.msg.accelerometer_integral[1] = fft_accel.peak_frequencies[1][0]; - pub.msg.accelerometer_integral[2] = fft_accel.peak_frequencies[2][0]; + pub.msg.accelerometer_integral[0] = fft_accel.dominant_frequency; + pub.msg.accelerometer_integral[1] = fft_accel.dominant_mag; + pub.msg.accelerometer_integral[2] = fft_accel.dominant_snr; } void ImuModule::update_gyro_fft() { @@ -111,7 +111,7 @@ void ImuModule::update_gyro_fft() { return; } fft_gyro.update(gyro.data()); - pub.msg.rate_gyro_integral[0] = fft_gyro.peak_frequencies[0][0]; - pub.msg.rate_gyro_integral[1] = fft_gyro.peak_frequencies[1][0]; - pub.msg.rate_gyro_integral[2] = fft_gyro.peak_frequencies[2][0]; + pub.msg.rate_gyro_integral[0] = fft_gyro.dominant_frequency; + pub.msg.rate_gyro_integral[1] = fft_gyro.dominant_mag; + pub.msg.rate_gyro_integral[2] = fft_gyro.dominant_snr; } diff --git a/Src/platform/stm32g0b1/rfft.hpp b/Src/platform/stm32g0b1/rfft.hpp index a41737f..3bf4504 100644 --- a/Src/platform/stm32g0b1/rfft.hpp +++ b/Src/platform/stm32g0b1/rfft.hpp @@ -6,112 +6,113 @@ typedef q15_t real_t; #define M_2PI 6.28318530717958647692 - -/* -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 N: The size of the Hanning window. -@return: The plan for the r2c transform. -*/ -inline arm_rfft_instance_q15 init_rfft(real_t* hanning_window, real_t* in, - real_t* out, uint16_t *N) { - (void) in; - (void) out; - arm_rfft_instance_q15 _rfft_q15; - switch (*N) { - case 256: - _rfft_q15.fftLenReal = 256; - _rfft_q15.twidCoefRModifier = 32U; - _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; - break; - - case 512: - _rfft_q15.fftLenReal = 512; - _rfft_q15.twidCoefRModifier = 16U; - _rfft_q15.pCfft = &arm_cfft_sR_q15_len256; - break; - - case 1024: - _rfft_q15.fftLenReal = 1024; - _rfft_q15.twidCoefRModifier = 8U; - _rfft_q15.pCfft = &arm_cfft_sR_q15_len512; - break; - default: - *N = 256; - _rfft_q15.fftLenReal = 256; - _rfft_q15.twidCoefRModifier = 32U; - _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; +namespace fft { + /* + 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 N: The size of the Hanning window. + @return: The plan for the r2c transform. + */ + inline arm_rfft_instance_q15 init_rfft(real_t* hanning_window, real_t* in, + real_t* out, uint16_t *N) { + (void) in; + (void) out; + arm_rfft_instance_q15 _rfft_q15; + switch (*N) { + case 256: + _rfft_q15.fftLenReal = 256; + _rfft_q15.twidCoefRModifier = 32U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; + break; + + case 512: + _rfft_q15.fftLenReal = 512; + _rfft_q15.twidCoefRModifier = 16U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len256; + break; + + case 1024: + _rfft_q15.fftLenReal = 1024; + _rfft_q15.twidCoefRModifier = 8U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len512; + break; + default: + *N = 256; + _rfft_q15.fftLenReal = 256; + _rfft_q15.twidCoefRModifier = 32U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; + } + _rfft_q15.pTwiddleAReal = (real_t *) realCoefAQ15; + _rfft_q15.pTwiddleBReal = (real_t *) realCoefBQ15; + _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; + arm_float_to_q15(&hanning_value, &hanning_window[n], 1); + } + + return _rfft_q15; } - _rfft_q15.pTwiddleAReal = (real_t *) realCoefAQ15; - _rfft_q15.pTwiddleBReal = (real_t *) realCoefBQ15; - _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; - arm_float_to_q15(&hanning_value, &hanning_window[n], 1); + /* + The function is written based on CMSIS-DSP library. + @param in: The input array. + @param out: The output array. + @param hanning_window: The Hanning window. + @param N: The size of the Hanning window. + */ + inline void apply_hanning_window(real_t* in, real_t* out, real_t* hanning_window, int N) { + arm_mult_q15(in, hanning_window, out, N); } - return _rfft_q15; -} - -/* -The function is written based on CMSIS-DSP library. -@param in: The input array. -@param out: The output array. -@param hanning_window: The Hanning window. -@param N: The size of the Hanning window. -*/ -inline void apply_hanning_window(real_t* in, real_t* out, real_t* hanning_window, int N) { - arm_mult_q15(in, hanning_window, out, N); -} - -/* -The function is written based on CMSIS-DSP library. -@param _rfft_q15: The plan for the r2c transform. -@param in: The input array. -@param out: The output array. -*/ -inline void rfft_one_cycle(arm_rfft_instance_q15 _rfft_q15, real_t* in, real_t* out) { - arm_rfft_q15(&_rfft_q15, in, out); -} + /* + The function is written based on CMSIS-DSP library. + @param _rfft_q15: The plan for the r2c transform. + @param in: The input array. + @param out: The output array. + */ + inline void rfft_one_cycle(arm_rfft_instance_q15 _rfft_q15, real_t* in, real_t* out) { + arm_rfft_q15(&_rfft_q15, in, out); + } -inline void convert_real_t_to_float(real_t* in, float* out, uint16_t N) { - arm_q15_to_float(in, out, N); -} + inline void convert_real_t_to_float(real_t* in, float* out, uint16_t N) { + arm_q15_to_float(in, out, N); + } -inline void convert_float_to_real_t(float* in, real_t* out, uint16_t N) { - arm_float_to_q15(in, out, N); -} + inline void convert_float_to_real_t(float* in, real_t* out, uint16_t N) { + arm_float_to_q15(in, out, N); + } -// FFT output buffer is ordered as follows: - // [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1] -template -inline void get_real_imag_by_index(T* in, T* out, uint16_t N, int index) { - (void)N; - out[0] = in[2 * index]; - out[1] = in[2 * index + 1]; -} + // FFT output buffer is ordered as follows: + // [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1] + template + inline void get_real_imag_by_index(T* in, T* out, uint16_t N, int index) { + (void)N; + out[0] = in[2 * index]; + out[1] = in[2 * index + 1]; + } -template -inline T get_real_by_index(T* in, int index) { - return in[index]; -} + template + inline T get_real_by_index(T* in, int index) { + return in[index]; + } -template -inline T get_imag_by_index(T* in, int index) { - return in[index + 1]; -} + template + inline T get_imag_by_index(T* in, int index) { + return in[index + 1]; + } +} // namespace fft #endif // SRC_PLATFORM_STM32_MATH_RFFT_HPP_ diff --git a/Src/platform/ubuntu/rfft.hpp b/Src/platform/ubuntu/rfft.hpp index f39f0b0..78d17b5 100644 --- a/Src/platform/ubuntu/rfft.hpp +++ b/Src/platform/ubuntu/rfft.hpp @@ -10,70 +10,71 @@ typedef double real_t; #include #define M_2PI 6.28318530717958647692 - -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; +namespace fft { + 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); } - // 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); -} -/* -The function apply_hanning_window applies the Hanning window to the input array. -@param in: The input array. -@param out: The output array. -@param hanning_window: The Hanning window. -@param N: The size of the input array. -*/ -inline void apply_hanning_window(real_t* in, real_t* out, real_t* hanning_window, int N) { - for (int i = 0; i < N; i++) { - out[i] = hanning_window[i] * in[i]; + /* + The function apply_hanning_window applies the Hanning window to the input array. + @param in: The input array. + @param out: The output array. + @param hanning_window: The Hanning window. + @param N: The size of the input array. + */ + inline void apply_hanning_window(real_t* in, real_t* out, real_t* hanning_window, int N) { + for (int i = 0; i < N; i++) { + out[i] = hanning_window[i] * in[i]; + } } -} -// FFT output buffer is ordered as follows: + // FFT output buffer is ordered as follows: // [real[0], real[1], real[2], ... real[(N/2)-1], imag[0], imag[1], imag[2], ... imag[(N/2)-1] -template -inline void get_real_imag_by_index(T* in, T out[2], uint16_t N, int index) { - out[0] = in[index]; - out[1] = 0; -} + template + inline void get_real_imag_by_index(T* in, T out[2], uint16_t N, int index) { + out[0] = in[index]; + out[1] = 0; + } -template -inline T get_real_by_index(T* in, uint16_t index) { - return in[index]; -} + template + inline T get_real_by_index(T* in, uint16_t index) { + return in[index]; + } -// For FFTW_R2HC kind the imaginary part is zero -// due to symmetries of the real-input DFT, and is not stored -template -inline T get_imag_by_index(T* in, uint16_t index) { - return 0; -} -/* -The function written based on fftw3 library. -@param plan: The plan for the r2c transform. -*/ -inline void rfft_one_cycle(fftw_plan plan, real_t* in, real_t* out) { - fftw_execute_r2r(plan, in, out); -} + // For FFTW_R2HC kind the imaginary part is zero + // due to symmetries of the real-input DFT, and is not stored + template + inline T get_imag_by_index(T* in, uint16_t index) { + return 0; + } + /* + The function written based on fftw3 library. + @param plan: The plan for the r2c transform. + */ + inline void rfft_one_cycle(fftw_plan plan, real_t* in, real_t* out) { + fftw_execute_r2r(plan, in, out); + } -inline void convert_real_t_to_float(real_t* in, float* out, uint16_t N) { - for (int n = 0; n < N; n++) { - out[n] = (float)in[n]; + inline void convert_real_t_to_float(real_t* in, float* out, uint16_t N) { + for (int n = 0; n < N; n++) { + out[n] = (float)in[n]; + } } -} -inline void convert_float_to_real_t(float* in, real_t* out, uint16_t N) { - for (int n = 0; n < N; n++) { - out[n] = (real_t)in[n]; + inline void convert_float_to_real_t(float* in, real_t* out, uint16_t N) { + for (int n = 0; n < N; n++) { + out[n] = (real_t)in[n]; + } } -} +} // namespace fft #endif // SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_ diff --git a/Tests/common/test_fft.cpp b/Tests/common/test_fft.cpp index 4b7feae..c4cc3dc 100644 --- a/Tests/common/test_fft.cpp +++ b/Tests/common/test_fft.cpp @@ -268,6 +268,7 @@ class TestFFTOneSignalParametrized : public TestFFTBase