From 1b5e65df04800887c76ec1106cf762b8cabdb3b9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Jul 2021 21:54:18 -0400 Subject: [PATCH] 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 | 12 ++++++------ posix-configs/SITL/init/test/test_imu_filtering | 2 +- src/modules/gyro_fft/GyroFFT.cpp | 14 +++++++------- src/modules/gyro_fft/GyroFFT.hpp | 2 +- src/modules/gyro_fft/parameters.c | 3 ++- 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/msg/sensor_gyro_fft.msg b/msg/sensor_gyro_fft.msg index eb310e1044..ed84d0a0d0 100644 --- a/msg/sensor_gyro_fft.msg +++ b/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 diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering index f091d4d1e8..9590e39124 100644 --- a/posix-configs/SITL/init/test/test_imu_filtering +++ b/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 diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index f9de5f09a0..44f2e0a208 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/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 ×tamp_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]; diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp index 1e69685ab2..fd1b0fe289 100644 --- a/src/modules/gyro_fft/GyroFFT.hpp +++ b/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 _median_filter[3][MAX_NUM_PEAKS] {}; + math::MedianFilter _median_filter[3][MAX_NUM_PEAKS] {}; sensor_gyro_fft_s _sensor_gyro_fft{}; diff --git a/src/modules/gyro_fft/parameters.c b/src/modules/gyro_fft/parameters.c index 9062b9d81b..6b6072f690 100644 --- a/src/modules/gyro_fft/parameters.c +++ b/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);