fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 03:11:35 +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/gyro_fft/GyroFFT.hpp
+17 -8
View File
@@ -31,7 +31,8 @@
*
****************************************************************************/
#pragma once
#ifndef GYRO_FFT_HPP
#define GYRO_FFT_HPP
#include <lib/mathlib/math/filter/MedianFilter.hpp>
#include <lib/matrix/matrix/math.hpp>
@@ -78,10 +79,19 @@ public:
bool init();
private:
float EstimatePeakFrequencyBin(q15_t fft[], int peak_index);
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
void Run() override;
inline void FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *fft_outupt_buffer);
inline float EstimatePeakFrequencyBin(q15_t fft[], int peak_index);
inline void Publish();
bool SensorSelectionUpdate(bool force = false);
void Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint8_t N);
inline void UpdateOutput(const hrt_abstime &timestamp_sample, int axis, float peak_frequencies[MAX_NUM_PEAKS],
float peak_snr[MAX_NUM_PEAKS], int num_peaks_found);
void VehicleIMUStatusUpdate(bool force = false);
template<size_t N>
@@ -100,11 +110,6 @@ private:
&& _fft_outupt_buffer);
}
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
uORB::Publication<sensor_gyro_fft_s> _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -142,7 +147,6 @@ private:
unsigned _gyro_last_generation{0};
float _peak_frequencies_prev[3][MAX_NUM_PEAKS] {};
math::MedianFilter<float, 5> _median_filter[3][MAX_NUM_PEAKS] {};
sensor_gyro_fft_s _sensor_gyro_fft{};
@@ -151,9 +155,14 @@ private:
int32_t _imu_gyro_fft_len{256};
bool _fft_updated{false};
bool _publish{false};
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
)
};
#endif // !GYRO_FFT_HPP