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/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 b44e9ad..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 @@ -52,6 +65,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 @@ -71,4 +87,4 @@ clean_releases: clean: -rm -fR ${BUILD_DIR}/*/obj distclean: - -rm -fR ${BUILD_DIR}/ + -rm -fR ${BUILD_DIR}/ \ No newline at end of file diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp new file mode 100644 index 0000000..640d3f0 --- /dev/null +++ b/Src/common/FFT.cpp @@ -0,0 +1,246 @@ + +/* + * 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" +#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) { + return false; + } + n_axes = num_axes; + 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; + _resolution_hz = sample_rate_hz / size; + fft_max_freq = _sample_rate_hz / 2; + return true; +} + +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]; + + 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; + } + + rfft::apply_hanning_window(data_buffer[axis].data(), _fft_input_buffer.data(), + _hanning_window.data(), size); + 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], + sizeof(real_t) * overlap_start * 3); + _fft_buffer_index[axis] = overlap_start * 3; + } + _find_dominant(); +} + +void FFT::find_peaks(uint8_t axis) { + 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.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; + } + std::array peak_magnitude; + std::array raw_peak_index; + + _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) { + 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]; + } + } + } + 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], + 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; + + // 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] = 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; + _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, 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 (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] / + (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; + } + 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++; + } + 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) { + 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; + } + + 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); + } + + 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 (std::fabs(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 (std::fabs(1.f - 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..4b72109 --- /dev/null +++ b/Src/common/FFT.hpp @@ -0,0 +1,84 @@ +/* + * 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 8.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 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; + 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; + + uint8_t n_axes; + float _sample_rate_hz; + + void find_peaks(uint8_t axis); + float _estimate_peak_freq(float fft[], int peak_index); + void _find_dominant(); + 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 + 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/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; } diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index df7e1ab..610bb0e 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -7,13 +7,17 @@ #include #include "imu.hpp" #include "params.hpp" +#include "common/logging.hpp" REGISTER_MODULE(ImuModule) void ImuModule::init() { - bool imu_initialized = imu.initialize(); + initialized = imu.initialize(); mode = Module::Mode::STANDBY; - initialized = imu_initialized; + fft_accel.init(256, 3, 256); + fft_accel.fft_min_freq = 20; + fft_gyro.init(256, 3, 256); + fft_gyro.fft_min_freq = 20; } void ImuModule::update_params() { @@ -28,6 +32,9 @@ void ImuModule::update_params() { } void ImuModule::spin_once() { + if (!initialized) { + initialized = imu.initialize(); + } if (!bitmask || !initialized) { return; } @@ -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) { @@ -75,13 +82,34 @@ 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() { + if (!(bitmask & static_cast>(Bitmask::ENABLE_FFT_ACC))) { + return; + } + fft_accel.update(accel.data()); + pub.msg.accelerometer_integral[0] = fft_accel.dominant_frequency; + pub.msg.accelerometer_integral[1] = fft_accel.dominant_mag * 1000; + pub.msg.accelerometer_integral[2] = fft_accel.dominant_snr; +} + +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.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 6d1d22a..56c6502 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_ @@ -11,6 +12,7 @@ #include "module.hpp" #include "publisher.hpp" #include "drivers/mpu9250/mpu9250.hpp" +#include "common/FFT.hpp" #ifdef __cplusplus extern "C" { @@ -25,24 +27,29 @@ class ImuModule : public Module { ENABLE_FFT_GYR = 8, ENABLE_ALL_BY_DEFAULT = 15, }; - ImuModule() : Module(400.0, Protocol::DRONECAN) {} + ImuModule() : Module(256, 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/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 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..35d5dd4 --- /dev/null +++ b/Src/platform/stm32g0b1/rfft.hpp @@ -0,0 +1,118 @@ +/* + * 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_ + +#include "arm_math.h" +#include "arm_const_structs.h" +typedef q15_t real_t; +#define M_2PI 6.28318530717958647692 + +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 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. + */ + 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; + + 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]; + } +} // namespace rfft + +#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..9ed840b --- /dev/null +++ b/Src/platform/ubuntu/rfft.hpp @@ -0,0 +1,91 @@ +/* + * 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_ + +typedef double real_t; +#include +#include +#include +#include + +#define M_2PI 6.28318530717958647692 +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) { + 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; + } + // 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]; + } + } +} // namespace rfft + +#endif // SRC_PLATFORM_UBUNTU_MATH_RFFT_HPP_ diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt new file mode 100644 index 0000000..4a4d21b --- /dev/null +++ b/Tests/CMakeLists.txt @@ -0,0 +1,38 @@ +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) +set(UNIT_TESTS_DIR ${ROOT_DIR}/Tests) + +# 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) + +function(gen_test app_name test_file) + add_executable(${app_name} + ${commonSources} + ${test_file}) + + target_include_directories(${app_name} PUBLIC ${ROOT_DIR}/Src/common ${ROOT_DIR}/Src/platform/ubuntu) + + target_link_libraries(${app_name} GTest::gtest GTest::gtest_main ${FFTW_LIBRARIES} m stdc++) + +endfunction() + +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..efa7400 --- /dev/null +++ b/Tests/Makefile @@ -0,0 +1,40 @@ +# 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)))) +REPO_DIR := $(abspath $(dir $(lastword $(CRNT_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/common/test_algorithms.cpp.gcda + + echo "\n2. FFT:" + 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 + +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..be15abb --- /dev/null +++ b/Tests/common/test_fft.cpp @@ -0,0 +1,420 @@ +/** + * This program is free software under the GNU General Public License v3. + * See for details. + * Author: Anastasiia Stepanova + */ + +#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))); +} +::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; +}; + +class SinSignalGenerator { +public: + SinSignalGenerator(){} + 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 = amplitude * sin; + secs += 1 / sample_rate_hz; + return sample; + } + float freq_hz; + float amplitude; + +private: + float sample_rate_hz; + float phase = 0; + float secs = 0; +}; + +class MultiSignalsSinGenerator { +public: + 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; + std::uniform_int_distribution dist(0, max_freq); + + for (int j = 0; j < n_signals; j++) { + 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)); + } + } + // sort by amplitude value + std::sort(dominant_sig.begin(), dominant_sig.end()); + } + + MultiSignalsSinGenerator() = default; + + 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) : + n_signals(parameters.n_signals), + sample_rate_hz(parameters.sample_rate_hz), + 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; + } +}; + +template +class TestFFTBase : public ::testing::Test { +public: + FFT fft; + std::vector signals_generator; + InitFFTParamType fft_parameters; + std::vector signals_parameters; + float abs_error; + + void print_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: + bool result; + bool is_heat_all_peaks{true}; + + void print_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 print_fft_results() { + std::cout <<"fft resolution: " << fft._resolution_hz << "\n"; + for (int axis = 0; axis < fft_parameters.n_axes; axis++) { + std::cout <<"AXIS " << axis << "\n"; + for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { + std::cout <<"peak index: " << peak_index << "\n"; + std::cout <<"fft peak freq: " << fft.peak_frequencies[axis][peak_index] << "\n"; + std::cout <<"fft peak snr: " << fft.peak_snr[axis][peak_index] << "\n"; + } + } + } + + void check_axis(int axis) { + bool heat_peak = false; + for (int j = 0; j < MAX_NUM_PEAKS; j++) { + auto in_range = IsBetweenInclusive(fft.peak_frequencies[axis][j], + signals_parameters[axis].freq_hz - abs_error, + signals_parameters[axis].freq_hz + abs_error); + if (in_range) { + heat_peak = true; + break; + } + } + if (result) { + EXPECT_TRUE(heat_peak); + } else { + ASSERT_FALSE(heat_peak); + } + } + + void SetUp() override { + 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); + for (int i = 0; i < fft_parameters.n_axes; i++) { + signals_parameters[i] = init_parameters.signals_parameters; + } + // To print FFT parameters for debug uncomment the following line + // print_signal_parameters(); + // print_fft_parameters(); + init(); + } +}; + +const std::array OneSignalTestParams = { +{ + // 0 + {{InitFFTParamType{ .sample_rate_hz = 100, .n_axes = 1, .window_size = 50}, + InitOneSignParamType{ .sample_rate_hz = 100, .freq_hz = 3, .amplitude = 10}}, + true}, + // 1 + {{InitFFTParamType{ .sample_rate_hz = 1000, .n_axes = 1, .window_size = 100}, + InitOneSignParamType{ .sample_rate_hz = 1000, .freq_hz = 5, .amplitude = 10}}, + true}, + // 2 + {{InitFFTParamType{ .sample_rate_hz = 512, .n_axes = 1, .window_size = 512}, + InitOneSignParamType{ .sample_rate_hz = 512, .freq_hz = 100, .amplitude = 10}}, + true}, + // 3 + {{InitFFTParamType{ .sample_rate_hz = 1000, .n_axes = 1, .window_size = 512}, + InitOneSignParamType{ .sample_rate_hz = 1000, .freq_hz = 100, .amplitude = 10}}, + true}, + // 4 + {{InitFFTParamType{ .sample_rate_hz = 1024, .n_axes = 3, .window_size = 512}, + InitOneSignParamType{ .sample_rate_hz = 1024, .freq_hz = 100, .amplitude = 10}}, + true}, + // 5 + {{InitFFTParamType{ .sample_rate_hz = 256, .n_axes = 3, .window_size = 256}, + InitOneSignParamType{ .sample_rate_hz = 256, .freq_hz = 200, .amplitude = 1}}, + false}, + // 6 + {{InitFFTParamType{ .sample_rate_hz = 100, .n_axes = 1, .window_size = 200}, + InitOneSignParamType{ .sample_rate_hz = 1000, .freq_hz = 100, .amplitude = 1}}, + false}, +} +}; + +TEST_P(TestFFTOneSignalParametrized, CheckOnWindow) { + float input[fft_parameters.n_axes]; + for (int i = 0; i < fft_parameters.window_size + 1; i++) { + for (int j = 0; j < fft_parameters.n_axes; j++) { + input[j] = signals_generator[j].get_next_sample(); + } + fft.update(input); + } + // 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); + } +} + +TEST_P(TestFFTOneSignalParametrized, CheckOnFewWindows) { + float input[fft_parameters.n_axes]; + std::uniform_int_distribution 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); + } + // 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); + } +} + +INSTANTIATE_TEST_SUITE_P(TestFFTOneSignalParametrized, + TestFFTOneSignalParametrized, + ::testing::ValuesIn( + OneSignalTestParams) + ); + + +class TestFFTOnMultiSignalsParametrized : public TestFFTBase, + public ::testing::WithParamInterface { +public: + bool result; + std::vector input; + void print_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: " << (int)signals_parameters[0].n_signals + << std::endl; + std::cout << " Max Freq: " << signals_parameters[0].max_freq + << std::endl; + } + + void SetUp() override { + 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); + for (int i = 0; i < fft_parameters.n_axes; i++) { + signals_parameters[i] = init_parameters.signals_parameters; + } + // To print FFT parameters for debug uncomment the following line + // print_signal_parameters(); + // print_fft_parameters(); + init(); + } + + void check_axis(int 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; + for (auto dominant : signal_generator.dominant_sig) { + } + for (int peak_index = 0; peak_index < MAX_NUM_PEAKS; peak_index++) { + 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); + } + } +}; + +const std::array MultiSignalTestParams = { + // 0 + {{{InitFFTParamType{ .sample_rate_hz = 24, .n_axes = 1, .window_size = 24}, + InitMultiSignalsParamType{ .sample_rate_hz = 24, .n_signals = 2, .max_freq = 12}}, + true}, + // 1 + {{InitFFTParamType{ .sample_rate_hz = 512, .n_axes = 1, .window_size = 512}, + InitMultiSignalsParamType{ .sample_rate_hz = 512, .n_signals = 5, .max_freq = 256}}, + true}, + // 2 + {{InitFFTParamType{ .sample_rate_hz = 1000, .n_axes = 1, .window_size = 100}, + InitMultiSignalsParamType{ .sample_rate_hz = 1000, .n_signals = 4, .max_freq = 50}}, + true}, + // 3 + {{InitFFTParamType{ .sample_rate_hz = 2000, .n_axes = 1, .window_size = 256}, + InitMultiSignalsParamType{ .sample_rate_hz = 2000, .n_signals = 10, .max_freq = 128}}, + true}, + // 4 + {{InitFFTParamType{ .sample_rate_hz = 2000, .n_axes = 1, .window_size = 200}, + InitMultiSignalsParamType{ .sample_rate_hz = 2000, .n_signals = 12, .max_freq = 100}}, + true}, + // 5 + {{InitFFTParamType{ .sample_rate_hz = 1024, .n_axes = 1, .window_size = 512}, + InitMultiSignalsParamType{ .sample_rate_hz = 1024, .n_signals = 15, .max_freq = 256}}, + true}, + // 6 + {{InitFFTParamType{ .sample_rate_hz = 256, .n_axes = 3, .window_size = 256}, + InitMultiSignalsParamType{ .sample_rate_hz = 256, .n_signals = 13, .max_freq = 128}}, + true}, + // 7 + {{InitFFTParamType{ .sample_rate_hz = 512, .n_axes = 3, .window_size = 512}, + InitMultiSignalsParamType{ .sample_rate_hz = 512, .n_signals = 10, .max_freq = 256}}, + true}, +} +}; + +TEST_P(TestFFTOnMultiSignalsParametrized, CheckOnWindow) { + for (int i = 0; i < fft_parameters.window_size + 10; i++) { + for (int j = 0; j < fft_parameters.n_axes; j++) { + input[j] = signals_generator[j].get_next_samples(); + } + fft.update(input.data()); + } + for (int axis = 0; axis < fft_parameters.n_axes; axis++) { + check_axis(axis); + } +} + +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 )