fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 15:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

gyro_fft: track FFT peaks and median filter frequency

Browse Source
This commit is contained in:
Daniel Agar
2021-07-06 12:32:25 -04:00
parent 72389c2306
commit a5ee28883a
4 changed files with 147 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gyro_fft/GyroFFT.hpp
+7 -1
View File
@@ -33,6 +33,7 @@
#pragma once
#include <lib/mathlib/math/filter/MedianFilter.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
@@ -77,7 +78,7 @@ public:
bool init();
private:
float EstimatePeakFrequency(q15_t fft[], int peak_index);
float EstimatePeakFrequencyBin(q15_t fft[], int peak_index);
void Run() override;
bool SensorSelectionUpdate(bool force = false);
void Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint8_t N);
@@ -141,8 +142,13 @@ private:
unsigned _gyro_last_generation{0};
float _peak_frequencies_prev[3][MAX_NUM_PEAKS] {};
math::MedianFilter<float, 3> _median_filter[3][MAX_NUM_PEAKS] {};
sensor_gyro_fft_s _sensor_gyro_fft{};
hrt_abstime _last_update[3][MAX_NUM_PEAKS] {};
int32_t _imu_gyro_fft_len{256};
DEFINE_PARAMETERS(