Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

gyro_fft: limit noise floor to configured range (IMU_GYRO_FFT_MIN/MAX)

Browse Source
This commit is contained in:
Daniel Agar
2021-11-02 17:45:45 -04:00
parent dd3e0b723a
commit 47afab62e7
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gyro_fft/GyroFFT.cpp
+6 -6
View File
@@ -439,12 +439,6 @@ void GyroFFT::FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *ff
// 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] // 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 (uint16_t bucket_index = 0; bucket_index < (2 * _imu_gyro_fft_len - 1); bucket_index = bucket_index + 2) {
const float real = fft_outupt_buffer[bucket_index];
const float imag = fft_outupt_buffer[bucket_index + 1];
const float fft_magnitude_squared = real * real + imag * imag;
bin_mag_sum += fft_magnitude_squared;
const float freq_hz = (bucket_index / 2) * resolution_hz; const float freq_hz = (bucket_index / 2) * resolution_hz;
@@ -452,6 +446,12 @@ void GyroFFT::FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *ff
&& (freq_hz >= _param_imu_gyro_fft_min.get()) && (freq_hz >= _param_imu_gyro_fft_min.get())
&& (freq_hz <= _param_imu_gyro_fft_max.get())) { && (freq_hz <= _param_imu_gyro_fft_max.get())) {
const float real = fft_outupt_buffer[bucket_index];
const float imag = fft_outupt_buffer[bucket_index + 1];
const float fft_magnitude_squared = real * real + imag * imag;
bin_mag_sum += fft_magnitude_squared;
for (int i = 0; i < MAX_NUM_PEAKS; i++) { for (int i = 0; i < MAX_NUM_PEAKS; i++) {
if (fft_magnitude_squared > peak_magnitude[i]) { if (fft_magnitude_squared > peak_magnitude[i]) {
peak_magnitude[i] = fft_magnitude_squared; peak_magnitude[i] = fft_magnitude_squared;