fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 11:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

gyro_fft: require higher SNR for initially tracking a peak

Browse Source 
- initial frequency peak tracking SNR increased from 10->15 db
 - after initial detection the threshold decreases to SNR 5db
 - gyro_fft large method refactored into smaller pieces
 - sensors/vehicle_angular_velocity: dynamic notch FFT make sample rate
check a percentage and relax lower bound safety threshold
This commit is contained in:
Daniel Agar
2021-10-04 10:17:16 -04:00
parent 6d78054f50
commit cc4152e10d
5 changed files with 248 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+2 -2
View File
@@ -523,10 +523,10 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft)
&& (sensor_gyro_fft.device_id == _selected_sensor_device_id)
&& (hrt_elapsed_time(&sensor_gyro_fft.timestamp) < DYNAMIC_NOTCH_FITLER_TIMEOUT)
&& (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 10.f)) {
&& ((fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) / _filter_sample_rate_hz) < 0.02f)) {
// ignore any peaks below half the gyro cutoff frequency
const float peak_freq_min = _param_imu_gyro_cutoff.get() / 2.f;
const float peak_freq_min = 10.f; // lower bound TODO: configurable?
const float peak_freq_max = _filter_sample_rate_hz / 3.f; // upper bound safety (well below Nyquist)
const float bandwidth = math::constrain(sensor_gyro_fft.resolution_hz, 8.f, 30.f); // TODO: base on numerical limits?