zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

gyro_fft: require higher SNR for initially tracking a peak 

- 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
master
Daniel Agar 3 years ago
parent
6d78054f50
commit
cc4152e10d
5 changed files with 225 additions and 180 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      posix-configs/SITL/init/test/test_imu_filtering
  2. 371
      src/modules/gyro_fft/GyroFFT.cpp
  3. 25
      src/modules/gyro_fft/GyroFFT.hpp
  4. 4
      src/modules/gyro_fft/parameters.c
  5. 4
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

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

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

371
src/modules/gyro_fft/GyroFFT.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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) @@ -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() @@ -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() @@ -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,206 +404,241 @@ void GyroFFT::Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint @@ -404,206 +404,241 @@ 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;
FindPeaks(timestamp_sample, axis, _fft_outupt_buffer);
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];
// reset
// shift buffer (3/4 overlap)
const int overlap_start = _imu_gyro_fft_len / 4;
memmove(&gyro_data_buffer[axis][0], &gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3);
buffer_index = overlap_start * 3;
const float fft_magnitude_squared = real * real + imag * imag;
perf_end(_fft_perf);
}
}
}
}
bin_mag_sum += fft_magnitude_squared;
}
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;
// find raw peaks
int raw_peak_index[MAX_NUM_PEAKS] {};
float raw_peak_magnitude[MAX_NUM_PEAKS] {};
float raw_peak_snr[MAX_NUM_PEAKS] {};
// sum total energy across all used buckets for SNR
float bin_mag_sum = 0;
// 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;
// find raw peaks
uint16_t raw_peak_index[MAX_NUM_PEAKS] {};
float peak_magnitude[MAX_NUM_PEAKS] {};
if ((freq_hz >= _param_imu_gyro_fft_max.get()) || (freq_hz >= (_gyro_sample_rate_hz / 2.f))) {
break;
}
// 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 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 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));
const float freq_hz = (bucket_index / 2) * resolution_hz;
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;
}
}
}
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;
}
}
}
}
int num_peaks_found = 0;
float peak_frequencies[MAX_NUM_PEAKS] {};
float peak_snr[MAX_NUM_PEAKS] {};
// 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?
// 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;
int num_peaks_found = 0;
float peak_frequencies[MAX_NUM_PEAKS] {};
float peak_snr[MAX_NUM_PEAKS] {};
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())) {
float *peak_frequencies_publish[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z };
peak_frequencies[num_peaks_found] = freq_adjusted;
peak_snr[num_peaks_found] = raw_peak_snr[i];
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) {
float peak_frequencies_diff[MAX_NUM_PEAKS][MAX_NUM_PEAKS];
if (num_peaks_found > 0) {
UpdateOutput(timestamp_sample, axis, peak_frequencies, peak_snr, num_peaks_found);
}
}
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)) {
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 };
peak_frequencies_diff[peak_new][peak_prev] = fabsf(peak_frequencies[peak_new] -
_peak_frequencies_prev[axis][peak_prev]);
// new peak: r, old peak: c
float peak_frequencies_diff[MAX_NUM_PEAKS][MAX_NUM_PEAKS];
} else {
peak_frequencies_diff[peak_new][peak_prev] = INFINITY;
}
}
}
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]);
// 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;
}
}
}
} else {
peak_frequencies_diff[peak_new][peak_prev] = INFINITY;
}
}
}
// 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;
}
}
// 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;
}
}
}
// 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;
}
}
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]);
// 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;
}
}
}
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];
}
}
// reset
// shift buffer (3/4 overlap)
const int overlap_start = _imu_gyro_fft_len / 4;
memmove(&gyro_data_buffer[axis][0], &gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3);
buffer_index = overlap_start * 3;
if (oldest_slot >= 0) {
// copy peak to output slot
float peak_frequency = _median_filter[axis][oldest_slot].apply(peak_frequencies[peak_new]);
perf_end(_fft_perf);
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;
}
}
}
}
}
}
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::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
Unescape View File

@ -31,7 +31,8 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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
Unescape View File

@ -49,7 +49,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0); @@ -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); @@ -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
Unescape View File

@ -523,10 +523,10 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) @@ -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?

Write Preview
Loading…
Cancel
Save