fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

gyro_fft: peak detection exclude side FFT buckets

Browse Source 
- when estimating the peak frequency the magnitude of side buckets will
be factored in, so it doesn't make sense to potentially treat them as
separatey detected peaks
This commit is contained in:
Daniel Agar 2022-01-19 09:04:22 -05:00
parent 9ab99a7689
commit afeab9587e
4 changed files with 72 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
posix-configs/SITL/init/test/test_imu_filtering
View File

@ -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

2
src/modules/gyro_fft/CMakeLists.txt
View File

@ -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

93
src/modules/gyro_fft/GyroFFT.cpp
View File

@ -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 &timestamp_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 &timestamp_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 (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;
if (PX4_ISFINITE(adjusted_bin)) {
const float freq_adjusted = resolution_hz * adjusted_bin;
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())) {
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(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 &timestamp_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;

8
src/modules/gyro_fft/GyroFFT.hpp
View File

@ -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<float, 5> _median_filter[3][MAX_NUM_PEAKS] {};
math::MedianFilter<float, 7> _median_filter[3][MAX_NUM_PEAKS] {};
sensor_gyro_fft_s _sensor_gyro_fft{};