fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
- 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)
This commit is contained in:
Daniel Agar 2021-07-06 21:54:18 -04:00 committed by GitHub
parent f296a41737
commit 1b5e65df04
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
msg/sensor_gyro_fft.msg
View File

@ -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

2
posix-configs/SITL/init/test/test_imu_filtering
View File

@ -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

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

@ -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];

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

@ -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{};

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

@ -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);