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: 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-09-19 10:16:48 -04:00
parent 6d78054f50
commit cc4152e10d
5 changed files with 248 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,7 @@ logger off
sensors status
listener sensor_gyro
listener sensor_gyro_fifo
listener sensor_gyro_fft
perf
echo "ALL TESTS PASSED"

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020 - 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -253,7 +253,7 @@ void GyroFFT::VehicleIMUStatusUpdate(bool force)
}
// helper function used for frequency estimation
static float tau(float x)
static inline float tau(float x)
{
// tau(x) = 1/4 * log(3x^2 + 6x + 1) – sqrt(6)/24 * log((x + 1 – sqrt(2/3)) / (x + 1 + sqrt(2/3)))
float p1 = logf(3.f * powf(x, 2.f) + 6.f * x + 1.f);
@ -323,6 +323,9 @@ void GyroFFT::Run()
const bool selection_updated = SensorSelectionUpdate();
VehicleIMUStatusUpdate(selection_updated);
// reset
_fft_updated = false;
if (_gyro_fifo) {
// run on sensor gyro fifo updates
sensor_gyro_fifo_s sensor_gyro_fifo;
@ -378,21 +381,18 @@ void GyroFFT::Run()
}
}
if (_publish) {
Publish();
_publish = false;
}
perf_end(_cycle_perf);
}
void GyroFFT::Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint8_t N)
{
float *peak_frequencies_publish[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z };
float *peak_snr_publish[] { _sensor_gyro_fft.peak_snr_x, _sensor_gyro_fft.peak_snr_y, _sensor_gyro_fft.peak_snr_z };
bool publish = false;
bool fft_updated = false;
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 = 10.f; // TODO: configurable?
for (int axis = 0; axis < 3; axis++) {
int &buffer_index = _fft_buffer_index[axis];
@ -404,186 +404,15 @@ void GyroFFT::Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint
}
// if we have enough samples begin processing, but only one FFT per cycle
if ((buffer_index >= _imu_gyro_fft_len) && !fft_updated) {
if ((buffer_index >= _imu_gyro_fft_len) && !_fft_updated) {
perf_begin(_fft_perf);
arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len);
arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);
fft_updated = true;
_fft_updated = true;
// sum total energy across all used buckets for SNR
float bin_mag_sum = 0;
for (uint16_t bucket_index = 2; bucket_index < (_imu_gyro_fft_len - 1); bucket_index = bucket_index + 2) {
const float real = _fft_outupt_buffer[bucket_index];
const float imag = _fft_outupt_buffer[bucket_index + 1];
const float fft_magnitude_squared = real * real + imag * imag;
bin_mag_sum += fft_magnitude_squared;
}
// find raw peaks
int raw_peak_index[MAX_NUM_PEAKS] {};
float raw_peak_magnitude[MAX_NUM_PEAKS] {};
float raw_peak_snr[MAX_NUM_PEAKS] {};
// start at 2 to skip DC
// output is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1]
for (uint16_t bucket_index = 2; bucket_index < (_imu_gyro_fft_len - 1); bucket_index = bucket_index + 2) {
const float freq_hz = (bucket_index / 2) * resolution_hz;
if ((freq_hz >= _param_imu_gyro_fft_max.get()) || (freq_hz >= (_gyro_sample_rate_hz / 2.f))) {
break;
}
const float real = _fft_outupt_buffer[bucket_index];
const float imag = _fft_outupt_buffer[bucket_index + 1];
const float fft_magnitude_squared = real * real + imag * imag;
float snr = 10.f * log10f((_imu_gyro_fft_len - 1) * fft_magnitude_squared / (bin_mag_sum - fft_magnitude_squared));
if (snr > (MIN_SNR_PUBLISH / 2.f)) {
for (int i = 0; i < MAX_NUM_PEAKS; i++) {
if (fft_magnitude_squared > raw_peak_magnitude[i]) {
raw_peak_magnitude[i] = fft_magnitude_squared;
raw_peak_snr[i] = snr;
raw_peak_index[i] = bucket_index;
break;
}
}
}
}
int num_peaks_found = 0;
float peak_frequencies[MAX_NUM_PEAKS] {};
float peak_snr[MAX_NUM_PEAKS] {};
// estimate adjusted frequency bin, magnitude, and SNR for the largest peaks found
for (int i = 0; i < MAX_NUM_PEAKS; i++) {
if (raw_peak_index[i] > 0) {
const float adjusted_bin = EstimatePeakFrequencyBin(_fft_outupt_buffer, raw_peak_index[i]);
const float freq_adjusted = (adjusted_bin / 2.f) * resolution_hz;
if (PX4_ISFINITE(adjusted_bin) && PX4_ISFINITE(freq_adjusted)
&& (fabsf(adjusted_bin - raw_peak_index[i]) < 2.f)
&& (freq_adjusted > _param_imu_gyro_fft_min.get())
&& (freq_adjusted < _param_imu_gyro_fft_max.get())) {
peak_frequencies[num_peaks_found] = freq_adjusted;
peak_snr[num_peaks_found] = raw_peak_snr[i];
num_peaks_found++;
}
}
}
if (num_peaks_found > 0) {
float peak_frequencies_diff[MAX_NUM_PEAKS][MAX_NUM_PEAKS];
for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) {
// compute distance to previous peaks
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if ((peak_frequencies[peak_new] > 0)
&& PX4_ISFINITE(_peak_frequencies_prev[axis][peak_prev])
&& (_peak_frequencies_prev[axis][peak_prev] > 0)) {
peak_frequencies_diff[peak_new][peak_prev] = fabsf(peak_frequencies[peak_new] -
_peak_frequencies_prev[axis][peak_prev]);
} else {
peak_frequencies_diff[peak_new][peak_prev] = INFINITY;
}
}
}
// go through peak_frequencies_diff and find smallest diff (closest peaks)
// - copy new peak to old peak slot
// - exclude new peak (row) and old peak (column) in search
// - repeat
//
// - finally copy unmatched peaks to empty slots
bool peak_new_copied[MAX_NUM_PEAKS] {};
bool peak_out_filled[MAX_NUM_PEAKS] {};
for (int new_peak = 0; new_peak < num_peaks_found; new_peak++) {
// find new peak with smallest difference to old peak
float smallest_diff = INFINITY;
int closest_new_peak = -1;
int closest_prev_peak = -1;
for (int peak_new = 0; peak_new < num_peaks_found; peak_new++) {
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if (!peak_new_copied[peak_new] && !peak_out_filled[peak_prev]
&& (peak_frequencies_diff[peak_new][peak_prev] < smallest_diff)) {
smallest_diff = peak_frequencies_diff[peak_new][peak_prev];
closest_new_peak = peak_new;
closest_prev_peak = peak_prev;
}
}
}
// new peak: r, old peak: c
if (PX4_ISFINITE(smallest_diff) && (smallest_diff > 0)) {
// copy new peak
_peak_frequencies_prev[axis][closest_prev_peak] = _median_filter[axis][closest_prev_peak].apply(
peak_frequencies[closest_new_peak]);
if (_peak_frequencies_prev[axis][closest_prev_peak] > 0 && peak_snr[closest_new_peak] > MIN_SNR_PUBLISH) {
peak_frequencies_publish[axis][closest_prev_peak] = _peak_frequencies_prev[axis][closest_prev_peak] ;
peak_snr_publish[axis][closest_prev_peak] = peak_snr[closest_new_peak];
_last_update[axis][closest_prev_peak] = timestamp_sample;
publish = true;
}
// clear
peak_frequencies[closest_new_peak] = NAN;
peak_frequencies_diff[closest_new_peak][closest_prev_peak] = NAN;
peak_new_copied[closest_new_peak] = true;
peak_out_filled[closest_prev_peak] = true;
}
}
// clear any stale entries
for (int peak_out = 0; peak_out < MAX_NUM_PEAKS; peak_out++) {
if (timestamp_sample - _last_update[axis][peak_out] > 100_ms) {
peak_frequencies_publish[axis][peak_out] = NAN;
peak_snr_publish[axis][peak_out] = NAN;
_last_update[axis][peak_out] = 0;
}
}
// copy any remaining new (unmatched) peaks to overwrite old or empty slots
for (int peak_new = 0; peak_new < num_peaks_found; peak_new++) {
if (PX4_ISFINITE(peak_frequencies[peak_new]) && (peak_frequencies[peak_new] > 0)) {
int oldest_slot = -1;
hrt_abstime oldest = timestamp_sample;
// find oldest slot and replace with new peak frequency
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if (_last_update[axis][peak_prev] < oldest) {
oldest_slot = peak_prev;
oldest = _last_update[axis][peak_prev];
}
}
if (oldest_slot >= 0) {
// copy peak to output slot
_peak_frequencies_prev[axis][oldest_slot] = _median_filter[axis][oldest_slot].apply(peak_frequencies[peak_new]);
if (_peak_frequencies_prev[axis][oldest_slot] > 0 && peak_snr[peak_new] > MIN_SNR_PUBLISH) {
peak_frequencies_publish[axis][oldest_slot] = _peak_frequencies_prev[axis][oldest_slot];
peak_snr_publish[axis][oldest_slot] = peak_snr[peak_new];
_last_update[axis][oldest_slot] = timestamp_sample;
publish = true;
}
}
}
}
}
FindPeaks(timestamp_sample, axis, _fft_outupt_buffer);
// reset
// shift buffer (3/4 overlap)
@ -595,15 +424,221 @@ void GyroFFT::Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint
}
}
}
}
if (publish) {
_sensor_gyro_fft.device_id = _selected_sensor_device_id;
_sensor_gyro_fft.sensor_sample_rate_hz = _gyro_sample_rate_hz;
_sensor_gyro_fft.resolution_hz = resolution_hz;
_sensor_gyro_fft.timestamp_sample = timestamp_sample;
_sensor_gyro_fft.timestamp = hrt_absolute_time();
_sensor_gyro_fft_pub.publish(_sensor_gyro_fft);
void GyroFFT::FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *fft_outupt_buffer)
{
const float resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len;
// sum total energy across all used buckets for SNR
float bin_mag_sum = 0;
// find raw peaks
uint16_t raw_peak_index[MAX_NUM_PEAKS] {};
float peak_magnitude[MAX_NUM_PEAKS] {};
// FFT output buffer is ordered [real[0], imag[0], real[1], imag[1], real[2], imag[2] ... real[(N/2)-1], imag[(N/2)-1]
for (uint16_t bucket_index = 0; bucket_index < (2 * _imu_gyro_fft_len - 1); bucket_index = bucket_index + 2) {
const float real = fft_outupt_buffer[bucket_index];
const float imag = fft_outupt_buffer[bucket_index + 1];
const float fft_magnitude_squared = real * real + imag * imag;
bin_mag_sum += fft_magnitude_squared;
const float freq_hz = (bucket_index / 2) * resolution_hz;
if ((bucket_index > 0) && (bucket_index < (_imu_gyro_fft_len - 1))
&& (freq_hz >= _param_imu_gyro_fft_min.get())
&& (freq_hz <= _param_imu_gyro_fft_max.get())) {
for (int i = 0; i < MAX_NUM_PEAKS; i++) {
if (fft_magnitude_squared > peak_magnitude[i]) {
peak_magnitude[i] = fft_magnitude_squared;
raw_peak_index[i] = bucket_index;
break;
}
}
}
}
// keep if peak has been previously seen and SNR > MIN_SNR
// or
// peak has SNR > MIN_SNR_INITIAL
static constexpr float MIN_SNR_INITIAL = 15.f; // TODO: configurable?
static constexpr float MIN_SNR = 1.f; // TODO: configurable?
int num_peaks_found = 0;
float peak_frequencies[MAX_NUM_PEAKS] {};
float peak_snr[MAX_NUM_PEAKS] {};
float *peak_frequencies_publish[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z };
for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) {
if (raw_peak_index[peak_new] > 0) {
const float snr = 10.f * log10f((_imu_gyro_fft_len - 1) * peak_magnitude[peak_new] /
(bin_mag_sum - peak_magnitude[peak_new]));
if (snr > MIN_SNR) {
// estimate adjusted frequency bin, magnitude, and SNR for the largest peaks found
const float adjusted_bin = EstimatePeakFrequencyBin(fft_outupt_buffer, raw_peak_index[peak_new]);
const float freq_adjusted = (adjusted_bin / 2.f) * resolution_hz;
if (PX4_ISFINITE(adjusted_bin) && PX4_ISFINITE(freq_adjusted)
&& (freq_adjusted > _param_imu_gyro_fft_min.get())
&& (freq_adjusted < _param_imu_gyro_fft_max.get())) {
// only keep if we're already tracking this frequency or if the SNR is significant
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if ((snr > MIN_SNR_INITIAL)
|| (fabsf(freq_adjusted - peak_frequencies_publish[axis][peak_prev]) < (resolution_hz * 0.5f))) {
// keep
peak_frequencies[num_peaks_found] = freq_adjusted;
peak_snr[num_peaks_found] = snr;
num_peaks_found++;
break;
}
}
}
}
}
}
if (num_peaks_found > 0) {
UpdateOutput(timestamp_sample, axis, peak_frequencies, peak_snr, num_peaks_found);
}
}
void GyroFFT::UpdateOutput(const hrt_abstime &timestamp_sample, int axis, float peak_frequencies[MAX_NUM_PEAKS],
float peak_snr[MAX_NUM_PEAKS], int num_peaks_found)
{
float *peak_frequencies_publish[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z };
float *peak_snr_publish[] { _sensor_gyro_fft.peak_snr_x, _sensor_gyro_fft.peak_snr_y, _sensor_gyro_fft.peak_snr_z };
// new peak: r, old peak: c
float peak_frequencies_diff[MAX_NUM_PEAKS][MAX_NUM_PEAKS];
for (int peak_new = 0; peak_new < MAX_NUM_PEAKS; peak_new++) {
// compute distance to previous peaks
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if ((peak_frequencies[peak_new] > 0)
&& (peak_frequencies_publish[axis][peak_prev] > 0) && PX4_ISFINITE(peak_frequencies_publish[axis][peak_prev])
) {
peak_frequencies_diff[peak_new][peak_prev] = fabsf(peak_frequencies[peak_new] -
peak_frequencies_publish[axis][peak_prev]);
} else {
peak_frequencies_diff[peak_new][peak_prev] = INFINITY;
}
}
}
// go through peak_frequencies_diff and find smallest diff (closest peaks)
// - copy new peak to old peak slot
// - exclude new peak (row) and old peak (column) in search
// - repeat
//
// - finally copy unmatched peaks to empty slots
bool peak_new_copied[MAX_NUM_PEAKS] {};
bool peak_out_filled[MAX_NUM_PEAKS] {};
int peaks_copied = 0;
for (int new_peak = 0; new_peak < num_peaks_found; new_peak++) {
float smallest_diff = INFINITY;
int closest_new_peak = -1;
int closest_prev_peak = -1;
// find new peak with smallest difference to old peak
for (int peak_new = 0; peak_new < num_peaks_found; peak_new++) {
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if (!peak_new_copied[peak_new] && !peak_out_filled[peak_prev]
&& (peak_frequencies_diff[peak_new][peak_prev] < smallest_diff)) {
smallest_diff = peak_frequencies_diff[peak_new][peak_prev];
closest_new_peak = peak_new;
closest_prev_peak = peak_prev;
}
}
}
if (PX4_ISFINITE(smallest_diff) && (smallest_diff > 0)) {
// smallest diff found, copy newly found peak into same slot previously published
float peak_frequency = _median_filter[axis][closest_prev_peak].apply(peak_frequencies[closest_new_peak]);
if (peak_frequency > 0) {
peak_frequencies_publish[axis][closest_prev_peak] = peak_frequency;
peak_snr_publish[axis][closest_prev_peak] = peak_snr[closest_new_peak];
peaks_copied++;
_last_update[axis][closest_prev_peak] = timestamp_sample;
_sensor_gyro_fft.timestamp_sample = timestamp_sample;
_publish = true;
// clear
peak_frequencies[closest_new_peak] = NAN;
peak_frequencies_diff[closest_new_peak][closest_prev_peak] = NAN;
peak_new_copied[closest_new_peak] = true;
peak_out_filled[closest_prev_peak] = true;
if (peaks_copied == num_peaks_found) {
break;
}
}
}
}
// clear any stale entries
for (int peak_out = 0; peak_out < MAX_NUM_PEAKS; peak_out++) {
if (timestamp_sample - _last_update[axis][peak_out] > 500_ms) {
peak_frequencies_publish[axis][peak_out] = NAN;
peak_snr_publish[axis][peak_out] = NAN;
_last_update[axis][peak_out] = 0;
}
}
// copy any remaining new (unmatched) peaks to overwrite old or empty slots
if (peaks_copied != num_peaks_found) {
for (int peak_new = 0; peak_new < num_peaks_found; peak_new++) {
if (PX4_ISFINITE(peak_frequencies[peak_new]) && (peak_frequencies[peak_new] > 0)) {
int oldest_slot = -1;
hrt_abstime oldest = timestamp_sample;
// find oldest slot and replace with new peak frequency
for (int peak_prev = 0; peak_prev < MAX_NUM_PEAKS; peak_prev++) {
if (_last_update[axis][peak_prev] < oldest) {
oldest_slot = peak_prev;
oldest = _last_update[axis][peak_prev];
}
}
if (oldest_slot >= 0) {
// copy peak to output slot
float peak_frequency = _median_filter[axis][oldest_slot].apply(peak_frequencies[peak_new]);
if (peak_frequency > 0) {
peak_frequencies_publish[axis][oldest_slot] = peak_frequency;
peak_snr_publish[axis][oldest_slot] = peak_snr[peak_new];
_last_update[axis][oldest_slot] = timestamp_sample;
_sensor_gyro_fft.timestamp_sample = timestamp_sample;
_publish = true;
}
}
}
}
}
}
void GyroFFT::Publish()
{
_sensor_gyro_fft.device_id = _selected_sensor_device_id;
_sensor_gyro_fft.sensor_sample_rate_hz = _gyro_sample_rate_hz;
_sensor_gyro_fft.resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len;
_sensor_gyro_fft.timestamp = hrt_absolute_time();
_sensor_gyro_fft_pub.publish(_sensor_gyro_fft);
}
int GyroFFT::task_spawn(int argc, char *argv[])

25
src/modules/gyro_fft/GyroFFT.hpp
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

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

@ -49,7 +49,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0);
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 32.f);
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 30.f);
/**
* IMU gyro FFT maximum frequency.
@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 32.f);
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 192.f);
PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 150.f);
/**
* IMU gyro FFT length.

4
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
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?