From dd3e0b723a5f451cb65ddf135fd350bd760f721d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 1 Nov 2021 19:10:47 -0400
Subject: [PATCH] gyro_fft: add IMU_GYRO_FFT_SNR parameter

---
 src/modules/gyro_fft/GyroFFT.cpp  | 3 +--
 src/modules/gyro_fft/GyroFFT.hpp  | 3 ++-
 src/modules/gyro_fft/parameters.c | 9 +++++++++
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp
index bc101024a8..248ebbcf70 100644
--- a/src/modules/gyro_fft/GyroFFT.cpp
+++ b/src/modules/gyro_fft/GyroFFT.cpp
@@ -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;
diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp
index 25274e1bcd..1a4511b9e0 100644
--- a/src/modules/gyro_fft/GyroFFT.hpp
+++ b/src/modules/gyro_fft/GyroFFT.hpp
@@ -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
 	)
 };
 
diff --git a/src/modules/gyro_fft/parameters.c b/src/modules/gyro_fft/parameters.c
index a1320d9b73..c0d7e3eebc 100644
--- a/src/modules/gyro_fft/parameters.c
+++ b/src/modules/gyro_fft/parameters.c
@@ -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);