diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering index 14a999a8fe..b122fd389c 100644 --- a/posix-configs/SITL/init/test/test_imu_filtering +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -18,6 +18,7 @@ param set IMU_GYRO_FFT_EN 1 param set IMU_GYRO_FFT_MIN 10 param set IMU_GYRO_FFT_MAX 1000 param set IMU_GYRO_FFT_LEN 512 +param set IMU_GYRO_FFT_SNR 10 # dynamic notches ESC/FFT/both #param set IMU_GYRO_DNF_EN 1 diff --git a/src/modules/gyro_fft/CMakeLists.txt b/src/modules/gyro_fft/CMakeLists.txt index 85c6bbf461..9bf6832ed4 100644 --- a/src/modules/gyro_fft/CMakeLists.txt +++ b/src/modules/gyro_fft/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 1724505351..fd01b69167 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 - 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -66,6 +66,7 @@ GyroFFT::~GyroFFT() delete[] _hanning_window; delete[] _fft_input_buffer; delete[] _fft_outupt_buffer; + delete[] _peak_magnitudes_all; } bool GyroFFT::init() @@ -435,33 +436,52 @@ void GyroFFT::FindPeaks(const hrt_abstime ×tamp_sample, int axis, q15_t *ff // sum total energy across all used buckets for SNR float bin_mag_sum = 0; + // FFT output buffer is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1] + for (uint16_t fft_index = 2; fft_index < _imu_gyro_fft_len; fft_index += 2) { + + const float real = fft_outupt_buffer[fft_index]; + const float imag = fft_outupt_buffer[fft_index + 1]; + + const float fft_magnitude = sqrtf(real * real + imag * imag); + + int bin_index = fft_index / 2; + + _peak_magnitudes_all[bin_index] = fft_magnitude; + bin_mag_sum += fft_magnitude; + } + + // find raw peaks uint16_t raw_peak_index[MAX_NUM_PEAKS] {}; float peak_magnitude[MAX_NUM_PEAKS] {}; - // FFT output buffer is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1] - for (uint16_t bucket_index = 0; bucket_index < (2 * _imu_gyro_fft_len - 1); bucket_index = bucket_index + 2) { + for (int i = 0; i < MAX_NUM_PEAKS; i++) { - const float freq_hz = (bucket_index / 2) * resolution_hz; + float largest_peak = 0; + int largest_peak_index = 0; - if ((bucket_index > 0) && (bucket_index < (_imu_gyro_fft_len - 1)) - && (freq_hz >= _param_imu_gyro_fft_min.get()) - && (freq_hz <= _param_imu_gyro_fft_max.get())) { + for (int bin_index = 1; bin_index < _imu_gyro_fft_len; bin_index++) { - const float real = fft_outupt_buffer[bucket_index]; - const float imag = fft_outupt_buffer[bucket_index + 1]; + const float freq_hz = bin_index * resolution_hz; - const float fft_magnitude_squared = real * real + imag * imag; - bin_mag_sum += fft_magnitude_squared; + if ((_peak_magnitudes_all[bin_index] > largest_peak) + && (freq_hz >= _param_imu_gyro_fft_min.get()) + && (freq_hz <= _param_imu_gyro_fft_max.get())) { - for (int i = 0; i < MAX_NUM_PEAKS; i++) { - if (fft_magnitude_squared > peak_magnitude[i]) { - peak_magnitude[i] = fft_magnitude_squared; - raw_peak_index[i] = bucket_index; - break; - } + largest_peak = _peak_magnitudes_all[bin_index]; + largest_peak_index = bin_index; } } + + if (largest_peak_index > 1) { + 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; + } } // keep if peak has been previously seen and SNR > MIN_SNR @@ -475,28 +495,43 @@ void GyroFFT::FindPeaks(const hrt_abstime ×tamp_sample, int axis, q15_t *ff float *peak_frequencies_publish[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z }; + float peak_frequencies_prev[MAX_NUM_PEAKS]; + + for (int i = 0; i < MAX_NUM_PEAKS; i++) { + peak_frequencies_prev[i] = peak_frequencies_publish[axis][i]; + } + for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) { if (raw_peak_index[peak_new] > 0) { - const float snr = 10.f * log10f((_imu_gyro_fft_len - 1) * peak_magnitude[peak_new] / - (bin_mag_sum - peak_magnitude[peak_new])); + const float adjusted_bin = 0.5f * EstimatePeakFrequencyBin(fft_outupt_buffer, 2 * raw_peak_index[peak_new]); + + if (PX4_ISFINITE(adjusted_bin)) { + const float freq_adjusted = resolution_hz * adjusted_bin; - if (snr > MIN_SNR) { - // estimate adjusted frequency bin, magnitude, and SNR for the largest peaks found - const float adjusted_bin = EstimatePeakFrequencyBin(fft_outupt_buffer, raw_peak_index[peak_new]); - const float freq_adjusted = (adjusted_bin / 2.f) * resolution_hz; + const float snr = 10.f * log10f((_imu_gyro_fft_len - 1) * peak_magnitude[peak_new] / + (bin_mag_sum - peak_magnitude[peak_new])); - if (PX4_ISFINITE(adjusted_bin) && PX4_ISFINITE(freq_adjusted) - && (freq_adjusted > _param_imu_gyro_fft_min.get()) - && (freq_adjusted < _param_imu_gyro_fft_max.get())) { + if (PX4_ISFINITE(freq_adjusted) + && (snr > MIN_SNR) + && (freq_adjusted >= _param_imu_gyro_fft_min.get()) + && (freq_adjusted <= _param_imu_gyro_fft_max.get())) { // 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++) { - if ((snr > _param_imu_gyro_fft_snr.get()) - || (fabsf(freq_adjusted - peak_frequencies_publish[axis][peak_prev]) < (resolution_hz * 0.5f))) { + bool snr_acceptable = (snr > _param_imu_gyro_fft_snr.get()); + bool peak_close = (fabsf(freq_adjusted - peak_frequencies_prev[peak_prev]) < (resolution_hz * 0.25f)); + + if (snr_acceptable || peak_close) { // keep peak_frequencies[num_peaks_found] = freq_adjusted; peak_snr[num_peaks_found] = snr; + + // remove + if (peak_close) { + peak_frequencies_prev[peak_prev] = NAN; + } + num_peaks_found++; break; } @@ -592,7 +627,7 @@ void GyroFFT::UpdateOutput(const hrt_abstime ×tamp_sample, int axis, float // clear any stale entries for (int peak_out = 0; peak_out < MAX_NUM_PEAKS; peak_out++) { - if (timestamp_sample - _last_update[axis][peak_out] > 500_ms) { + if (timestamp_sample - _last_update[axis][peak_out] > 100_ms) { peak_frequencies_publish[axis][peak_out] = NAN; peak_snr_publish[axis][peak_out] = NAN; diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp index 1a4511b9e0..1edfe2ff5e 100644 --- a/src/modules/gyro_fft/GyroFFT.hpp +++ b/src/modules/gyro_fft/GyroFFT.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -104,6 +104,8 @@ private: _fft_input_buffer = new q15_t[N]; _fft_outupt_buffer = new q15_t[N * 2]; + _peak_magnitudes_all = new float[N]; + return (_gyro_data_buffer_x && _gyro_data_buffer_y && _gyro_data_buffer_z && _hanning_window && _fft_input_buffer @@ -139,6 +141,8 @@ private: q15_t *_fft_input_buffer{nullptr}; q15_t *_fft_outupt_buffer{nullptr}; + float *_peak_magnitudes_all{nullptr}; + float _gyro_sample_rate_hz{8000}; // 8 kHz default float _fifo_last_scale{0}; @@ -147,7 +151,7 @@ private: unsigned _gyro_last_generation{0}; - math::MedianFilter _median_filter[3][MAX_NUM_PEAKS] {}; + math::MedianFilter _median_filter[3][MAX_NUM_PEAKS] {}; sensor_gyro_fft_s _sensor_gyro_fft{};