diff --git a/msg/sensor_gyro_fft.msg b/msg/sensor_gyro_fft.msg index 31d1906d15..203c43aa8e 100644 --- a/msg/sensor_gyro_fft.msg +++ b/msg/sensor_gyro_fft.msg @@ -3,12 +3,9 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -float32 dt # delta time between samples (microseconds) - +float32 sensor_sample_rate_hz float32 resolution_hz -float32[3] peak_frequency # peak frequency per axis - -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[6] peak_frequencies_x # x axis peak frequencies +float32[6] peak_frequencies_y # y axis peak frequencies +float32[6] peak_frequencies_z # z axis peak frequencies diff --git a/src/examples/gyro_fft/GyroFFT.cpp b/src/examples/gyro_fft/GyroFFT.cpp index accbd0e05d..2cca1c2af8 100644 --- a/src/examples/gyro_fft/GyroFFT.cpp +++ b/src/examples/gyro_fft/GyroFFT.cpp @@ -44,11 +44,45 @@ GyroFFT::GyroFFT() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { - arm_rfft_init_q15(&_rfft_q15, FFT_LENGTH, 0, 1); + switch (_param_imu_gyro_fft_len.get()) { + case 128: + AllocateBuffers<128>(); + break; + + case 256: + AllocateBuffers<256>(); + break; + + case 512: + AllocateBuffers<512>(); + break; + + case 1024: + AllocateBuffers<1024>(); + break; + + case 2048: + AllocateBuffers<2048>(); + break; + + case 4096: + AllocateBuffers<4096>(); + break; + + default: + // otherwise default to 256 + PX4_ERR("Invalid IMU_GYRO_FFT_LEN=%.3f, resetting", (double)_param_imu_gyro_fft_len.get()); + AllocateBuffers<256>(); + _param_imu_gyro_fft_len.set(256); + _param_imu_gyro_fft_len.commit(); + break; + } + + arm_rfft_init_q15(&_rfft_q15, _param_imu_gyro_fft_len.get(), 0, 1); // init Hanning window - for (int n = 0; n < FFT_LENGTH; n++) { - const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (FFT_LENGTH - 1))); + for (int n = 0; n < _param_imu_gyro_fft_len.get(); n++) { + const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (_param_imu_gyro_fft_len.get() - 1))); arm_float_to_q15(&hanning_value, &_hanning_window[n], 1); } } @@ -59,6 +93,13 @@ GyroFFT::~GyroFFT() perf_free(_cycle_interval_perf); perf_free(_fft_perf); perf_free(_gyro_fifo_generation_gap_perf); + + delete _gyro_data_buffer_x; + delete _gyro_data_buffer_y; + delete _gyro_data_buffer_z; + delete _hanning_window; + delete _fft_input_buffer; + delete _fft_outupt_buffer; } bool GyroFFT::init() @@ -77,13 +118,11 @@ bool GyroFFT::SensorSelectionUpdate(bool force) sensor_selection_s sensor_selection{}; _sensor_selection_sub.copy(&sensor_selection); - if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { + if ((sensor_selection.gyro_device_id != 0) && (_selected_sensor_device_id != sensor_selection.gyro_device_id)) { for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; - if ((sensor_gyro_fifo_sub.get().device_id != 0) - && (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) { - + if (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id) { if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) { // find corresponding vehicle_imu_status instance for (uint8_t imu_status = 0; imu_status < MAX_SENSOR_COUNT; imu_status++) { @@ -94,6 +133,7 @@ bool GyroFFT::SensorSelectionUpdate(bool force) if (imu_status_sub.copy(&vehicle_imu_status)) { if (vehicle_imu_status.gyro_device_id == sensor_selection.gyro_device_id) { _vehicle_imu_status_sub.ChangeInstance(imu_status); + _selected_sensor_device_id = sensor_selection.gyro_device_id; return true; } } @@ -117,8 +157,11 @@ void GyroFFT::VehicleIMUStatusUpdate() vehicle_imu_status_s vehicle_imu_status; if (_vehicle_imu_status_sub.update(&vehicle_imu_status)) { - if ((vehicle_imu_status.gyro_rate_hz > 0) && (fabsf(vehicle_imu_status.gyro_rate_hz - _gyro_sample_rate_hz) > 1.f)) { - _gyro_sample_rate_hz = vehicle_imu_status.gyro_rate_hz; + if ((vehicle_imu_status.gyro_device_id == _selected_sensor_device_id) + && (vehicle_imu_status.gyro_rate_hz > 0) + && (fabsf(vehicle_imu_status.gyro_rate_hz - _gyro_sample_rate_hz) > 1.f)) { + + _gyro_sample_rate_hz = vehicle_imu_status.gyro_raw_rate_hz; } } } @@ -133,7 +176,7 @@ static constexpr float tau(float x) return (1.f / 4.f * p1 - sqrtf(6) / 24 * p2); } -float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index) +float GyroFFT::EstimatePeakFrequency(q15_t fft[], uint8_t peak_index) { // find peak location using Quinn's Second Estimator (2020-06-14: http://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/) int16_t real[3] { fft[peak_index - 2], fft[peak_index], fft[peak_index + 2] }; @@ -154,20 +197,11 @@ float GyroFFT::EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_ind float d = (dp + dm) / 2 + tau(dp * dp) - tau(dm * dm); float adjusted_bin = peak_index + d; - float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (FFT_LENGTH * 2.f)); + float peak_freq_adjusted = (_gyro_sample_rate_hz * adjusted_bin / (_param_imu_gyro_fft_len.get() * 2.f)); return peak_freq_adjusted; } -static int float_cmp(const void *elem1, const void *elem2) -{ - if (*(const float *)elem1 < * (const float *)elem2) { - return -1; - } - - return *(const float *)elem1 > *(const float *)elem2; -} - void GyroFFT::Run() { if (should_exit()) { @@ -192,8 +226,9 @@ void GyroFFT::Run() } SensorSelectionUpdate(); + VehicleIMUStatusUpdate(); - const float resolution_hz = _gyro_sample_rate_hz / FFT_LENGTH; + const float resolution_hz = _gyro_sample_rate_hz / _param_imu_gyro_fft_len.get(); bool publish = false; bool fft_updated = false; @@ -212,40 +247,35 @@ void GyroFFT::Run() perf_count(_gyro_fifo_generation_gap_perf); } + if (fabsf(sensor_gyro_fifo.scale - _fifo_last_scale) > FLT_EPSILON) { + // force reset if scale has changed + _fft_buffer_index[0] = 0; + _fft_buffer_index[1] = 0; + _fft_buffer_index[2] = 0; + + _fifo_last_scale = sensor_gyro_fifo.scale; + } + _gyro_last_generation = _sensor_gyro_fifo_sub.get_last_generation(); const int N = sensor_gyro_fifo.samples; for (int axis = 0; axis < 3; axis++) { - int16_t *input = nullptr; - - switch (axis) { - case 0: - input = sensor_gyro_fifo.x; - break; - - case 1: - input = sensor_gyro_fifo.y; - break; - - case 2: - input = sensor_gyro_fifo.z; - break; - } + int16_t *input[] {sensor_gyro_fifo.x, sensor_gyro_fifo.y, sensor_gyro_fifo.z}; + q15_t *gyro_data_buffer[] {_gyro_data_buffer_x, _gyro_data_buffer_y, _gyro_data_buffer_z}; int &buffer_index = _fft_buffer_index[axis]; for (int n = 0; n < N; n++) { - if (buffer_index < FFT_LENGTH) { + if (buffer_index < _param_imu_gyro_fft_len.get()) { // convert int16_t -> q15_t (scaling isn't relevant) - _gyro_data_buffer[axis][buffer_index] = input[n] / 2; + gyro_data_buffer[axis][buffer_index] = input[axis][n] / 2; buffer_index++; } // if we have enough samples begin processing, but only one FFT per cycle - if ((buffer_index >= FFT_LENGTH) && !fft_updated) { - - arm_mult_q15(_gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, FFT_LENGTH); + if ((buffer_index >= _param_imu_gyro_fft_len.get()) && !fft_updated) { + arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _param_imu_gyro_fft_len.get()); perf_begin(_fft_perf); arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer); @@ -254,16 +284,13 @@ void GyroFFT::Run() static constexpr uint16_t MIN_SNR = 10; // TODO: - uint32_t max_peak_magnitude = 0; - uint8_t max_peak_index = 0; - - static constexpr int MAX_NUM_PEAKS = 4; + bool peaks_detected = false; uint32_t peaks_magnitude[MAX_NUM_PEAKS] {}; uint8_t peak_index[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 (uint8_t bucket_index = 2; bucket_index < (FFT_LENGTH / 2); bucket_index = bucket_index + 2) { + for (uint8_t bucket_index = 2; bucket_index < (_param_imu_gyro_fft_len.get() / 2); bucket_index = bucket_index + 2) { const float freq_hz = (bucket_index / 2) * resolution_hz; if (freq_hz > _param_imu_gyro_fft_max.get()) { @@ -277,17 +304,11 @@ void GyroFFT::Run() const uint32_t fft_magnitude_squared = real * real + complex * complex; if (fft_magnitude_squared > MIN_SNR) { - - if (fft_magnitude_squared > max_peak_magnitude) { - max_peak_magnitude = fft_magnitude_squared; - max_peak_index = bucket_index; - } - for (int i = 0; i < MAX_NUM_PEAKS; i++) { if (fft_magnitude_squared > peaks_magnitude[i]) { peaks_magnitude[i] = fft_magnitude_squared; peak_index[i] = bucket_index; - publish = true; + peaks_detected = true; break; } } @@ -295,64 +316,45 @@ void GyroFFT::Run() } } - if (max_peak_index > 0) { - _sensor_gyro_fft.peak_frequency[axis] = _median_filter[axis].apply(EstimatePeakFrequency(_fft_outupt_buffer, - max_peak_index)); - } + if (peaks_detected) { + float *peak_frequencies[] { _sensor_gyro_fft.peak_frequencies_x, _sensor_gyro_fft.peak_frequencies_y, _sensor_gyro_fft.peak_frequencies_z}; - if (publish) { - float *peak_frequencies; - - switch (axis) { - case 0: - peak_frequencies = _sensor_gyro_fft.peak_frequencies_x; - break; - - case 1: - peak_frequencies = _sensor_gyro_fft.peak_frequencies_y; - break; - - case 2: - peak_frequencies = _sensor_gyro_fft.peak_frequencies_z; - break; - } - - int peaks_found = 0; + int num_peaks_found = 0; for (int i = 0; i < MAX_NUM_PEAKS; i++) { - if ((peak_index[i] > 0) && (peak_index[i] < FFT_LENGTH) && (peaks_magnitude[i] > 0)) { + if ((peak_index[i] > 0) && (peak_index[i] < _param_imu_gyro_fft_len.get()) && (peaks_magnitude[i] > 0)) { const float freq = EstimatePeakFrequency(_fft_outupt_buffer, peak_index[i]); if (freq >= _param_imu_gyro_fft_min.get() && freq <= _param_imu_gyro_fft_max.get()) { - peak_frequencies[peaks_found] = freq; - peaks_found++; + + if (fabsf(peak_frequencies[axis][num_peaks_found] - freq) > 0.1f) { + publish = true; + } + + peak_frequencies[axis][num_peaks_found] = freq; + num_peaks_found++; } } } // mark remaining slots empty - for (int i = peaks_found; i < MAX_NUM_PEAKS; i++) { - peak_frequencies[i] = NAN; - } - - // publish in sorted order for convenience - if (peaks_found > 0) { - qsort(peak_frequencies, peaks_found, sizeof(float), float_cmp); + for (int i = num_peaks_found; i < MAX_NUM_PEAKS; i++) { + peak_frequencies[axis][i] = NAN; } } // reset - // shift buffer (75% overlap) - int overlap_start = FFT_LENGTH / 4; - memmove(&_gyro_data_buffer[axis][0], &_gyro_data_buffer[axis][overlap_start], sizeof(q15_t) * overlap_start * 3); + // shift buffer (3/4 overlap) + const int overlap_start = _param_imu_gyro_fft_len.get() / 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 (publish) { - _sensor_gyro_fft.dt = 1e6f / _gyro_sample_rate_hz; _sensor_gyro_fft.device_id = sensor_gyro_fifo.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 = sensor_gyro_fifo.timestamp_sample; _sensor_gyro_fft.timestamp = hrt_absolute_time(); @@ -390,6 +392,7 @@ int GyroFFT::task_spawn(int argc, char *argv[]) int GyroFFT::print_status() { + PX4_INFO("gyro sample rate: %.3f Hz", (double)_gyro_sample_rate_hz); perf_print_counter(_cycle_perf); perf_print_counter(_cycle_interval_perf); perf_print_counter(_fft_perf); diff --git a/src/examples/gyro_fft/GyroFFT.hpp b/src/examples/gyro_fft/GyroFFT.hpp index 9f19400e2c..509dacfd1f 100644 --- a/src/examples/gyro_fft/GyroFFT.hpp +++ b/src/examples/gyro_fft/GyroFFT.hpp @@ -77,14 +77,26 @@ public: bool init(); private: - static constexpr uint16_t FFT_LENGTH = 2048; - - float EstimatePeakFrequency(q15_t fft[FFT_LENGTH * 2], uint8_t peak_index); + float EstimatePeakFrequency(q15_t fft[], uint8_t peak_index); void Run() override; bool SensorSelectionUpdate(bool force = false); void VehicleIMUStatusUpdate(); - static constexpr int MAX_SENSOR_COUNT = 3; + template + void AllocateBuffers() + { + _gyro_data_buffer_x = new q15_t[N]; + _gyro_data_buffer_y = new q15_t[N]; + _gyro_data_buffer_z = new q15_t[N]; + _hanning_window = new q15_t[N]; + _fft_input_buffer = new q15_t[N]; + _fft_outupt_buffer = new q15_t[N * 2]; + } + + 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_pub{ORB_ID(sensor_gyro_fft)}; @@ -104,22 +116,25 @@ private: arm_rfft_instance_q15 _rfft_q15; - q15_t _gyro_data_buffer[3][FFT_LENGTH] {}; - q15_t _hanning_window[FFT_LENGTH] {}; - q15_t _fft_input_buffer[FFT_LENGTH] {}; - q15_t _fft_outupt_buffer[FFT_LENGTH * 2] {}; + q15_t *_gyro_data_buffer_x{nullptr}; + q15_t *_gyro_data_buffer_y{nullptr}; + q15_t *_gyro_data_buffer_z{nullptr}; + q15_t *_hanning_window{nullptr}; + q15_t *_fft_input_buffer{nullptr}; + q15_t *_fft_outupt_buffer{nullptr}; float _gyro_sample_rate_hz{8000}; // 8 kHz default + float _fifo_last_scale{0}; + int _fft_buffer_index[3] {}; unsigned _gyro_last_generation{0}; - math::MedianFilter _median_filter[3] {}; - sensor_gyro_fft_s _sensor_gyro_fft{}; DEFINE_PARAMETERS( + (ParamInt) _param_imu_gyro_fft_len, (ParamFloat) _param_imu_gyro_fft_min, (ParamFloat) _param_imu_gyro_fft_max ) diff --git a/src/examples/gyro_fft/parameters.c b/src/examples/gyro_fft/parameters.c index 4c1158ae66..0704905700 100644 --- a/src/examples/gyro_fft/parameters.c +++ b/src/examples/gyro_fft/parameters.c @@ -61,3 +61,18 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MIN, 50.0f); * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_FFT_MAX, 200.0f); + +/** +* IMU gyro FFT length. +* +* @value 128 128 +* @value 256 256 +* @value 512 512 +* @value 1024 1024 +* @value 2048 2048 +* @value 4096 4096 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_FFT_LEN, 256); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index a2778ca36e..587cc8e59c 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -151,6 +151,10 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons // angular acceleration low pass _lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()); _lp_filter_acceleration[axis].reset(angular_acceleration(axis)); + + // dynamic notch filters, first disable, then force update (if available) + DisableDynamicNotchFFT(); + UpdateDynamicNotchFFT(true); } _reset_filters = false; @@ -281,6 +285,68 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) } } +void VehicleAngularVelocity::DisableDynamicNotchFFT() +{ +#if !defined(CONSTRAINED_FLASH) + + // device id mismatch, disable all + for (auto &dnf : _dynamic_notch_filter) { + for (int axis = 0; axis < 3; axis++) { + dnf[axis].setParameters(0, 0, 0); + } + } + + _dynamic_notch_available = false; +#endif // !CONSTRAINED_FLASH +} + +void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) +{ +#if !defined(CONSTRAINED_FLASH) + + if (_param_imu_gyro_dyn_nf.get() && (_sensor_gyro_fft_sub.updated() || force)) { + sensor_gyro_fft_s sensor_gyro_fft; + + if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id) + && (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 0.05f)) { + + float *peak_frequencies[] { sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z}; + + for (int axis = 0; axis < 3; axis++) { + for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) { + auto &dnf = _dynamic_notch_filter[i][axis]; + const float &peak_freq = peak_frequencies[axis][i]; + + if (PX4_ISFINITE(peak_freq) && (peak_freq > 1.f)) { + const float change_percent = fabsf(dnf.getNotchFreq() - peak_freq) / peak_freq; + + if (change_percent > 0.001f) { + // peak frequency changed by at least 0.1% + dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz); + + // only reset if there's sufficient change (> 1%) + if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) { + dnf.reset(_angular_velocity(axis) / _fifo_last_scale); + } + } + + _dynamic_notch_available = true; + + } else { + // disable this notch filter + dnf.setParameters(0, 0, 0); + } + } + } + + } else { + DisableDynamicNotchFFT(); + } + } + +#endif // !CONSTRAINED_FLASH +} + void VehicleAngularVelocity::Run() { // backup schedule @@ -319,6 +385,8 @@ void VehicleAngularVelocity::Run() } } + UpdateDynamicNotchFFT(); + Vector3f angular_velocity_unscaled; Vector3f angular_acceleration_unscaled; @@ -332,6 +400,19 @@ void VehicleAngularVelocity::Run() data[n] = raw_data_array[axis][n]; } +#if !defined(CONSTRAINED_FLASH) + + // Apply dynamic notch filter from FFT + if (_dynamic_notch_available) { + for (auto &dnf : _dynamic_notch_filter) { + if (dnf[axis].getNotchFreq() > 0.f) { + dnf[axis].applyDF1(data, N); + } + } + } + +#endif // !CONSTRAINED_FLASH + // Apply general notch filter (IMU_GYRO_NF_FREQ) if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { _notch_filter_velocity[axis].apply(data, N); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index d0b84735ad..89287331d6 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -66,22 +67,21 @@ public: VehicleAngularVelocity(); ~VehicleAngularVelocity() override; + void PrintStatus(); bool Start(); void Stop(); - void PrintStatus(); - private: void Run() override; - void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration); - bool UpdateSampleRate(); - + void DisableDynamicNotchFFT(); void ParametersUpdate(bool force = false); + void Publish(const hrt_abstime ×tamp_sample); + void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration); void SensorBiasUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false); - - void Publish(const hrt_abstime ×tamp_sample); + void UpdateDynamicNotchFFT(bool force = false); + bool UpdateSampleRate(); static constexpr int MAX_SENSOR_COUNT = 4; @@ -90,6 +90,9 @@ private: uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; +#if !defined(CONSTRAINED_FLASH) + uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; +#endif // !CONSTRAINED_FLASH uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -118,6 +121,14 @@ private: math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; math::NotchFilterArray _notch_filter_velocity[3] {}; +#if !defined(CONSTRAINED_FLASH) + static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( + sensor_gyro_fft_s::peak_frequencies_x[0]); + + math::NotchFilterArray _dynamic_notch_filter[MAX_NUM_FFT_PEAKS][3] {}; + bool _dynamic_notch_available{false}; +#endif // !CONSTRAINED_FLASH + // angular acceleration filter math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; @@ -133,7 +144,8 @@ private: (ParamFloat) _param_imu_gyro_nf_freq, (ParamFloat) _param_imu_gyro_nf_bw, (ParamInt) _param_imu_gyro_ratemax, - (ParamFloat) _param_imu_dgyro_cutoff + (ParamFloat) _param_imu_dgyro_cutoff, + (ParamBool) _param_imu_gyro_dyn_nf ) }; diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index b625657116..eeac87b81e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -121,3 +121,15 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f); + +/** +* IMU gyro dynamic notch filtering +* +* Enable bank of dynamically updating notch filters. +* Requires onboard FFT (IMU_GYRO_FFT_EN). +* +* @boolean +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);