Skip to content

Commit

Permalink
split fft find_peaks into parts, add magnitude into RawIMU mes
Browse files Browse the repository at this point in the history
  • Loading branch information
@AsiiaPine
AsiiaPine committed Oct 8, 2024
1 parent 264bccd commit 01b56f5
Show file tree
Hide file tree
Showing 7 changed files with 271 additions and 241 deletions.
9 changes: 0 additions & 9 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
# Copyright (C) 2023-2024 Dmitry Ponomarev <[email protected]>
# 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})
Expand Down Expand Up @@ -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)
Expand Down
146 changes: 81 additions & 65 deletions Src/common/FFT.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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];

Expand All @@ -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
Expand All @@ -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;
Expand All @@ -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) {
Expand All @@ -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;
Expand Down
36 changes: 28 additions & 8 deletions Src/common/FFT.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::array<float, MAX_NUM_PEAKS>, MAX_NUM_AXES> peak_frequencies {0};
std::array<std::array<float, MAX_NUM_PEAKS>, MAX_NUM_AXES> peak_magnitudes {0};
std::array<std::array<float, MAX_NUM_PEAKS>, 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<bool, MAX_NUM_AXES> _fft_updated{false};
std::array<real_t, FFT_MAX_SIZE> _hanning_window;
std::array<real_t, FFT_MAX_SIZE * 2> _fft_output_buffer;
std::array<real_t, FFT_MAX_SIZE> _fft_input_buffer;
std::array<float, NUMBER_OF_SAMPLES> _peak_magnitudes_all;
std::array<uint16_t, MAX_NUM_AXES> _fft_buffer_index;
std::array<std::array<real_t, NUMBER_OF_SAMPLES>, MAX_NUM_AXES> data_buffer;
std::array<float, NUMBER_OF_SAMPLES> _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
Expand Down
12 changes: 6 additions & 6 deletions Src/modules/imu/imu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,17 +101,17 @@ 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() {
if (!(bitmask & static_cast<std::underlying_type_t<Bitmask>>(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];
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;
}
Loading

0 comments on commit 01b56f5

Please sign in to comment.