dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gyro_fft: add IMU_GYRO_FFT_SNR parameter

This commit is contained in:
Daniel Agar 2021-11-01 19:10:47 -04:00
parent 6fab6242db
commit dd3e0b723a
3 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -465,7 +465,6 @@ void GyroFFT::FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *ff
// keep if peak has been previously seen and SNR > MIN_SNR
// or
// peak has SNR > MIN_SNR_INITIAL
static constexpr float MIN_SNR_INITIAL = 15.f; // TODO: configurable?
static constexpr float MIN_SNR = 1.f; // TODO: configurable?
int num_peaks_found = 0;
@ -491,7 +490,7 @@ void GyroFFT::FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *ff
// 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 > MIN_SNR_INITIAL)
if ((snr > _param_imu_gyro_fft_snr.get())
|| (fabsf(freq_adjusted - peak_frequencies_publish[axis][peak_prev]) < (resolution_hz * 0.5f))) {
// keep
peak_frequencies[num_peaks_found] = freq_adjusted;

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

@ -161,7 +161,8 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_FFT_LEN>) _param_imu_gyro_fft_len,
(ParamFloat<px4::params::IMU_GYRO_FFT_MIN>) _param_imu_gyro_fft_min,
(ParamFloat<px4::params::IMU_GYRO_FFT_MAX>) _param_imu_gyro_fft_max
(ParamFloat<px4::params::IMU_GYRO_FFT_MAX>) _param_imu_gyro_fft_max,
(ParamFloat<px4::params::IMU_GYRO_FFT_SNR>) _param_imu_gyro_fft_snr
)
};

9
src/modules/gyro_fft/parameters.c
View File

@ -74,3 +74,12 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 150.f);
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 512);
/**
* IMU gyro FFT SNR.
*
* @min 1
* @max 30
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_SNR, 10.f);