From 6ca485107d10024d2fb20e5bafbee98f283d630d Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Fri, 4 Oct 2024 15:40:29 +0300 Subject: [PATCH 01/14] add fft with tests --- CMakeLists.txt | 9 + Makefile | 8 +- Src/common/FFT.cpp | 221 +++++++++++++ Src/common/FFT.hpp | 65 ++++ Src/modules/imu/imu.cpp | 31 +- Src/modules/imu/imu.hpp | 10 +- Src/platform/stm32g0b1/CMakeLists.txt | 6 +- Src/platform/stm32g0b1/rfft.hpp | 115 +++++++ Src/platform/ubuntu/rfft.hpp | 79 +++++ Tests/CMakeLists.txt | 48 +++ Tests/Makefile | 41 +++ Tests/common/test_fft.cpp | 429 ++++++++++++++++++++++++++ cmake/stm32g0b1.cmake | 3 +- 13 files changed, 1057 insertions(+), 8 deletions(-) create mode 100644 Src/common/FFT.cpp create mode 100644 Src/common/FFT.hpp create mode 100644 Src/platform/stm32g0b1/rfft.hpp create mode 100644 Src/platform/ubuntu/rfft.hpp create mode 100644 Tests/CMakeLists.txt create mode 100644 Tests/Makefile create mode 100644 Tests/common/test_fft.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index dcce0ab..d16c469 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ # 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}) @@ -59,8 +61,15 @@ 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/Makefile b/Makefile index b44e9ad..43eefe8 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,11 @@ # Copyright (C) 2023-2024 Dmitry Ponomarev # Distributed under the terms of the GPL v3 license, available in the file LICENSE. +# Variables +CC := arm-none-eabi-gcc +CFLAGS := -g -Og + +# Add compile flags to cmake +CMAKE_FLAGS := "-DCMAKE_C_COMPILER=${CC} -DCMAKE_CXX_COMPILER=${CC} -DCMAKE_BUILD_TYPE=Debug" ROOT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) BUILD_DIR:=$(ROOT_DIR)/build @@ -38,7 +44,7 @@ sitl_dronecan: checks clean cd ${BUILD_DIR}/dronecan_sitl/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_UBUNTU=ON -G "Unix Makefiles" ../../.. && make dronecan_v3: checks clean mkdir -p ${BUILD_DIR}/dronecan_v3/obj - cd ${BUILD_DIR}/dronecan_v3/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_NODE_V3=ON -G "Unix Makefiles" ../../.. && make + cd ${BUILD_DIR}/dronecan_v3/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_NODE_V3=ON ${CMAKE_FLAGS} -G "Unix Makefiles" ../../.. && make # Cyphal & DroneCAN v2: checks generate_dsdl clean diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp new file mode 100644 index 0000000..f5229fc --- /dev/null +++ b/Src/common/FFT.cpp @@ -0,0 +1,221 @@ + +/* + * Copyright (C) 2024 Stepanova Anastasiia + * 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/. + */ +#include +#include +#include "FFT.hpp" + +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; + rfft_spec = init_rfft(&_hanning_window, &_fft_input_buffer, + &_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; + return true; +} + +void FFT::update(float *input) { + real_t conv_input[n_axes]; + 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]; + + if (buffer_index < size) { + // 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); + 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); + _fft_buffer_index[axis] = overlap_start * 3; + } +} + +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); + + 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); + 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 + imag_f * imag_f); + _peak_magnitudes_all[fft_index] = fft_magnitude; + bin_mag_sum += fft_magnitude; + } + + 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 freq_hz = _resolution_hz * bin_index; + + if ((_peak_magnitudes_all[bin_index] > largest_peak) + && (freq_hz >= fft_min_freq) + && (freq_hz <= fft_max_freq)) { + largest_peak_index = bin_index; + largest_peak = _peak_magnitudes_all[bin_index]; + } + } + + if (largest_peak_index > 0) { + raw_peak_index[i] = largest_peak_index; + peak_magnitude[i] = _peak_magnitudes_all[largest_peak_index]; + // remove peak + sides (included in frequency estimate later) + _peak_magnitudes_all[largest_peak_index - 1] = 0; + _peak_magnitudes_all[largest_peak_index] = 0; + _peak_magnitudes_all[largest_peak_index + 1] = 0; + } + } + + for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) { + 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 (adjusted_bin > size || adjusted_bin < 0) { + continue; + } + + float freq_adjusted = _resolution_hz * adjusted_bin; + // snr is in dB + 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; + } + + // 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)); + 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; + + // remove + if (peak_close) { + peak_frequencies_prev[peak_prev] = NAN; + } + num_peaks_found++; + break; + } + } + 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][0] = peak_frequencies[axis][max_peak_index]; + peak_snr[axis][0] = peak_snr[axis][max_peak_index]; +} + +static constexpr float tau(float x) { + // tau(x) = 1/4 * log(3*x*x + 6*x + 1) - sqrt(6)/24*log((x + 1 - sqrt(2/3))/(x + 1 + sqrt(2/3))) + float addend_1 = 1/4 * logf(3 * x * x + 6 * x + 1); + float multiplier_2 = sqrtf(6) / 24; + float addend_2 = logf((x + 1 - sqrtf(2 / 3)) / (x + 1 + sqrtf(2 / 3))); + return addend_1 - multiplier_2 * addend_2; +} + +float FFT::estimate_peak_freq(float fft[], int peak_index) { + 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[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); + } + + 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) + float d = (dp + dm) / 2.f + tau(dp * dp) - tau(dm * dm); + + // k’ = k + d + return peak_index + 2.f * d; +} diff --git a/Src/common/FFT.hpp b/Src/common/FFT.hpp new file mode 100644 index 0000000..474d877 --- /dev/null +++ b/Src/common/FFT.hpp @@ -0,0 +1,65 @@ +/* + * 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_COMMON_ALGORITHMS_FFT_H_ +#define SRC_COMMON_ALGORITHMS_FFT_H_ + +#include +#include "rfft.hpp" +#include "main.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define FFT_MAX_SIZE 512 +#define MAX_NUM_PEAKS 3 +#define MIN_SNR 1.0f +#define MAX_NUM_AXES 3 +constexpr size_t NUMBER_OF_SAMPLES = 512; + +class FFT { +public: + bool init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz); + void update(float *input); + 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}; + 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::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); + #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 + // plan for the r2c transform from fftw3 library. + fftw_plan rfft_spec; + #endif +}; + +#ifdef __cplusplus +} +#endif + +#endif // SRC_COMMON_ALGORITHMS_FFT_H_ diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index df7e1ab..61d607e 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -14,6 +14,13 @@ 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); + fft_gyro.fft_min_freq = 20; } void ImuModule::update_params() { @@ -41,8 +48,6 @@ void ImuModule::spin_once() { std::array accel_raw = {0, 0, 0}; std::array gyro_raw = {0, 0, 0}; - std::array gyro = {0.0f, 0.0f, 0.0f}; - std::array accel = {0.0f, 0.0f, 0.0f}; if (imu.read_gyroscope(&gyro_raw) >= 0) { gyro = { raw_gyro_to_rad_per_second(gyro_raw[0]), @@ -52,6 +57,7 @@ void ImuModule::spin_once() { pub.msg.rate_gyro_latest[1] = gyro[1]; pub.msg.rate_gyro_latest[2] = gyro[2]; updated[0] = true; + update_gyro_fft(); } if (imu.read_accelerometer(&accel_raw) >= 0) { @@ -64,6 +70,7 @@ void ImuModule::spin_once() { pub.msg.accelerometer_latest[1] = accel[1]; pub.msg.accelerometer_latest[2] = accel[2]; updated[1] = true; + update_accel_fft(); } if (pub_timeout_ms != 0 && HAL_GetTick() - pub.msg.timestamp / 1000 > pub_timeout_ms) { @@ -85,3 +92,23 @@ void ImuModule::get_vibration(std::array data) { return; } } + +void ImuModule::update_accel_fft() { + if (!(bitmask & static_cast>(Bitmask::ENABLE_FFT_ACC))) { + 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]; +} + +void ImuModule::update_gyro_fft() { + if (!(bitmask & static_cast>(Bitmask::ENABLE_FFT_GYR))) { + 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]; +} diff --git a/Src/modules/imu/imu.hpp b/Src/modules/imu/imu.hpp index 6d1d22a..537efeb 100644 --- a/Src/modules/imu/imu.hpp +++ b/Src/modules/imu/imu.hpp @@ -11,6 +11,7 @@ #include "module.hpp" #include "publisher.hpp" #include "drivers/mpu9250/mpu9250.hpp" +#include "common/FFT.hpp" #ifdef __cplusplus extern "C" { @@ -25,24 +26,29 @@ class ImuModule : public Module { ENABLE_FFT_GYR = 8, ENABLE_ALL_BY_DEFAULT = 15, }; - ImuModule() : Module(400.0, Protocol::DRONECAN) {} + ImuModule() : Module(512.0, Protocol::DRONECAN) {} void init() override; protected: void spin_once() override; void update_params() override; void get_vibration(std::array data); - void fft(); + void update_accel_fft(); + void update_gyro_fft(); private: DronecanPublisher pub; DronecanPublisher mag; Mpu9250 imu; + FFT fft_accel; + FFT fft_gyro; float vibration = 0.0f; bool initialized{false}; bool enabled{false}; uint8_t bitmask{0}; uint16_t pub_timeout_ms{0}; + std::array gyro = {0.0f, 0.0f, 0.0f}; + std::array accel = {0.0f, 0.0f, 0.0f}; static constexpr float raw_gyro_to_rad_per_second(int16_t raw_gyro) { return raw_gyro * std::numbers::pi_v / 131.0f / 180.0f; } diff --git a/Src/platform/stm32g0b1/CMakeLists.txt b/Src/platform/stm32g0b1/CMakeLists.txt index 0d1d3a9..d55848a 100644 --- a/Src/platform/stm32g0b1/CMakeLists.txt +++ b/Src/platform/stm32g0b1/CMakeLists.txt @@ -13,6 +13,7 @@ add_executable(${EXECUTABLE} ${APPLICATION_SOURCES} ${PERIPHERAL_SOURCES} ${BUILD_SRC_DIR}/params.cpp + ${ROOT_DIR}/Src/common/FFT.cpp ${ROOT_DIR}/Src/common/algorithms.cpp ${ROOT_DIR}/Src/common/logging.cpp ${ROOT_DIR}/Src/common/application.cpp @@ -35,9 +36,10 @@ target_include_directories(${EXECUTABLE} PRIVATE ${BUILD_SRC_DIR} ${APPLICATION_HEADERS} ${ROOT_DIR}/Src/common - + ${CMAKE_CURRENT_LIST_DIR} ${stm32cubeMxProjectPath}/Core/Inc ${stm32cubeMxProjectPath}/Drivers/CMSIS/Include + ${stm32cubeMxProjectPath}/Drivers/CMSIS/DSP/Include ${stm32cubeMxProjectPath}/Drivers/CMSIS/Device/ST/STM32F1xx/Include ${stm32cubeMxProjectPath}/Drivers/STM32F1xx_HAL_Driver/Inc @@ -57,4 +59,4 @@ add_custom_command(TARGET ${EXECUTABLE} COMMAND ${CMAKE_OBJCOPY} -I binary -O elf32-little ${EXECUTABLE} ${BUILD_OBJ_DIR}/${PROJECT_NAME}.elf ) -include(${CMAKE_DIR}/create_binary_with_meaningful_name.cmake) +include(${CMAKE_DIR}/create_binary_with_meaningful_name.cmake) \ No newline at end of file diff --git a/Src/platform/stm32g0b1/rfft.hpp b/Src/platform/stm32g0b1/rfft.hpp new file mode 100644 index 0000000..83479c4 --- /dev/null +++ b/Src/platform/stm32g0b1/rfft.hpp @@ -0,0 +1,115 @@ +#ifndef SRC_PLATFORM_STM32_MATH_RFFT_HPP_ +#define SRC_PLATFORM_STM32_MATH_RFFT_HPP_ + +#include "arm_math.h" +#include "arm_const_structs.h" +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) { + 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; +} + +/* +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); +} + +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); +} + +// 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_imag_by_index(T* in, int index) { + return in[index + 1]; +} + +#endif // SRC_PLATFORM_STM32_MATH_RFFT_HPP_ diff --git a/Src/platform/ubuntu/rfft.hpp b/Src/platform/ubuntu/rfft.hpp new file mode 100644 index 0000000..f7ec1b6 --- /dev/null +++ b/Src/platform/ubuntu/rfft.hpp @@ -0,0 +1,79 @@ +/* These definitions need to be changed depending on the floating-point precision */ +#pragma once +#ifndef SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_ +#define SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_ + +typedef double real_t; +#include +#include +#include +#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; + } + // 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]; + } +} + +// 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 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); +} + +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]; + } +} + +#endif // SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_ diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt new file mode 100644 index 0000000..b1c7aa2 --- /dev/null +++ b/Tests/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.15.3) +project(tests CXX C) + +set(TESTS_DIR ${CMAKE_CURRENT_LIST_DIR}) +cmake_path(GET TESTS_DIR PARENT_PATH ROOT_DIR) + +# Check if the COVERAGE flag is set +option(COVERAGE "Enable coverage flags" OFF) +option(TEST_NUM "Enable test num" OFF) +if(COVERAGE EQUAL 1) + message(STATUS "Code coverage enabled!") + add_compile_options(-g -O0 --coverage) + add_link_options(--coverage) +endif() + +enable_testing() +find_package(GTest REQUIRED) +include_directories(${GTEST_INCLUDE_DIR}) + +find_package(PkgConfig REQUIRED) +pkg_search_module(FFTW REQUIRED fftw3) +include_directories(${FFTW_INCLUDE_DIRS}) +FILE(GLOB commonSources ${ROOT_DIR}/Src/common/FFT.cpp + ${ROOT_DIR}/Src/common/algorithms.cpp) + +function(gen_test app_name test_file) + # Create the executable target + add_executable(${app_name} + ${commonSources} + ${test_file}) + + # Include directories for the target + target_include_directories(${app_name} PUBLIC ${ROOT_DIR}/Src/common ${ROOT_DIR}/Src/platform/ubuntu) + + # Link libraries to the target + target_link_libraries(${app_name} GTest::gtest GTest::gtest_main ${FFTW_LIBRARIES} m stdc++) + + # Printing the target properties (optional, can be removed) + get_target_property(var ${app_name} INCLUDE_DIRECTORIES) + message(STATUS "${app_name} include directories: ${var}") +endfunction() + +# Define the path to the test files directory +set(UNIT_TESTS_DIR ${ROOT_DIR}/Tests) + +# Generate test executables +gen_test(fft ${UNIT_TESTS_DIR}/common/test_fft.cpp) +gen_test(algorithms ${UNIT_TESTS_DIR}/common/test_algorithms.cpp) diff --git a/Tests/Makefile b/Tests/Makefile new file mode 100644 index 0000000..2cb49a4 --- /dev/null +++ b/Tests/Makefile @@ -0,0 +1,41 @@ +# Copyright (c) 2023 Dmitry Ponomarev +# 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/. + +CRNT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) +TESTS_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) +REPO_DIR := $(abspath $(dir $(lastword $(TESTS_DIR)))) +BUILD_DIR=${REPO_DIR}/build/Tests + +$(info CRNT_DIR: ${CRNT_DIR}) +$(info TESTS_DIR: ${TESTS_DIR}) +$(info REPO_DIR: ${REPO_DIR}) +$(info BUILD_DIR: ${BUILD_DIR}) + +CFLAGS := "-g -O0 -DDEBUG" +CMAKE_FLAGS := -DCMAKE_C_COMPILER=${CC} -DCMAKE_CXX_COMPILER=${CC} -DCMAKE_C_FLAGS=${CFLAGS} -DCMAKE_CXX_FLAGS=${CFLAGS} -DCMAKE_BUILD_TYPE=Debug + +all: test_common + +test_common: fft algorithms + +create_build_dir: + mkdir -p ${BUILD_DIR} + +coverage: create_build_dir + cd ${BUILD_DIR} && cmake -DCOVERAGE=1 ${CRNT_DIR} && make + cd ${BUILD_DIR} && ./algorithms + cd ${BUILD_DIR} && ./fft + + echo "\n1. Algorithms:" + cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/algorithms.dir${REPO_DIR}/common/algorithms.cpp.gcda + + echo "\n2. FFT:" + cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/fft.dir${REPO_DIR}/common/fft.c.gcda + +algorithms: create_build_dir + cd ${BUILD_DIR} && cmake -S ${CRNT_DIR} && make && ./algorithms + +fft: create_build_dir + cd ${BUILD_DIR} && cmake ${CRNT_DIR} ${CMAKE_FLAGS} && make && ./fft diff --git a/Tests/common/test_fft.cpp b/Tests/common/test_fft.cpp new file mode 100644 index 0000000..fce67d0 --- /dev/null +++ b/Tests/common/test_fft.cpp @@ -0,0 +1,429 @@ +/** + * This program is free software under the GNU General Public License v3. + * See for details. + * Author: Anastasiia Stepanova + */ + +#include +#include // For std::fabs +#include // For std::clamp +#include // for tuple + +#include "FFT.hpp" + +static constexpr auto ABS_ERR = 0.1f; +unsigned int seed = 1; + +template +auto IsInRange(T lo, T hi) { + return AllOf(Ge((lo)), Le((hi))); +} +::testing::AssertionResult IsBetweenInclusive(int val, int a, int b) +{ + if((val >= a) && (val <= b)) + return ::testing::AssertionSuccess(); + else + return ::testing::AssertionFailure() + << val << " is outside the range " << a << " to " << b; +} + +struct InitFFTParamType { + float sample_rate_hz; + uint8_t n_axes; + uint16_t window_size; +}; + +template +struct InitParamType { + InitFFTParamType fft_parameters; + T signals_parameters; +}; + +struct InitOneSignParamType { + float sample_rate_hz = 0; + float freq_hz = 0; + float amplitude = 0; +}; + +struct InitMultiSignalsParamType { + uint16_t sample_rate_hz; + uint8_t n_signals; + uint16_t max_freq; +}; + +struct InitParamOneSignalWithRes { + InitParamType parameters; + bool result; +}; + +struct InitParamMultiSignalWithRes { + InitParamType parameters; + bool result; +}; + +InitParamOneSignalWithRes OneSignalTestParams[7] = { + // 0 + {{InitFFTParamType{ .sample_rate_hz = 100, .n_axes = 1, .window_size = 24}, + InitOneSignParamType{ .sample_rate_hz = 100, .freq_hz = 3, .amplitude = 1}}, + 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 = 400}, + InitOneSignParamType{ .sample_rate_hz = 1024, .freq_hz = 100, .amplitude = 1}}, + true}, + // 5 + {{InitFFTParamType{ .sample_rate_hz = 256, .n_axes = 3, .window_size = 256}, + InitOneSignParamType{ .sample_rate_hz = 256, .freq_hz = 100, .amplitude = 1}}, + true}, + // 7 + {{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(){} + explicit SinSignalGenerator(InitOneSignParamType signal_parameters) { + this->sample_rate_hz = signal_parameters.sample_rate_hz; + this->freq_hz = signal_parameters.freq_hz; + this->amplitude = signal_parameters.amplitude; + this->phase = 0; + this->secs = 0; + } + SinSignalGenerator(float sample_rate_hz, float freq_hz, float amplitude) { + this->sample_rate_hz = sample_rate_hz; + this->freq_hz = freq_hz; + this->amplitude = amplitude; + this->phase = 0; + this->secs = 0; + } + float get_next_sample() { + auto sin = sinf(2 * M_PI * freq_hz * secs + phase); + float sample = (float)amplitude * sin; + secs += 1 / sample_rate_hz; + return sample; + } + float freq_hz; + float amplitude; + +private: + float sample_rate_hz; + float phase; + float secs; +}; + +class MultiSignalsSinGenerator { +public: + std::vector signals_generator; + uint8_t n_signals; + std::vector> dominant_sig; + + void init() { + signals_generator.resize(n_signals); + uint16_t max_amplitude = 0; + for (int j = 0; j < n_signals; j++) { + uint16_t freq_hz = rand_r(&seed) % (max_freq - min_freq) + min_freq; + uint16_t amplitude = 1 + rand_r(&seed) % 100; + signals_generator[j] = SinSignalGenerator(sample_rate_hz, freq_hz, amplitude); + if (amplitude > max_amplitude) { + max_amplitude = amplitude; + dominant_sig.insert(dominant_sig.begin(), std::make_tuple(amplitude, freq_hz)); + } + } + for (int j = 0; j < dominant_sig.size(); j++) { + printf("dominant freq: %d\n", std::get<1>(dominant_sig[j])); + } + // sort by amplitude value + std::sort(dominant_sig.begin(), dominant_sig.end()); + } + + MultiSignalsSinGenerator() {} + + MultiSignalsSinGenerator(uint8_t n_signals, uint16_t sample_rate_hz, uint16_t max_freq) { + this->n_signals = n_signals; + this->sample_rate_hz = sample_rate_hz; + this->max_freq = max_freq; + init(); + } + + explicit MultiSignalsSinGenerator(InitMultiSignalsParamType parameters) { + this->n_signals = parameters.n_signals; + this->sample_rate_hz = parameters.sample_rate_hz; + this->max_freq = parameters.max_freq; + init(); + } + + float get_next_samples() { + float sample = 0; + for (int j = 0; j < n_signals; j++) { + sample +=signals_generator[j].get_next_sample(); + } + return sample; + } + +private: + uint16_t max_freq; + uint16_t min_freq = 0; + uint16_t sample_rate_hz; +}; + +template +class TestFFTBase : public ::testing::Test { +public: + /* data */ + FFT fft; + std::vector signals_generator; + InitFFTParamType fft_parameters; + std::vector signals_parameters; + float abs_error; + + void print_fft_parameters() { + // Print the FFT parameters + std::cout << "FFT Parameters: " << std::endl; + std::cout << " Sample Rate (Hz): " << fft_parameters.sample_rate_hz << std::endl; + std::cout << " Window Size: " << fft_parameters.window_size << std::endl; + std::cout << " Number of Axes: " << static_cast(fft_parameters.n_axes) << std::endl; + } + + void init() { + fft.init(fft_parameters.window_size, fft_parameters.n_axes, fft_parameters.sample_rate_hz); + signals_generator.resize(fft_parameters.n_axes); + for (int i = 0; i < fft_parameters.n_axes; i++) { + auto signal_parameters = signals_parameters[i]; + signals_generator[i] = T(signal_parameters); + } + abs_error = 10 * fft._resolution_hz; + } +}; + +class TestFFTOneSignalParametrized : public TestFFTBase, + public testing::WithParamInterface { +public: + /* data */ + bool result; + + void print_signal_parameters() { + // Print the signal parameters + std::cout << "Signal Parameters: " << std::endl; + std::cout << " Sample Rate (Hz): " << signals_parameters[0].sample_rate_hz + << std::endl; + std::cout << " Frequency (Hz): " << signals_parameters[0].freq_hz + << std::endl; + std::cout << " Amplitude: " << signals_parameters[0].amplitude + << std::endl; + } + void SetUp() override { + auto parameters = GetParam(); + InitParamType init_parameters = parameters.parameters; + result = parameters.result; + fft_parameters = init_parameters.fft_parameters; + // expand signal_parameters to multiple axes if needed + signals_parameters.resize(fft_parameters.n_axes); + for (int i = 0; i < fft_parameters.n_axes; i++) { + signals_parameters[i] = init_parameters.signals_parameters; + } + print_signal_parameters(); + print_fft_parameters(); + init(); + } +}; + +TEST_P(TestFFTOneSignalParametrized, CheckOnWindow) { + float input[fft_parameters.n_axes]; + for (int i = 0; i < fft_parameters.window_size + 10; i++) { + for (int j = 0; j < fft_parameters.n_axes; j++) { + input[j] = signals_generator[j].get_next_sample(); + } + fft.update(input); + } + for (int j = 0; j < fft_parameters.n_axes; j++) { + printf("AXIS: %d\n", j); + for (int i = 0; i < MAX_NUM_PEAKS; i++) { + printf("%d:\n freq:\t%f\n", i, + fft.peak_frequencies[j][i]); + printf(" snr:\t%f\n", fft.peak_snr[j][i]); + } + } + printf("fft resolution: %f\n", fft._resolution_hz); + if (result) { + for (int i = 0; i < fft_parameters.n_axes; i++) { + EXPECT_NEAR(fft.peak_frequencies[i][0], + signals_parameters[i].freq_hz, 10 * fft._resolution_hz); + } + } else { + for (int i = 0; i < fft_parameters.n_axes; i++) { + ASSERT_FALSE(IsBetweenInclusive(fft.peak_frequencies[i][0], + signals_parameters[i].freq_hz - 10 * fft._resolution_hz, + signals_parameters[i].freq_hz + 10 * fft._resolution_hz)); + } + } +} + +TEST_P(TestFFTOneSignalParametrized, CheckOnFewWindows) { + float input[fft_parameters.n_axes]; + auto n_updates = (rand_r(&seed) % 8 + 2) * fft_parameters.window_size; + // auto n_updates = (2 + rand_r(&seed) % 10) * fft_parameters.window_size; + for (int i = 0; i < n_updates + 10; i++) { + for (int j = 0; j < fft_parameters.n_axes; j++) { + input[j] = signals_generator[j].get_next_sample(); + } + fft.update(input); + } + for (int axis = 0; axis < fft_parameters.n_axes; axis++) { + printf("AXIS %d\n", axis); + for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { + printf("peak index: %d\n", peak_index); + printf("fft peak freq: %f\n", fft.peak_frequencies[axis][peak_index]); + } + } + if (result) { + for (int i = 0; i < fft_parameters.n_axes; i++) { + EXPECT_NEAR(fft.peak_frequencies[i][0], + signals_generator[i].freq_hz, 10 * fft._resolution_hz); + } + } else { + for (int i = 0; i < fft_parameters.n_axes; i++) { + ASSERT_FALSE(IsBetweenInclusive(fft.peak_frequencies[i][0], + signals_generator[i].freq_hz - 10 * fft._resolution_hz, + signals_generator[i].freq_hz + 10 * fft._resolution_hz)); + } + } +} + +INSTANTIATE_TEST_SUITE_P(TestFFTOneSignalParametrized, + TestFFTOneSignalParametrized, + ::testing::ValuesIn( + OneSignalTestParams) + ); + + +class TestFFTOnMultiSignalsParametrized : public TestFFTBase, + public ::testing::WithParamInterface { +public: + /* data */ + bool result; + std::vector input; + void print_signal_parameters() { + // Print the signal parameters + std::cout << "Signal Parameters: " << std::endl; + std::cout << " Sample Rate (Hz): " << signals_parameters[0].sample_rate_hz + << std::endl; + std::cout << " Number of Signals: " << signals_parameters[0].n_signals + << std::endl; + std::cout << " Max Freq: " << signals_parameters[0].max_freq + << std::endl; + } + + void SetUp() override { + auto parameters = GetParam(); + InitParamType init_parameters = parameters.parameters; + result = parameters.result; + fft_parameters = init_parameters.fft_parameters; + signals_parameters.resize(fft_parameters.n_axes); + input.resize(fft_parameters.n_axes); + for (int i = 0; i < fft_parameters.n_axes; i++) { + signals_parameters[i] = init_parameters.signals_parameters; + } + print_signal_parameters(); + print_fft_parameters(); + init(); + } +}; + +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++) { + input[j] = signals_generator[j].get_next_samples(); + } + fft.update(input.data()); + } + for (int axis = 0; axis < fft_parameters.n_axes; axis++) { + bool heat_peak; + auto signal_generator = signals_generator[axis]; + auto n_dominants = signal_generator.dominant_sig.size(); + auto n_signals = signal_generator.n_signals; + printf("abs error: %f\n", abs_error); + for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { + printf("peak index: %d\n", peak_index); + printf("fft peak freq: %f\n", fft.peak_frequencies[axis][peak_index]); + for (int j = 0; j < n_dominants; j++) { + auto dominant_freq = signal_generator.dominant_sig[j]; + printf("dominant freq: %d\n", std::get<1>(dominant_freq)); + if (IsBetweenInclusive(fft.peak_frequencies[axis][peak_index], + std::get<1>(dominant_freq) - abs_error, + std::get<1>(dominant_freq) + abs_error)) { + heat_peak = true; + break; + } + } + if (heat_peak) { + break; + } + } + if (result) { + EXPECT_TRUE(heat_peak); + } else { + ASSERT_FALSE(heat_peak); + } + } +} + +INSTANTIATE_TEST_SUITE_P(TestFFTOnMultiSignalsParametrized, + TestFFTOnMultiSignalsParametrized, + ::testing::ValuesIn( + MultiSignalTestParams) + ); + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/cmake/stm32g0b1.cmake b/cmake/stm32g0b1.cmake index 6186487..b93a58c 100644 --- a/cmake/stm32g0b1.cmake +++ b/cmake/stm32g0b1.cmake @@ -6,7 +6,8 @@ set(CPU STM32G0B1xx) set(stm32cubeMxProjectPath ${ROOT_DIR}/Libs/mini-v3-ioc) FILE(GLOB ldFile ${stm32cubeMxProjectPath}/*_FLASH.ld) FILE(GLOB coreSources ${stm32cubeMxProjectPath}/Core/Src/*) -FILE(GLOB driversSources ${stm32cubeMxProjectPath}/Drivers/*/*/*.c) +FILE(GLOB driversSources ${stm32cubeMxProjectPath}/Drivers/*/*/*.c + ${stm32cubeMxProjectPath}/Drivers/*/*/*/*.c) FILE(GLOB startupFile ${stm32cubeMxProjectPath}/*.s ${stm32cubeMxProjectPath}/Core/Startup/*.s ) From 425f222e6a5d5abfc094a9294b080240a69a8134 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Mon, 7 Oct 2024 13:45:18 +0300 Subject: [PATCH 02/14] restrict snr, reset frequencies if no new peak was found --- Src/common/FFT.cpp | 20 ++++++++++++-------- Src/common/FFT.hpp | 8 ++++---- Src/modules/imu/imu.cpp | 9 +++++++++ Src/platform/stm32g0b1/rfft.hpp | 16 +++++++++------- Src/platform/ubuntu/rfft.hpp | 12 ++++++------ 5 files changed, 40 insertions(+), 25 deletions(-) diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index f5229fc..042a0a2 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 = init_rfft(_hanning_window, _fft_input_buffer, + _fft_output_buffer, &window_size); size = window_size; _sample_rate_hz = sample_rate_hz; _resolution_hz = sample_rate_hz / size; @@ -109,9 +109,6 @@ 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 (adjusted_bin > size || adjusted_bin < 0) { @@ -138,9 +135,9 @@ void FFT::find_peaks(uint8_t axis) { 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)); - if (!peak_close && peak_frequencies_prev[peak_prev] > 0) { - continue; - } + // if (!peak_close && peak_frequencies_prev[peak_prev] > 0) { + // continue; + // } _fft_updated[axis] = true; // keep @@ -155,6 +152,13 @@ void FFT::find_peaks(uint8_t axis) { 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++) { diff --git a/Src/common/FFT.hpp b/Src/common/FFT.hpp index 474d877..8d925ce 100644 --- a/Src/common/FFT.hpp +++ b/Src/common/FFT.hpp @@ -18,7 +18,7 @@ extern "C" { #define FFT_MAX_SIZE 512 #define MAX_NUM_PEAKS 3 -#define MIN_SNR 1.0f +#define MIN_SNR 8.0f #define MAX_NUM_AXES 3 constexpr size_t NUMBER_OF_SAMPLES = 512; @@ -36,9 +36,9 @@ class FFT { private: uint16_t size; - real_t *_hanning_window{nullptr}; - real_t *_fft_output_buffer{nullptr}; - real_t *_fft_input_buffer{nullptr}; + 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, MAX_NUM_AXES> data_buffer; std::array _peak_magnitudes_all; diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index 61d607e..ef1f37d 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -7,9 +7,12 @@ #include #include "imu.hpp" #include "params.hpp" +#include "common/logging.hpp" REGISTER_MODULE(ImuModule) +Logging logger{"IMU"}; + void ImuModule::init() { bool imu_initialized = imu.initialize(); mode = Module::Mode::STANDBY; @@ -79,6 +82,12 @@ void ImuModule::spin_once() { pub.publish(); } } + if (HAL_GetTick() % 1000 == 0) { + char buffer[90]; + snprintf(buffer, sizeof(buffer), "%d %d %d\n", int(1000 * fft_accel.peak_snr[0][0]), + int(1000 * fft_accel.peak_snr[1][0]), int(1000 * fft_accel.peak_snr[2][0])); + logger.log_info(buffer); + } } void ImuModule::get_vibration(std::array data) { diff --git a/Src/platform/stm32g0b1/rfft.hpp b/Src/platform/stm32g0b1/rfft.hpp index 83479c4..a41737f 100644 --- a/Src/platform/stm32g0b1/rfft.hpp +++ b/Src/platform/stm32g0b1/rfft.hpp @@ -16,8 +16,10 @@ The function specifies arm_rfft_instance_q15 from CMSIS-DSP library based on the @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) { +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: @@ -52,13 +54,13 @@ inline arm_rfft_instance_q15 init_rfft(real_t** hanning_window, real_t** in, // *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)); + // *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); + hanning_window[n] = hanning_value; + arm_float_to_q15(&hanning_value, &hanning_window[n], 1); } return _rfft_q15; diff --git a/Src/platform/ubuntu/rfft.hpp b/Src/platform/ubuntu/rfft.hpp index f7ec1b6..f39f0b0 100644 --- a/Src/platform/ubuntu/rfft.hpp +++ b/Src/platform/ubuntu/rfft.hpp @@ -11,17 +11,17 @@ typedef double real_t; #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); +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; + 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); + // *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); + return fftw_plan_r2r_1d(*N, in, out, FFTW_R2HC, FFTW_ESTIMATE); } /* From 0bd8a71706f15b0e61e09038adad230975fd4b50 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Mon, 7 Oct 2024 15:50:42 +0300 Subject: [PATCH 03/14] relaxed tests such that we check all peaks instead of the first one --- Src/common/FFT.cpp | 13 +++--- Src/modules/imu/imu.cpp | 6 --- Tests/common/test_fft.cpp | 84 ++++++++++++++++++++------------------- 3 files changed, 50 insertions(+), 53 deletions(-) diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index 042a0a2..daae553 100644 --- a/Src/common/FFT.cpp +++ b/Src/common/FFT.cpp @@ -119,7 +119,11 @@ void FFT::find_peaks(uint8_t axis) { // snr is in dB float snr; if (bin_mag_sum - peak_magnitude[peak_new] < 1.0e-19f) { - snr = 0; + if (bin_mag_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])); @@ -135,9 +139,6 @@ void FFT::find_peaks(uint8_t axis) { 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)); - // if (!peak_close && peak_frequencies_prev[peak_prev] > 0) { - // continue; - // } _fft_updated[axis] = true; // keep @@ -203,7 +204,7 @@ float FFT::estimate_peak_freq(float fft[], int peak_index) { float ap = (real[k + 1] * real[k] + imag[k + 1] * imag[k]) / divider; // dp = -ap / (1 – ap) - if (1.0f - ap < 1.0e-19f) { + if (std::fabs(1.0f - ap) < 1.0e-19f) { return -1; } float dp = -ap / (1.f - ap); @@ -212,7 +213,7 @@ float FFT::estimate_peak_freq(float fft[], int peak_index) { float am = (real[k - 1] * real[k] + imag[k - 1] * imag[k]) / divider; // dm = am / (1 – am) - if (1.0f - am < 1.0e-19f) { + if (std::fabs(1.f - am) < 1.0e-19f) { return -1; } float dm = am / (1.f - am); diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index ef1f37d..029383b 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -82,12 +82,6 @@ void ImuModule::spin_once() { pub.publish(); } } - if (HAL_GetTick() % 1000 == 0) { - char buffer[90]; - snprintf(buffer, sizeof(buffer), "%d %d %d\n", int(1000 * fft_accel.peak_snr[0][0]), - int(1000 * fft_accel.peak_snr[1][0]), int(1000 * fft_accel.peak_snr[2][0])); - logger.log_info(buffer); - } } void ImuModule::get_vibration(std::array data) { diff --git a/Tests/common/test_fft.cpp b/Tests/common/test_fft.cpp index fce67d0..4b7feae 100644 --- a/Tests/common/test_fft.cpp +++ b/Tests/common/test_fft.cpp @@ -11,7 +11,6 @@ #include "FFT.hpp" -static constexpr auto ABS_ERR = 0.1f; unsigned int seed = 1; template @@ -63,8 +62,8 @@ struct InitParamMultiSignalWithRes { InitParamOneSignalWithRes OneSignalTestParams[7] = { // 0 - {{InitFFTParamType{ .sample_rate_hz = 100, .n_axes = 1, .window_size = 24}, - InitOneSignParamType{ .sample_rate_hz = 100, .freq_hz = 3, .amplitude = 1}}, + {{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}, @@ -79,14 +78,14 @@ InitParamOneSignalWithRes OneSignalTestParams[7] = { InitOneSignParamType{ .sample_rate_hz = 512, .freq_hz = 100, .amplitude = 10}}, true}, // 4 - {{InitFFTParamType{ .sample_rate_hz = 1024, .n_axes = 3, .window_size = 400}, - InitOneSignParamType{ .sample_rate_hz = 1024, .freq_hz = 100, .amplitude = 1}}, + {{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}, - // 7 + // 6 {{InitFFTParamType{ .sample_rate_hz = 300, .n_axes = 1, .window_size = 200}, InitOneSignParamType{ .sample_rate_hz = 1000, .freq_hz = 100, .amplitude = 1}}, false}, @@ -249,6 +248,7 @@ class TestFFTOneSignalParametrized : public TestFFTBase init_parameters = parameters.parameters; @@ -278,32 +306,18 @@ class TestFFTOneSignalParametrized : public TestFFTBase Date: Tue, 8 Oct 2024 11:36:29 +0300 Subject: [PATCH 04/14] split fft find_peaks into parts, add magnitude into RawIMU mes --- CMakeLists.txt | 9 -- Src/common/FFT.cpp | 146 ++++++++++++----------- Src/common/FFT.hpp | 36 ++++-- Src/modules/imu/imu.cpp | 12 +- Src/platform/stm32g0b1/rfft.hpp | 197 ++++++++++++++++---------------- Src/platform/ubuntu/rfft.hpp | 109 +++++++++--------- Tests/common/test_fft.cpp | 3 +- 7 files changed, 271 insertions(+), 241 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d16c469..dcce0ab 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 Date: Tue, 8 Oct 2024 23:19:53 +0300 Subject: [PATCH 05/14] simplified dominant search logic, add mag multiplier --- Src/common/FFT.cpp | 19 ++++++++----------- Src/common/FFT.hpp | 4 ++-- Src/modules/imu/imu.cpp | 4 ++-- Src/platform/ubuntu/rfft.hpp | 1 + 4 files changed, 13 insertions(+), 15 deletions(-) diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index 7d77c80..d4cb39d 100644 --- a/Src/common/FFT.cpp +++ b/Src/common/FFT.cpp @@ -50,7 +50,7 @@ void FFT::update(float *input) { sizeof(real_t) * overlap_start * 3); _fft_buffer_index[axis] = overlap_start * 3; } - find_dominant(); + _find_dominant(); } void FFT::find_peaks(uint8_t axis) { @@ -61,12 +61,9 @@ void FFT::find_peaks(uint8_t axis) { 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}; - fft::get_real_imag_by_index(_fft_output_buffer.data(), real_imag, size, fft_index); - float real_f, imag_f; - 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); + 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); + const float fft_magnitude = sqrtf(real * real + imag * imag); _peak_magnitudes_all[fft_index] = fft_magnitude; bin_mag_sum += fft_magnitude; } @@ -88,7 +85,7 @@ void FFT::find_peaks(uint8_t axis) { } } -void FFT::find_dominant() { +void FFT::_find_dominant() { if (!is_updated()) { return; } @@ -126,7 +123,7 @@ void FFT::_identify_peaks_bins(float peak_magnitude[MAX_NUM_PEAKS], if (largest_peak_index > 0) { raw_peak_index[i] = largest_peak_index; - peak_magnitude[i] = _peak_magnitudes_all[largest_peak_index]; + peak_magnitude[i] = largest_peak; // remove peak + sides (included in frequency estimate later) _peak_magnitudes_all[largest_peak_index - 1] = 0; _peak_magnitudes_all[largest_peak_index] = 0; @@ -145,7 +142,7 @@ uint16_t FFT::_estimate_peaks(float* peak_magnitude, continue; } float adjusted_bin = 0.5f * - estimate_peak_freq(fft, 2 * raw_peak_index[peak_new]); + _estimate_peak_freq(fft, 2 * raw_peak_index[peak_new]); if (adjusted_bin > size || adjusted_bin < 0) { continue; } @@ -196,7 +193,7 @@ static constexpr float tau(float x) { return addend_1 - multiplier_2 * addend_2; } -float FFT::estimate_peak_freq(float fft[], int peak_index) { +float FFT::_estimate_peak_freq(float fft[], int peak_index) { if (peak_index < 2 || peak_index >= size) { return -1; } diff --git a/Src/common/FFT.hpp b/Src/common/FFT.hpp index 536d574..f6efbf2 100644 --- a/Src/common/FFT.hpp +++ b/Src/common/FFT.hpp @@ -58,9 +58,9 @@ class FFT { uint8_t n_axes; float _sample_rate_hz; - float estimate_peak_freq(float fft[], int peak_index); + float _estimate_peak_freq(float fft[], int peak_index); void find_peaks(uint8_t axis); - void find_dominant(); + 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]); diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index 8ea4e4a..6bf818c 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -102,7 +102,7 @@ void ImuModule::update_accel_fft() { } fft_accel.update(accel.data()); pub.msg.accelerometer_integral[0] = fft_accel.dominant_frequency; - pub.msg.accelerometer_integral[1] = fft_accel.dominant_mag; + pub.msg.accelerometer_integral[1] = fft_accel.dominant_mag * 1000; pub.msg.accelerometer_integral[2] = fft_accel.dominant_snr; } @@ -112,6 +112,6 @@ void ImuModule::update_gyro_fft() { } fft_gyro.update(gyro.data()); 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[1] = fft_gyro.dominant_mag * 1000; pub.msg.rate_gyro_integral[2] = fft_gyro.dominant_snr; } diff --git a/Src/platform/ubuntu/rfft.hpp b/Src/platform/ubuntu/rfft.hpp index 78d17b5..3ba897f 100644 --- a/Src/platform/ubuntu/rfft.hpp +++ b/Src/platform/ubuntu/rfft.hpp @@ -56,6 +56,7 @@ namespace fft { 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. From 88acc990120167137b94e948f3f610a18143818c Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Wed, 9 Oct 2024 09:24:55 +0300 Subject: [PATCH 06/14] 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 Date: Wed, 9 Oct 2024 11:24:43 +0300 Subject: [PATCH 07/14] resolve sonarcloud issues --- .github/workflows/tests.yml | 5 + Makefile | 3 + Src/common/FFT.cpp | 31 +++--- Src/common/FFT.hpp | 3 +- Src/modules/imu/imu.cpp | 2 - Tests/CMakeLists.txt | 11 +- Tests/Makefile | 7 +- Tests/common/test_fft.cpp | 206 +++++++++++++++++------------------- 8 files changed, 125 insertions(+), 143 deletions(-) diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index c1cd804..d023669 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -19,6 +19,11 @@ jobs: - name: Install gtest run: | wget https://gist.githubusercontent.com/Ponomarevda/d970c24de8deab5d6ccfee8f5f719bcc/raw/install_gtest_ubuntu.sh && chmod +x install_gtest_ubuntu.sh && ./install_gtest_ubuntu.sh + sudo apt-get install -y make gcc-arm-none-eabi + + -name: Install fftw3 + run: | + wget https://gist.githubusercontent.com/AsiiaPine/89276529c5b13be04264f2fe8b4e6c4f/raw/f200b348b1a8e04b2cdf6cc09c619e6520ce0562/install_fftw3.sh && chmod +x install_fftw3.sh && ./install_fftw3.sh - name: Build and run tests run: make tests diff --git a/Makefile b/Makefile index 43eefe8..2c66357 100644 --- a/Makefile +++ b/Makefile @@ -58,6 +58,9 @@ v3: checks generate_dsdl clean checks: @python scripts/prebuild_check.py || (echo "Requirements verification failed. Stopping build." && exit 1) +coverage: + cd Tests && $(MAKE) -s coverage + code_style: cpplint Src/modules/*/*pp Src/peripheral/*/*pp Src/platform/*/*pp diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index f005c0f..24a19e8 100644 --- a/Src/common/FFT.cpp +++ b/Src/common/FFT.cpp @@ -24,8 +24,8 @@ 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]; - rfft::convert_float_to_real_t(input, conv_input, n_axes); + std::array conv_input; + rfft::convert_float_to_real_t(input, conv_input.data(), n_axes); for (uint8_t axis = 0; axis < n_axes; axis++) { uint16_t &buffer_index = _fft_buffer_index[axis]; @@ -52,25 +52,25 @@ void FFT::update(float *input) { } void FFT::find_peaks(uint8_t axis) { - float fft_output_buffer_float[size]; - rfft::convert_real_t_to_float(_fft_output_buffer.data(), fft_output_buffer_float, size); + std::array fft_output_buffer_float; + rfft::convert_real_t_to_float(_fft_output_buffer.data(), fft_output_buffer_float.data(), 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 = rfft::get_real_by_index(fft_output_buffer_float, fft_index); - auto imag = rfft::get_imag_by_index(fft_output_buffer_float, fft_index); + auto real = rfft::get_real_by_index(fft_output_buffer_float.data(), fft_index); + auto imag = rfft::get_imag_by_index(fft_output_buffer_float.data(), fft_index); const float fft_magnitude = sqrtf(real * real + imag * imag); _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] {}; + std::array peak_magnitude; + std::array raw_peak_index; - _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); + _identify_peaks_bins(peak_magnitude.data(), raw_peak_index.data()); + auto num_peaks_found = _estimate_peaks(peak_magnitude.data(), raw_peak_index.data(), + fft_output_buffer_float.data(), bin_mag_sum, axis); // reset values if no peaks found _fft_updated[axis] = true; if (num_peaks_found == 0) { @@ -183,23 +183,22 @@ uint16_t FFT::_estimate_peaks(float* peak_magnitude, return num_peaks_found; } +// tau(x) = 1/4 * log(3*x^2 + 6*x + 1) - sqrt(6)/24*log((x + 1 - sqrt(2/3))/(x + 1 + sqrt(2/3))) static constexpr float tau(float x) { - // tau(x) = 1/4 * log(3*x*x + 6*x + 1) - sqrt(6)/24*log((x + 1 - sqrt(2/3))/(x + 1 + sqrt(2/3))) float addend_1 = 1/4 * logf(3 * x * x + 6 * x + 1); float multiplier_2 = sqrtf(6) / 24; float addend_2 = logf((x + 1 - sqrtf(2 / 3)) / (x + 1 + sqrtf(2 / 3))); return addend_1 - multiplier_2 * addend_2; } +// find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/) float FFT::_estimate_peak_freq(float fft[], int peak_index) { 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[3]; - float imag[3]; + std::array real; + std::array imag; for (int i = -1; i < 2; 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); diff --git a/Src/common/FFT.hpp b/Src/common/FFT.hpp index f6efbf2..4b72109 100644 --- a/Src/common/FFT.hpp +++ b/Src/common/FFT.hpp @@ -58,10 +58,9 @@ class FFT { uint8_t n_axes; float _sample_rate_hz; - float _estimate_peak_freq(float fft[], int peak_index); void find_peaks(uint8_t axis); + float _estimate_peak_freq(float fft[], int peak_index); 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, diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index 61f6339..eab432e 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -11,8 +11,6 @@ REGISTER_MODULE(ImuModule) -Logging logger{"IMU"}; - void ImuModule::init() { bool imu_initialized = imu.initialize(); mode = Module::Mode::STANDBY; diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index b1c7aa2..60ec641 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -3,6 +3,7 @@ project(tests CXX C) set(TESTS_DIR ${CMAKE_CURRENT_LIST_DIR}) cmake_path(GET TESTS_DIR PARENT_PATH ROOT_DIR) +set(UNIT_TESTS_DIR ${ROOT_DIR}/Tests) # Check if the COVERAGE flag is set option(COVERAGE "Enable coverage flags" OFF) @@ -24,25 +25,15 @@ FILE(GLOB commonSources ${ROOT_DIR}/Src/common/FFT.cpp ${ROOT_DIR}/Src/common/algorithms.cpp) function(gen_test app_name test_file) - # Create the executable target add_executable(${app_name} ${commonSources} ${test_file}) - # Include directories for the target target_include_directories(${app_name} PUBLIC ${ROOT_DIR}/Src/common ${ROOT_DIR}/Src/platform/ubuntu) - # Link libraries to the target target_link_libraries(${app_name} GTest::gtest GTest::gtest_main ${FFTW_LIBRARIES} m stdc++) - # Printing the target properties (optional, can be removed) - get_target_property(var ${app_name} INCLUDE_DIRECTORIES) - message(STATUS "${app_name} include directories: ${var}") endfunction() -# Define the path to the test files directory -set(UNIT_TESTS_DIR ${ROOT_DIR}/Tests) - -# Generate test executables gen_test(fft ${UNIT_TESTS_DIR}/common/test_fft.cpp) gen_test(algorithms ${UNIT_TESTS_DIR}/common/test_algorithms.cpp) diff --git a/Tests/Makefile b/Tests/Makefile index 2cb49a4..efa7400 100644 --- a/Tests/Makefile +++ b/Tests/Makefile @@ -4,8 +4,7 @@ # file, You can obtain one at https://mozilla.org/MPL/2.0/. CRNT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) -TESTS_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) -REPO_DIR := $(abspath $(dir $(lastword $(TESTS_DIR)))) +REPO_DIR := $(abspath $(dir $(lastword $(CRNT_DIR)))) BUILD_DIR=${REPO_DIR}/build/Tests $(info CRNT_DIR: ${CRNT_DIR}) @@ -29,10 +28,10 @@ coverage: create_build_dir cd ${BUILD_DIR} && ./fft echo "\n1. Algorithms:" - cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/algorithms.dir${REPO_DIR}/common/algorithms.cpp.gcda + cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/algorithms.dir/common/test_algorithms.cpp.gcda echo "\n2. FFT:" - cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/fft.dir${REPO_DIR}/common/fft.c.gcda + cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/fft.dir/common/test_fft.cpp.gcda algorithms: create_build_dir cd ${BUILD_DIR} && cmake -S ${CRNT_DIR} && make && ./algorithms diff --git a/Tests/common/test_fft.cpp b/Tests/common/test_fft.cpp index af82d8d..576eadd 100644 --- a/Tests/common/test_fft.cpp +++ b/Tests/common/test_fft.cpp @@ -5,8 +5,9 @@ */ #include -#include // For std::fabs #include // For std::clamp +#include +#include // For std::fabs #include // for tuple #include "FFT.hpp" @@ -63,23 +64,17 @@ struct InitParamMultiSignalWithRes { class SinSignalGenerator { public: SinSignalGenerator(){} - explicit SinSignalGenerator(InitOneSignParamType signal_parameters) { - this->sample_rate_hz = signal_parameters.sample_rate_hz; - this->freq_hz = signal_parameters.freq_hz; - this->amplitude = signal_parameters.amplitude; - this->phase = 0; - this->secs = 0; - } - SinSignalGenerator(float sample_rate_hz, float freq_hz, float amplitude) { - this->sample_rate_hz = sample_rate_hz; - this->freq_hz = freq_hz; - this->amplitude = amplitude; - this->phase = 0; - this->secs = 0; - } + explicit SinSignalGenerator(InitOneSignParamType signal_parameters) : + sample_rate_hz(signal_parameters.sample_rate_hz), + freq_hz(signal_parameters.freq_hz), + amplitude(signal_parameters.amplitude) {} + SinSignalGenerator(float sample_rate_hz, float freq_hz, float amplitude) : + sample_rate_hz(sample_rate_hz), + freq_hz(freq_hz), + amplitude(amplitude) {} float get_next_sample() { auto sin = sinf(2 * M_PI * freq_hz * secs + phase); - float sample = (float)amplitude * sin; + float sample = amplitude * sin; secs += 1 / sample_rate_hz; return sample; } @@ -88,48 +83,51 @@ class SinSignalGenerator { private: float sample_rate_hz; - float phase; - float secs; + float phase = 0; + float secs = 0; }; class MultiSignalsSinGenerator { public: - std::vector signals_generator; + uint16_t max_freq; uint8_t n_signals; + uint16_t min_freq = 0; + uint16_t sample_rate_hz; + std::vector signals_generator; std::vector> dominant_sig; void init() { signals_generator.resize(n_signals); uint16_t max_amplitude = 0; for (int j = 0; j < n_signals; j++) { - uint16_t freq_hz = rand_r(&seed) % (max_freq - min_freq) + min_freq; - uint16_t amplitude = 1 + rand_r(&seed) % 100; + uint16_t freq_hz = std::rand() % (max_freq - min_freq) + min_freq; + uint16_t amplitude = 1 + std::rand() % 100; signals_generator[j] = SinSignalGenerator(sample_rate_hz, freq_hz, amplitude); if (amplitude > max_amplitude) { max_amplitude = amplitude; dominant_sig.insert(dominant_sig.begin(), std::make_tuple(amplitude, freq_hz)); } } - for (int j = 0; j < dominant_sig.size(); j++) { - printf("dominant freq: %d\n", std::get<1>(dominant_sig[j])); + for (auto dominant : dominant_sig) { + std::cout <<"dominant freq: " << std::get<1>(dominant) << "\n"; } // sort by amplitude value std::sort(dominant_sig.begin(), dominant_sig.end()); } - MultiSignalsSinGenerator() {} + MultiSignalsSinGenerator() = default; - MultiSignalsSinGenerator(uint8_t n_signals, uint16_t sample_rate_hz, uint16_t max_freq) { - this->n_signals = n_signals; - this->sample_rate_hz = sample_rate_hz; - this->max_freq = max_freq; + MultiSignalsSinGenerator(uint8_t n_signals, uint16_t sample_rate_hz, uint16_t max_freq) : + n_signals(n_signals), + sample_rate_hz(sample_rate_hz), + max_freq(max_freq) { init(); } - explicit MultiSignalsSinGenerator(InitMultiSignalsParamType parameters) { - this->n_signals = parameters.n_signals; - this->sample_rate_hz = parameters.sample_rate_hz; - this->max_freq = parameters.max_freq; + explicit MultiSignalsSinGenerator(InitMultiSignalsParamType parameters) : + n_signals(parameters.n_signals), + sample_rate_hz(parameters.sample_rate_hz), + max_freq(parameters.max_freq) { init(); } @@ -140,17 +138,11 @@ class MultiSignalsSinGenerator { } return sample; } - -private: - uint16_t max_freq; - uint16_t min_freq = 0; - uint16_t sample_rate_hz; }; template class TestFFTBase : public ::testing::Test { public: - /* data */ FFT fft; std::vector signals_generator; InitFFTParamType fft_parameters; @@ -158,7 +150,6 @@ class TestFFTBase : public ::testing::Test { float abs_error; void print_fft_parameters() { - // Print the FFT parameters std::cout << "FFT Parameters: " << std::endl; std::cout << " Sample Rate (Hz): " << fft_parameters.sample_rate_hz << std::endl; std::cout << " Window Size: " << fft_parameters.window_size << std::endl; @@ -180,12 +171,10 @@ class TestFFTOneSignalParametrized : public TestFFTBase, public testing::WithParamInterface { public: - /* data */ bool result; bool is_heat_all_peaks{true}; void print_signal_parameters() { - // Print the signal parameters std::cout << "Signal Parameters: " << std::endl; std::cout << " Sample Rate (Hz): " << signals_parameters[0].sample_rate_hz << std::endl; @@ -196,37 +185,38 @@ class TestFFTOneSignalParametrized : public TestFFTBase init_parameters = parameters.parameters; - result = parameters.result; + InitParamType init_parameters = GetParam().parameters; + result = GetParam().result; fft_parameters = init_parameters.fft_parameters; // expand signal_parameters to multiple axes if needed signals_parameters.resize(fft_parameters.n_axes); @@ -239,7 +229,8 @@ class TestFFTOneSignalParametrized : public TestFFTBase OneSignalTestParams = { +{ // 0 {{InitFFTParamType{ .sample_rate_hz = 100, .n_axes = 1, .window_size = 50}, InitOneSignParamType{ .sample_rate_hz = 100, .freq_hz = 3, .amplitude = 10}}, @@ -268,6 +259,7 @@ InitParamOneSignalWithRes OneSignalTestParams[7] = { {{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) { @@ -279,18 +271,14 @@ TEST_P(TestFFTOneSignalParametrized, CheckOnWindow) { fft.update(input); } print_fft_results(); - check_values(); - if (result) { - EXPECT_TRUE(is_heat_all_peaks); - } else { - ASSERT_FALSE(is_heat_all_peaks); + for (int axis = 0; axis < fft_parameters.n_axes; axis++) { + check_axis(axis); } } TEST_P(TestFFTOneSignalParametrized, CheckOnFewWindows) { float input[fft_parameters.n_axes]; - auto n_updates = (rand_r(&seed) % 8 + 2) * fft_parameters.window_size; - // auto n_updates = (2 + rand_r(&seed) % 10) * fft_parameters.window_size; + auto n_updates = (std::rand() % 8 + 2) * fft_parameters.window_size; for (int i = 0; i < n_updates + 10; i++) { for (int j = 0; j < fft_parameters.n_axes; j++) { input[j] = signals_generator[j].get_next_sample(); @@ -298,11 +286,8 @@ TEST_P(TestFFTOneSignalParametrized, CheckOnFewWindows) { fft.update(input); } print_fft_results(); - check_values(); - if (result) { - EXPECT_TRUE(is_heat_all_peaks); - } else { - ASSERT_FALSE(is_heat_all_peaks); + for (int axis = 0; axis < fft_parameters.n_axes; axis++) { + check_axis(axis); } } @@ -317,24 +302,21 @@ class TestFFTOnMultiSignalsParametrized : public TestFFTBase, public ::testing::WithParamInterface { public: - /* data */ bool result; std::vector input; void print_signal_parameters() { - // Print the signal parameters std::cout << "Signal Parameters: " << std::endl; std::cout << " Sample Rate (Hz): " << signals_parameters[0].sample_rate_hz << std::endl; - std::cout << " Number of Signals: " << signals_parameters[0].n_signals + std::cout << " Number of Signals: " << (int)signals_parameters[0].n_signals << std::endl; std::cout << " Max Freq: " << signals_parameters[0].max_freq << std::endl; } void SetUp() override { - auto parameters = GetParam(); - InitParamType init_parameters = parameters.parameters; - result = parameters.result; + InitParamType init_parameters = GetParam().parameters; + result = GetParam().result; fft_parameters = init_parameters.fft_parameters; signals_parameters.resize(fft_parameters.n_axes); input.resize(fft_parameters.n_axes); @@ -345,11 +327,42 @@ class TestFFTOnMultiSignalsParametrized : public TestFFTBase(dominant)) << "\n"; + } + std::cout << "peak freq: \n"; + for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { + std::cout << fft.peak_frequencies[axis][peak_index] << "\n"; + for (auto dominant : signal_generator.dominant_sig) { + if (IsBetweenInclusive(fft.peak_frequencies[axis][peak_index], + (int)std::get<1>(dominant) - abs_error, + (int)std::get<1>(dominant) + abs_error)) { + heat_peak = true; + break; + } + } + if (heat_peak) { + break; + } + } + if (result) { + EXPECT_TRUE(heat_peak); + } else { + ASSERT_FALSE(heat_peak); + } + } }; -InitParamMultiSignalWithRes MultiSignalTestParams[8] = { +const std::array MultiSignalTestParams = { // 0 - {{InitFFTParamType{ .sample_rate_hz = 24, .n_axes = 1, .window_size = 24}, + {{{InitFFTParamType{ .sample_rate_hz = 24, .n_axes = 1, .window_size = 24}, InitMultiSignalsParamType{ .sample_rate_hz = 24, .n_signals = 2, .max_freq = 12}}, true}, // 1 @@ -380,6 +393,7 @@ InitParamMultiSignalWithRes MultiSignalTestParams[8] = { {{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) { @@ -390,33 +404,7 @@ TEST_P(TestFFTOnMultiSignalsParametrized, CheckOnWindow) { fft.update(input.data()); } for (int axis = 0; axis < fft_parameters.n_axes; axis++) { - bool heat_peak; - auto signal_generator = signals_generator[axis]; - auto n_dominants = signal_generator.dominant_sig.size(); - auto n_signals = signal_generator.n_signals; - printf("abs error: %f\n", abs_error); - for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { - printf("peak index: %d\n", peak_index); - printf("fft peak freq: %f\n", fft.peak_frequencies[axis][peak_index]); - for (int j = 0; j < n_dominants; j++) { - auto dominant_freq = signal_generator.dominant_sig[j]; - printf("dominant freq: %d\n", std::get<1>(dominant_freq)); - if (IsBetweenInclusive(fft.peak_frequencies[axis][peak_index], - std::get<1>(dominant_freq) - abs_error, - std::get<1>(dominant_freq) + abs_error)) { - heat_peak = true; - break; - } - } - if (heat_peak) { - break; - } - } - if (result) { - EXPECT_TRUE(heat_peak); - } else { - ASSERT_FALSE(heat_peak); - } + check_axis(axis); } } From 45909f4c22740b29735f9504fa305521703d8146 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Tue, 12 Nov 2024 09:52:11 +0300 Subject: [PATCH 08/14] hot fix of imu initialization error --- Src/modules/imu/imu.cpp | 6 ++++-- Src/modules/imu/params.yaml | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index eab432e..69099c7 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -12,9 +12,8 @@ REGISTER_MODULE(ImuModule) void ImuModule::init() { - bool imu_initialized = imu.initialize(); + initialized = imu.initialize(); mode = Module::Mode::STANDBY; - initialized = imu_initialized; fft_accel.init(512, 3, 512); fft_accel.fft_min_freq = 20; fft_gyro.init(512, 3, 512); @@ -33,6 +32,9 @@ void ImuModule::update_params() { } void ImuModule::spin_once() { + if (!initialized) { + initialized = imu.initialize(); + } if (!bitmask || !initialized) { return; } diff --git a/Src/modules/imu/params.yaml b/Src/modules/imu/params.yaml index 8f19024..ddc0cc7 100644 --- a/Src/modules/imu/params.yaml +++ b/Src/modules/imu/params.yaml @@ -11,7 +11,7 @@ imu.mode: flags: mutable default: 0 min: 0 - max: 16 + max: 15 imu.pub_frequency: type: Integer From 999afb571694469f7ee781eb6b19e11d4d9dc9ce Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Tue, 19 Nov 2024 10:35:44 +0300 Subject: [PATCH 09/14] rmv makefile changes --- Makefile | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/Makefile b/Makefile index 2c66357..6622be5 100644 --- a/Makefile +++ b/Makefile @@ -1,11 +1,5 @@ # Copyright (C) 2023-2024 Dmitry Ponomarev # Distributed under the terms of the GPL v3 license, available in the file LICENSE. -# Variables -CC := arm-none-eabi-gcc -CFLAGS := -g -Og - -# Add compile flags to cmake -CMAKE_FLAGS := "-DCMAKE_C_COMPILER=${CC} -DCMAKE_CXX_COMPILER=${CC} -DCMAKE_BUILD_TYPE=Debug" ROOT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) BUILD_DIR:=$(ROOT_DIR)/build @@ -44,7 +38,7 @@ sitl_dronecan: checks clean cd ${BUILD_DIR}/dronecan_sitl/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_UBUNTU=ON -G "Unix Makefiles" ../../.. && make dronecan_v3: checks clean mkdir -p ${BUILD_DIR}/dronecan_v3/obj - cd ${BUILD_DIR}/dronecan_v3/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_NODE_V3=ON ${CMAKE_FLAGS} -G "Unix Makefiles" ../../.. && make + cd ${BUILD_DIR}/dronecan_v3/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_NODE_V3=ON -G "Unix Makefiles" ../../.. && make # Cyphal & DroneCAN v2: checks generate_dsdl clean @@ -80,4 +74,4 @@ clean_releases: clean: -rm -fR ${BUILD_DIR}/*/obj distclean: - -rm -fR ${BUILD_DIR}/ + -rm -fR ${BUILD_DIR}/ \ No newline at end of file From d298fc733cb9ba08099a16b7870067925710a5e5 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Tue, 19 Nov 2024 10:40:54 +0300 Subject: [PATCH 10/14] . --- Libs/Dronecan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Libs/Dronecan b/Libs/Dronecan index 663ca38..8cf4a58 160000 --- a/Libs/Dronecan +++ b/Libs/Dronecan @@ -1 +1 @@ -Subproject commit 663ca3885a4db03894c21d896a1224462cac3f68 +Subproject commit 8cf4a5807276951edf2a67e152c7360c2bdeb278 From 7d7fb3e07454c79d5a8e70b42d742f53f15dd731 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Tue, 19 Nov 2024 10:45:23 +0300 Subject: [PATCH 11/14] upd dronecan after rebase --- Libs/Dronecan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Libs/Dronecan b/Libs/Dronecan index 8cf4a58..663ca38 160000 --- a/Libs/Dronecan +++ b/Libs/Dronecan @@ -1 +1 @@ -Subproject commit 8cf4a5807276951edf2a67e152c7360c2bdeb278 +Subproject commit 663ca3885a4db03894c21d896a1224462cac3f68 From 3a3756db81dbd3fb30187c6279f18b03f46595ac Mon Sep 17 00:00:00 2001 From: Anastasiia Stepanova Date: Mon, 25 Nov 2024 12:09:29 +0300 Subject: [PATCH 12/14] fft test used secure rand --- Tests/CMakeLists.txt | 3 +-- Tests/common/test_fft.cpp | 36 ++++++++++++++++++------------------ 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index 60ec641..4a4d21b 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -21,8 +21,7 @@ include_directories(${GTEST_INCLUDE_DIR}) find_package(PkgConfig REQUIRED) pkg_search_module(FFTW REQUIRED fftw3) include_directories(${FFTW_INCLUDE_DIRS}) -FILE(GLOB commonSources ${ROOT_DIR}/Src/common/FFT.cpp - ${ROOT_DIR}/Src/common/algorithms.cpp) +FILE(GLOB commonSources ${ROOT_DIR}/Src/common/FFT.cpp) function(gen_test app_name test_file) add_executable(${app_name} diff --git a/Tests/common/test_fft.cpp b/Tests/common/test_fft.cpp index 576eadd..be15abb 100644 --- a/Tests/common/test_fft.cpp +++ b/Tests/common/test_fft.cpp @@ -5,15 +5,15 @@ */ #include +#include #include // For std::clamp #include #include // For std::fabs #include // for tuple - #include "FFT.hpp" unsigned int seed = 1; - +std::random_device rd; // Will be used to obtain a seed for the random number engine template auto IsInRange(T lo, T hi) { return AllOf(Ge((lo)), Le((hi))); @@ -99,18 +99,17 @@ class MultiSignalsSinGenerator { void init() { signals_generator.resize(n_signals); uint16_t max_amplitude = 0; + std::uniform_int_distribution dist(0, max_freq); + for (int j = 0; j < n_signals; j++) { - uint16_t freq_hz = std::rand() % (max_freq - min_freq) + min_freq; - uint16_t amplitude = 1 + std::rand() % 100; + uint16_t freq_hz = dist(rd) % (max_freq - min_freq) + min_freq; + uint16_t amplitude = 1 + dist(rd) % 100; signals_generator[j] = SinSignalGenerator(sample_rate_hz, freq_hz, amplitude); if (amplitude > max_amplitude) { max_amplitude = amplitude; dominant_sig.insert(dominant_sig.begin(), std::make_tuple(amplitude, freq_hz)); } } - for (auto dominant : dominant_sig) { - std::cout <<"dominant freq: " << std::get<1>(dominant) << "\n"; - } // sort by amplitude value std::sort(dominant_sig.begin(), dominant_sig.end()); } @@ -223,8 +222,9 @@ class TestFFTOneSignalParametrized : public TestFFTBase dist(0, 8); + auto n_updates = (dist(rd) % 8 + 2) * fft_parameters.window_size; for (int i = 0; i < n_updates + 10; i++) { for (int j = 0; j < fft_parameters.n_axes; j++) { input[j] = signals_generator[j].get_next_sample(); } fft.update(input); } - print_fft_results(); + // To print FFT results for debug uncomment the following line + // print_fft_results(); for (int axis = 0; axis < fft_parameters.n_axes; axis++) { check_axis(axis); } @@ -323,8 +326,9 @@ class TestFFTOnMultiSignalsParametrized : public TestFFTBase(dominant)) << "\n"; } - std::cout << "peak freq: \n"; for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { - std::cout << fft.peak_frequencies[axis][peak_index] << "\n"; for (auto dominant : signal_generator.dominant_sig) { if (IsBetweenInclusive(fft.peak_frequencies[axis][peak_index], (int)std::get<1>(dominant) - abs_error, From 81f4bca3d4198e06b9bdc2fe03311c27e6a81909 Mon Sep 17 00:00:00 2001 From: Anastasiia Stepanova Date: Mon, 25 Nov 2024 14:19:44 +0300 Subject: [PATCH 13/14] add logs to imu driver --- Src/drivers/mpu9250/mpu9250.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/Src/drivers/mpu9250/mpu9250.cpp b/Src/drivers/mpu9250/mpu9250.cpp index 07a3b66..18c58fa 100644 --- a/Src/drivers/mpu9250/mpu9250.cpp +++ b/Src/drivers/mpu9250/mpu9250.cpp @@ -6,8 +6,9 @@ #include "mpu9250.hpp" #include +#include #include "peripheral/spi/spi.hpp" - +#include // Register Map for Gyroscope and Accelerometer enum class Mpu9250Resgiter : uint8_t { ACCEL_XOUT_H = 0x3B, @@ -31,9 +32,13 @@ bool Mpu9250::initialize() { } int8_t Mpu9250::read_accelerometer(std::array* accel) const { + static Logging logger("Mpu9250::read_gyroscope"); std::array buffer; auto reg = std::byte(Mpu9250Resgiter::ACCEL_XOUT_H); if (auto res = HAL::SPI::read_registers(reg, buffer.data(), 6); res < 0) { + char msg[10]; + snprintf(msg, sizeof(msg), "acc: %d", res); + logger.log_error(msg); return res; } @@ -45,9 +50,13 @@ int8_t Mpu9250::read_accelerometer(std::array* accel) const { } int8_t Mpu9250::read_gyroscope(std::array* gyro) const { + static Logging logger("Mpu9250::read_gyroscope"); std::array buffer; auto reg = std::byte(Mpu9250Resgiter::GYRO_XOUT_H); if (auto res = HAL::SPI::read_registers(reg, buffer.data(), buffer.size()); res < 0) { + char msg[10]; + snprintf(msg, sizeof(msg), "gyr: %d", res); + logger.log_error(msg); return res; } From 601f79786e66647b5864b1b83a0f628f7220bcf7 Mon Sep 17 00:00:00 2001 From: Anastasiia Stepanova Date: Mon, 9 Dec 2024 09:16:42 +0300 Subject: [PATCH 14/14] add debug flag --- CMakeLists.txt | 7 +++++-- Makefile | 13 +++++++++++++ Src/common/FFT.cpp | 10 ++++++++++ Src/modules/imu/imu.cpp | 6 +++--- Src/modules/imu/imu.hpp | 2 +- 5 files changed, 32 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dcce0ab..465badf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,8 +61,11 @@ 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_C_FLAGS "${CMAKE_DEBUG_FLAGS} ${CMAKE_C_FLAGS} ${WARNING_FLAGS}") +set(CMAKE_CXX_FLAGS "-g ${CMAKE_CXX_FLAGS} ${WARNING_FLAGS} -Wno-volatile") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${WARNING_FLAGS} -Wno-volatile") +# set(CMAKE_C_FLAGS_DEBUG "-g -DDEBUG") +# set(CMAKE_CXX_FLAGS_DEBUG "-g -DDEBUG") set(CMAKE_CXX_STANDARD 20) # Prebuild diff --git a/Makefile b/Makefile index 6622be5..ff55467 100644 --- a/Makefile +++ b/Makefile @@ -4,6 +4,15 @@ ROOT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST)))) BUILD_DIR:=$(ROOT_DIR)/build +# CFLAGS := "-g -O0 -o -DDEBUG" +CC := arm-none-eabi-gcc +CFLAGS := "-g -O0 -o -DDEBUG" + +# Make sure these flags are used in the cmake build command as well +CMAKE_DEBUG_FLAGS := -DCMAKE_C_COMPILER=${CC} -DCMAKE_CXX_COMPILER=${CC} -DCMAKE_C_FLAGS=${CFLAGS} -DCMAKE_CXX_FLAGS=${CFLAGS} -DCMAKE_BUILD_TYPE=Debug +# # Make sure these flags are used in the cmake build command as well +# CMAKE_DEBUG_FLAGS := -DCMAKE_C_FLAGS=${CFLAGS} -DCMAKE_CXX_FLAGS=${CFLAGS} + all: clean_releases cyphal_v2 cyphal_v3 dronecan_v2 dronecan_v3 v3 sitl_dronecan sitl_cyphal # Cyphal @@ -40,6 +49,10 @@ dronecan_v3: checks clean mkdir -p ${BUILD_DIR}/dronecan_v3/obj cd ${BUILD_DIR}/dronecan_v3/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_NODE_V3=ON -G "Unix Makefiles" ../../.. && make +dronecan_v3_debug: checks clean + mkdir -p ${BUILD_DIR}/dronecan_v3/obj + cd ${BUILD_DIR}/dronecan_v3/obj && cmake -DCAN_PROTOCOL=dronecan -DUSE_PLATFORM_NODE_V3=ON ${CMAKE_DEBUG_FLAGS} -G "Unix Makefiles" ../../.. && make + # Cyphal & DroneCAN v2: checks generate_dsdl clean mkdir -p ${BUILD_DIR}/both_v2/obj diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index 24a19e8..640d3f0 100644 --- a/Src/common/FFT.cpp +++ b/Src/common/FFT.cpp @@ -8,6 +8,7 @@ #include #include #include "FFT.hpp" +#include "common/logging.hpp" bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) { if (window_size > FFT_MAX_SIZE) { @@ -26,6 +27,8 @@ bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) { void FFT::update(float *input) { std::array conv_input; rfft::convert_float_to_real_t(input, conv_input.data(), n_axes); + static Logging logger{"FFT"}; + static char buffer[30]; for (uint8_t axis = 0; axis < n_axes; axis++) { uint16_t &buffer_index = _fft_buffer_index[axis]; @@ -42,6 +45,9 @@ void FFT::update(float *input) { rfft::rfft_one_cycle(rfft_spec, _fft_input_buffer.data(), _fft_output_buffer.data()); find_peaks(axis); + snprintf(buffer, sizeof(buffer), "%d, %d %d", axis, (int)_fft_buffer_index[axis], + (int)is_updated()); + logger.log_info(buffer); // 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], @@ -99,6 +105,10 @@ void FFT::_find_dominant() { } } } + static char buffer[30]; + snprintf(buffer, sizeof(buffer), "updated dominant %d", size); + static Logging logger{"FFT"}; + logger.log_info(buffer); } void FFT::_identify_peaks_bins(float peak_magnitude[MAX_NUM_PEAKS], diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index 69099c7..610bb0e 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -14,9 +14,9 @@ REGISTER_MODULE(ImuModule) void ImuModule::init() { initialized = imu.initialize(); mode = Module::Mode::STANDBY; - fft_accel.init(512, 3, 512); + fft_accel.init(256, 3, 256); fft_accel.fft_min_freq = 20; - fft_gyro.init(512, 3, 512); + fft_gyro.init(256, 3, 256); fft_gyro.fft_min_freq = 20; } @@ -108,7 +108,7 @@ void ImuModule::update_gyro_fft() { if (!(bitmask & static_cast>(Bitmask::ENABLE_FFT_GYR))) { return; } - fft_gyro.update(gyro.data()); + // fft_gyro.update(gyro.data()); pub.msg.rate_gyro_integral[0] = fft_gyro.dominant_frequency; pub.msg.rate_gyro_integral[1] = fft_gyro.dominant_mag * 1000; pub.msg.rate_gyro_integral[2] = fft_gyro.dominant_snr; diff --git a/Src/modules/imu/imu.hpp b/Src/modules/imu/imu.hpp index f9da74d..56c6502 100644 --- a/Src/modules/imu/imu.hpp +++ b/Src/modules/imu/imu.hpp @@ -27,7 +27,7 @@ class ImuModule : public Module { ENABLE_FFT_GYR = 8, ENABLE_ALL_BY_DEFAULT = 15, }; - ImuModule() : Module(512.0, Protocol::DRONECAN) {} + ImuModule() : Module(256, Protocol::DRONECAN) {} void init() override; protected: