From 84e8724b9dac16d21391c914877b9fd4c2905389 Mon Sep 17 00:00:00 2001 From: AsiiaPine Date: Wed, 2 Oct 2024 17:53:41 +0300 Subject: [PATCH] works on stm, add DSP lib to mini_v3 ioc commit --- Libs/mini-v3-ioc | 2 +- Src/common/FFT.cpp | 72 ++++++++++++++++++++------------- Src/common/FFT.hpp | 18 ++++----- Src/drivers/mpu9250/mpu9250.cpp | 2 +- Src/drivers/mpu9250/mpu9250.hpp | 4 ++ Src/modules/imu/imu.cpp | 58 ++++++++++---------------- Src/modules/imu/imu.hpp | 2 +- Src/platform/stm32g0b1/rfft.hpp | 26 ++++++++---- Src/platform/ubuntu/rfft.hpp | 27 ++++++++----- Tests/CMakeLists.txt | 5 --- Tests/Makefile | 3 +- 11 files changed, 119 insertions(+), 100 deletions(-) diff --git a/Libs/mini-v3-ioc b/Libs/mini-v3-ioc index 1bec9cc..3e37a9a 160000 --- a/Libs/mini-v3-ioc +++ b/Libs/mini-v3-ioc @@ -1 +1 @@ -Subproject commit 1bec9cc6ae46c0e5e229540a97a53b25ea92eb3f +Subproject commit 3e37a9a721bfe83dad20923b8a759bd209663fef diff --git a/Src/common/FFT.cpp b/Src/common/FFT.cpp index 1bb56d7..9dacdcc 100644 --- a/Src/common/FFT.cpp +++ b/Src/common/FFT.cpp @@ -8,9 +8,7 @@ #include #include #include "FFT.hpp" -#include "common/logging.hpp" -Logging logger("FFT"); bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) { if (window_size > FFT_MAX_SIZE) { return false; @@ -18,15 +16,11 @@ bool FFT::init(uint16_t window_size, uint16_t num_axes, float sample_rate_hz) { n_axes = num_axes; bool buffers_allocated = AllocateBuffers(window_size); rfft_spec = init_rfft(&_hanning_window, &_fft_input_buffer, - &_fft_output_buffer, window_size); + &_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; - char buffer[100]; - snprintf(buffer, sizeof(buffer), "_resolution_hz: %d, num_axes: %d, sample_rate_hz: %d", - (int)( _resolution_hz * 1000), n_axes, (int)(_sample_rate_hz * 1000)); - logger.log_info(buffer); return buffers_allocated; } @@ -40,13 +34,14 @@ void FFT::update(float *input) { // 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); - _fft_updated = true; find_peaks(axis); // reset @@ -54,8 +49,7 @@ void FFT::update(float *input) { const int overlap_start = size / 4; memmove(&data_buffer[axis][0], &data_buffer[axis][overlap_start], sizeof(real_t) * overlap_start * 3); - buffer_index = overlap_start * 3; - _fft_updated = false; + _fft_buffer_index[axis] = overlap_start * 3; } } @@ -81,20 +75,20 @@ void FFT::find_peaks(uint8_t axis) { 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); + 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; } - + // char buffer[50]; + // snprintf(buffer, sizeof(buffer), "bin_mag_sum: %d", (int)bin_mag_sum); + // logger.log_info(buffer); 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 frhz = float(_resolution_hz); - float fbi = float(bin_index); - float freq_hz = frhz * fbi; + float freq_hz = _resolution_hz * bin_index; if ((_peak_magnitudes_all[bin_index] > largest_peak) && (freq_hz >= fft_min_freq) @@ -118,19 +112,25 @@ 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 (!std::isfinite(adjusted_bin)) { + if (adjusted_bin > size || adjusted_bin < 0) { continue; } float freq_adjusted = _resolution_hz * adjusted_bin; // snr is in dB - const float snr = 10.f * log10f((size - 1) * peak_magnitude[peak_new] / - (bin_mag_sum - peak_magnitude[peak_new])); - - if (!std::isfinite(freq_adjusted) - || (snr < MIN_SNR) + 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; @@ -144,6 +144,8 @@ void FFT::find_peaks(uint8_t axis) { 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; @@ -156,6 +158,13 @@ void FFT::find_peaks(uint8_t axis) { break; } } + float max_snr_peak = 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_frequencies[axis][peak_index]; + } + } + peak_frequencies[axis][0] = max_snr_peak; } static constexpr float tau(float x) { @@ -167,34 +176,41 @@ static constexpr float tau(float x) { } float FFT::estimate_peak_freq(float fft[], int peak_index) { - if (peak_index < 2) { - return NAN; + 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_imag[3][2]; float real[3]; float imag[3]; for (int i = -1; i < 2; i++) { - get_real_imag_by_index(fft, real_imag[i + 1], size, peak_index + i); - real[i + 1] = real_imag[i + 1][0]; - imag[i + 1] = real_imag[i + 1][1]; + 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) diff --git a/Src/common/FFT.hpp b/Src/common/FFT.hpp index 1df384f..995d9c7 100644 --- a/Src/common/FFT.hpp +++ b/Src/common/FFT.hpp @@ -19,7 +19,7 @@ extern "C" { #define FFT_MAX_SIZE 2048 #define MAX_NUM_PEAKS 3 #define MIN_SNR 1.0f -#define NUM_AXES 3 +#define MAX_NUM_AXES 3 class FFT { public: @@ -28,12 +28,13 @@ class FFT { float fft_min_freq = 0; float fft_max_freq; float _resolution_hz; - float peak_frequencies [NUM_AXES][MAX_NUM_PEAKS] {}; - float peak_snr [NUM_AXES][MAX_NUM_PEAKS] {}; + 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}; @@ -41,7 +42,6 @@ class FFT { std::vector _peak_magnitudes_all; uint16_t _fft_buffer_index[3] {}; - bool _fft_updated{false}; uint8_t n_axes; float _sample_rate_hz; @@ -56,14 +56,14 @@ class FFT { return (data_buffer[0].data() && data_buffer[1].data() && data_buffer[2].data() && _peak_magnitudes_all.data()); } - // #ifdef HAL_MODULE_ENABLED + #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 + #else // plan for the r2c transform from fftw3 library. - // fftw_plan rfft_spec; - // #endif + fftw_plan rfft_spec; + #endif }; #ifdef __cplusplus diff --git a/Src/drivers/mpu9250/mpu9250.cpp b/Src/drivers/mpu9250/mpu9250.cpp index 07a3b66..01d3e12 100644 --- a/Src/drivers/mpu9250/mpu9250.cpp +++ b/Src/drivers/mpu9250/mpu9250.cpp @@ -17,7 +17,7 @@ enum class Mpu9250Resgiter : uint8_t { // Register Map for Magnetometer (AK8963) enum class MagnetometerResgiter : uint8_t { - REG_MAG_XOUT_L = 0x03, + REG_MAG_XOUT_L = 0x03, }; constexpr auto MPU9250_WHO_AM_I_ID = std::byte(0x71); // REG_WHO_AM_I expected value diff --git a/Src/drivers/mpu9250/mpu9250.hpp b/Src/drivers/mpu9250/mpu9250.hpp index 034b560..d9ba4d3 100644 --- a/Src/drivers/mpu9250/mpu9250.hpp +++ b/Src/drivers/mpu9250/mpu9250.hpp @@ -34,6 +34,10 @@ class Mpu9250 { * @return 0 on success, negative otherwise */ int8_t read_magnetometer(std::array* mag) const; + + int8_t set_accel_odr(uint8_t odr); + + int8_t set_gyro_odr(uint8_t odr); private: bool initialized{false}; }; diff --git a/Src/modules/imu/imu.cpp b/Src/modules/imu/imu.cpp index a02d736..745b4ee 100644 --- a/Src/modules/imu/imu.cpp +++ b/Src/modules/imu/imu.cpp @@ -16,10 +16,8 @@ void ImuModule::init() { uint16_t num_axes = 3; uint16_t window_size = 512; bool fft_initialized = fft.init(window_size, num_axes, 1000.0f/period_ms); + fft.fft_min_freq = 10; initialized = imu_initialized && fft_initialized; - // snprintf(buffer, sizeof(buffer), "initialized: %d %d", imu_initialized, fft_initialized); - // logger.log_info(buffer); - // logger.log_info("initialized"); } void ImuModule::update_params() { @@ -28,14 +26,6 @@ void ImuModule::update_params() { if (enabled) { mode = initialized ? Mode::STANDBY : Mode::INITIALIZATION; } - // static uint32_t prev_time = HAL_GetTick(); - // if (prev_time + 1000 < HAL_GetTick()) { - // prev_time = HAL_GetTick(); - // char buffer[90]; - // snprintf(buffer, sizeof(buffer), "enabled: %d %d %d", enabled, initialized, - // static_cast(mode)); - // logger.log_info(buffer); - // } } void ImuModule::spin_once() { @@ -48,55 +38,51 @@ void ImuModule::spin_once() { mag.publish(); } - bool updated{false}; + bool updated[2]{false, false}; std::array accel_raw = {0, 0, 0}; - std::array gyro_raw = {0, 0, 0}; + std::array gyro_raw = {0, 0, 0}; - static std::vector gyro; if (imu.read_gyroscope(&gyro_raw) >= 0) { - gyro = {raw_gyro_to_rad_per_second(gyro_raw[0]), + float gyro[3] = { + raw_gyro_to_rad_per_second(gyro_raw[0]), raw_gyro_to_rad_per_second(gyro_raw[1]), raw_gyro_to_rad_per_second(gyro_raw[2])}; pub.msg.rate_gyro_latest[0] = gyro[0]; pub.msg.rate_gyro_latest[1] = gyro[1]; pub.msg.rate_gyro_latest[2] = gyro[2]; - // fft.update(gyro.data()); - updated = true; + fft.update(gyro); + updated[0] = true; } - auto status = imu.read_accelerometer(&accel_raw); - if (status >= 0) { - static float accel[3] = { + if (imu.read_accelerometer(&accel_raw) >= 0) { + float accel[3] = { raw_accel_to_meter_per_square_second(accel_raw[0]), raw_accel_to_meter_per_square_second(accel_raw[1]), raw_accel_to_meter_per_square_second(accel_raw[2])}; pub.msg.accelerometer_latest[0] = accel[0]; pub.msg.accelerometer_latest[1] = accel[1]; pub.msg.accelerometer_latest[2] = accel[2]; - // fft.update(accel); - updated = true; - } else { - updated = false; + updated[1] = true; } - if (updated) { + if (updated[0] && updated[1]) { pub.msg.timestamp = HAL_GetTick() * 1000; pub.publish(); } + static uint32_t prev_time = HAL_GetTick(); - if (prev_time + 1000 < HAL_GetTick()) { + if (prev_time + 1000 < HAL_GetTick() || fft._fft_updated[0]) { prev_time = HAL_GetTick(); - char buffer[90]; - snprintf(buffer, sizeof(buffer), "mode: %d", - static_cast(mode)); - // snprintf(buffer, sizeof(buffer), "peak freq: %d %d %d snr: %d %d %d", - // (int)(fft.peak_frequencies[0][0]), - // (int)(fft.peak_frequencies[1][0]), - // (int)(fft.peak_frequencies[2][0]), - // (int)(fft.peak_snr[0][0]), - // (int)(fft.peak_snr[1][0]), - // (int)(fft.peak_snr[2][0])); + char buffer[60]; + snprintf(buffer, sizeof(buffer), "peaks\nfreq: %d %d %d\n snrs: %d %d %d", + (int)(fft.peak_frequencies[0][0]), + (int)(fft.peak_frequencies[1][0]), + (int)(fft.peak_frequencies[2][0]), + (int)(fft.peak_snr[0][0]), + (int)(fft.peak_snr[1][0]), + (int)(fft.peak_snr[2][0]) + ); logger.log_info(buffer); } } diff --git a/Src/modules/imu/imu.hpp b/Src/modules/imu/imu.hpp index ec8807c..14d28a6 100644 --- a/Src/modules/imu/imu.hpp +++ b/Src/modules/imu/imu.hpp @@ -20,7 +20,7 @@ extern "C" { class ImuModule : public Module { public: - ImuModule() : Module(400.0, Protocol::DRONECAN) {} + ImuModule() : Module(00.0, Protocol::DRONECAN) {} void init() override; protected: diff --git a/Src/platform/stm32g0b1/rfft.hpp b/Src/platform/stm32g0b1/rfft.hpp index b07c92f..83479c4 100644 --- a/Src/platform/stm32g0b1/rfft.hpp +++ b/Src/platform/stm32g0b1/rfft.hpp @@ -17,9 +17,9 @@ The function specifies arm_rfft_instance_q15 from CMSIS-DSP library based on the @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) { + real_t** out, uint16_t *N) { arm_rfft_instance_q15 _rfft_q15; - switch (N) { + switch (*N) { case 256: _rfft_q15.fftLenReal = 256; _rfft_q15.twidCoefRModifier = 32U; @@ -38,7 +38,7 @@ inline arm_rfft_instance_q15 init_rfft(real_t** hanning_window, real_t** in, _rfft_q15.pCfft = &arm_cfft_sR_q15_len512; break; default: - N = 256; + *N = 256; _rfft_q15.fftLenReal = 256; _rfft_q15.twidCoefRModifier = 32U; _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; @@ -52,11 +52,11 @@ 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)); - for (int n = 0; n < N; n++) { - const float hanning_value = 0.5f * (1.f - cos(M_2PI * n / (N - 1))); + *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); } @@ -102,4 +102,14 @@ inline void get_real_imag_by_index(T* in, T* out, uint16_t N, int 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 index f01c1fc..f7ec1b6 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); - for (int n = 0; n < N; n++) { - const float hanning_value = 0.5f * (1.f - cos(M_2PI * n / (N - 1))); +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)*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); } /* @@ -42,11 +42,20 @@ inline void apply_hanning_window(real_t* in, real_t* out, real_t* hanning_window template inline void get_real_imag_by_index(T* in, T out[2], uint16_t N, int index) { out[0] = in[index]; - // For FFTW_R2HC kind the imaginary part is zero - // due to symmetries of the real-input DFT, and is not stored 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. diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index 01fb663..b1c7aa2 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -4,11 +4,6 @@ project(tests CXX C) set(TESTS_DIR ${CMAKE_CURRENT_LIST_DIR}) cmake_path(GET TESTS_DIR PARENT_PATH ROOT_DIR) -message(STATUS "ROOT_DIR: ${ROOT_DIR}") -message(STATUS "CMAKE_CURRENT_LIST_DIR: ${CMAKE_CURRENT_LIST_DIR}") -message(STATUS "CMAKE_CURRENT_SOURCE_DIR: ${CMAKE_CURRENT_SOURCE_DIR}") -message(STATUS "CRNT_DIR: ${CRNT_DIR}") - # Check if the COVERAGE flag is set option(COVERAGE "Enable coverage flags" OFF) option(TEST_NUM "Enable test num" OFF) diff --git a/Tests/Makefile b/Tests/Makefile index 6c89dcc..2cb49a4 100644 --- a/Tests/Makefile +++ b/Tests/Makefile @@ -15,7 +15,6 @@ $(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 -# cmake /home/user/Documents/work/mini_v2_node/Tests -DCMAKE_C_COMPILER=cc -DCMAKE_CXX_COMPILER=cc -DCMAKE_C_FLAGS="-g -O0 -DDEBUG" -DCMAKE_CXX_FLAGS="-g -O0 -DDEBUG" -DCMAKE_BUILD_TYPE=Debug all: test_common @@ -27,7 +26,7 @@ create_build_dir: coverage: create_build_dir cd ${BUILD_DIR} && cmake -DCOVERAGE=1 ${CRNT_DIR} && make cd ${BUILD_DIR} && ./algorithms - cd ${BUILD_DIR} && ./ft + cd ${BUILD_DIR} && ./fft echo "\n1. Algorithms:" cd ${BUILD_DIR} && gcov ${BUILD_DIR}/CMakeFiles/algorithms.dir${REPO_DIR}/common/algorithms.cpp.gcda