gyro_fft: change default length 1024 -> 512 to decrease latency

- change default FFT length (1024 -> 512)
      - this doubles the update rate because half the number of samples are required for each
 - decrease number of peaks (4 -> 3)
      - so far 3 seems to be sufficient on most vehicles
 - increase median filter window (3 -> 5)
 - decrease SNR requirement (likely needs to be configurable)

msg/sensor_gyro_fft.msg

@@ -6,10 +6,10 @@ uint32 device_id          # unique device ID for the sensor that does not change
 float32 sensor_sample_rate_hz
 float32 resolution_hz
 
-float32[4] peak_frequencies_x # x axis peak frequencies
-float32[4] peak_frequencies_y # y axis peak frequencies
-float32[4] peak_frequencies_z # z axis peak frequencies
+float32[3] peak_frequencies_x # x axis peak frequencies
+float32[3] peak_frequencies_y # y axis peak frequencies
+float32[3] peak_frequencies_z # z axis peak frequencies
 
-float32[4] peak_snr_x # x axis peak SNR
-float32[4] peak_snr_y # y axis peak SNR
-float32[4] peak_snr_z # z axis peak SNR
+float32[3] peak_snr_x # x axis peak SNR
+float32[3] peak_snr_y # y axis peak SNR
+float32[3] peak_snr_z # z axis peak SNR

posix-configs/SITL/init/test/test_imu_filtering

@@ -18,7 +18,7 @@ param set IMU_GYRO_RATEMAX 1000
 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 1024
+param set IMU_GYRO_FFT_LEN 512
 
 # dynamic notches ESC/FFT/both
 #param set IMU_GYRO_DYN_NF 1

src/modules/gyro_fft/GyroFFT.cpp

@@ -91,12 +91,12 @@ bool GyroFFT::init()
 		_rfft_q15.pCfft = &arm_cfft_sR_q15_len128;
 		break;
 
-	// case 512:
-	// 	buffers_allocated = AllocateBuffers<512>();
-	// 	_rfft_q15.fftLenReal = 512;
-	// 	_rfft_q15.twidCoefRModifier = 16U;
-	// 	_rfft_q15.pCfft = &arm_cfft_sR_q15_len256;
-	// 	break;
+	case 512:
+		buffers_allocated = AllocateBuffers<512>();
+		_rfft_q15.fftLenReal = 512;
+		_rfft_q15.twidCoefRModifier = 16U;
+		_rfft_q15.pCfft = &arm_cfft_sR_q15_len256;
+		break;
 
 	case 1024:
 		buffers_allocated = AllocateBuffers<1024>();
@@ -381,7 +381,7 @@ void GyroFFT::Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint
 	const float resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len;
 	q15_t *gyro_data_buffer[] {_gyro_data_buffer_x, _gyro_data_buffer_y, _gyro_data_buffer_z};
 
-	static constexpr float MIN_SNR_PUBLISH = 15.f; // TODO: configurable?
+	static constexpr float MIN_SNR_PUBLISH = 10.f; // TODO: configurable?
 
 	for (int axis = 0; axis < 3; axis++) {
 		int &buffer_index = _fft_buffer_index[axis];

src/modules/gyro_fft/GyroFFT.hpp

@@ -143,7 +143,7 @@ private:
 	unsigned _gyro_last_generation{0};
 
 	float _peak_frequencies_prev[3][MAX_NUM_PEAKS] {};
-	math::MedianFilter<float, 3> _median_filter[3][MAX_NUM_PEAKS] {};
+	math::MedianFilter<float, 5> _median_filter[3][MAX_NUM_PEAKS] {};
 
 	sensor_gyro_fft_s _sensor_gyro_fft{};
 

src/modules/gyro_fft/parameters.c

@@ -66,10 +66,11 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 192.f);
 * IMU gyro FFT length.
 *
 * @value 256 256
+* @value 512 512
 * @value 1024 1024
 * @value 4096 4096
 * @unit Hz
 * @reboot_required true
 * @group Sensors
 */
-PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 1024);
+PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 512);