From 376b72fb2fee02d49c392f1a038cc2a75402c89e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 3 Nov 2021 11:23:37 -0400 Subject: [PATCH] sensors/vehicle_angular_velocity: add new parameter for ESC RPM notch filter BW --- .../SITL/init/test/test_imu_filtering | 6 +-- .../VehicleAngularVelocity.cpp | 44 ++++++++++++------- .../VehicleAngularVelocity.hpp | 11 ++++- .../imu_gyro_parameters.c | 14 +++++- 4 files changed, 53 insertions(+), 22 deletions(-) diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering index 28a0dde645..14a999a8fe 100644 --- a/posix-configs/SITL/init/test/test_imu_filtering +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -20,9 +20,9 @@ param set IMU_GYRO_FFT_MAX 1000 param set IMU_GYRO_FFT_LEN 512 # dynamic notches ESC/FFT/both -#param set IMU_GYRO_DYN_NF 1 -#param set IMU_GYRO_DYN_NF 2 -param set IMU_GYRO_DYN_NF 3 +#param set IMU_GYRO_DNF_EN 1 +#param set IMU_GYRO_DNF_EN 2 +param set IMU_GYRO_DNF_EN 3 # test values param set IMU_GYRO_CUTOFF 60 diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index f341595a2b..5c4bf66275 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -55,9 +55,14 @@ VehicleAngularVelocity::~VehicleAngularVelocity() perf_free(_filter_reset_perf); perf_free(_selection_changed_perf); #if !defined(CONSTRAINED_FLASH) + perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); + perf_free(_dynamic_notch_filter_esc_rpm_reset_perf); perf_free(_dynamic_notch_filter_esc_rpm_update_perf); - perf_free(_dynamic_notch_filter_fft_update_perf); perf_free(_dynamic_notch_filter_esc_rpm_perf); + + perf_free(_dynamic_notch_filter_fft_disable_perf); + perf_free(_dynamic_notch_filter_fft_reset_perf); + perf_free(_dynamic_notch_filter_fft_update_perf); perf_free(_dynamic_notch_filter_fft_perf); #endif // CONSTRAINED_FLASH } @@ -313,13 +318,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) #if !defined(CONSTRAINED_FLASH) - if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm) { - if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) { + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) { + if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) { + _dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT, + MODULE_NAME": gyro dynamic notch filter ESC RPM disable"); + _dynamic_notch_filter_esc_rpm_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM reset"); _dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM update"); - } - - if (_dynamic_notch_filter_esc_rpm_perf == nullptr) { _dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter"); } @@ -327,12 +332,11 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) DisableDynamicNotchEscRpm(); } - if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) { - if (_dynamic_notch_filter_fft_update_perf == nullptr) { + if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) { + if (_dynamic_notch_filter_fft_disable_perf == nullptr) { + _dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable"); + _dynamic_notch_filter_fft_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT reset"); _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); - } - - if (_dynamic_notch_filter_fft_perf == nullptr) { _dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter"); } @@ -392,6 +396,7 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm() } _esc_available.set(esc, false); + perf_count(_dynamic_notch_filter_esc_rpm_disable_perf); } } @@ -413,6 +418,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() } _dynamic_notch_fft_available = false; + perf_count(_dynamic_notch_filter_fft_disable_perf); } #endif // !CONSTRAINED_FLASH @@ -421,7 +427,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) { #if !defined(CONSTRAINED_FLASH) - const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm; + const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm; if (enabled && (_esc_status_sub.updated() || force)) { @@ -454,10 +460,9 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) // update filter parameters if frequency changed or forced if (force || reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > FLT_EPSILON)) { - static constexpr float ESC_NOTCH_BW_HZ = 8.f; // TODO: configurable bandwidth // force reset if the notch frequency jumps significantly - if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > ESC_NOTCH_BW_HZ)) { + if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > _param_imu_gyro_dnf_bw.get())) { reset = true; } @@ -465,7 +470,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) const float frequency_hz = esc_hz * (harmonic + 1); for (int axis = 0; axis < 3; axis++) { - _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz, ESC_NOTCH_BW_HZ); + _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz, + _param_imu_gyro_dnf_bw.get()); } } @@ -480,6 +486,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset(reset_angular_velocity(axis)); } } + + perf_count(_dynamic_notch_filter_esc_rpm_reset_perf); } _dynamic_notch_esc_rpm_available = true; @@ -491,6 +499,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) // if this ESC was previously available disable all notch filters if forced or timeout _esc_available.set(esc, false); + perf_count(_dynamic_notch_filter_esc_rpm_disable_perf); + for (int axis = 0; axis < 3; axis++) { for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { _dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0); @@ -510,7 +520,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) { #if !defined(CONSTRAINED_FLASH) - const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT; + const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::FFT; if (enabled && (_sensor_gyro_fft_sub.updated() || force)) { @@ -556,6 +566,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) if (force || reset || (notch_freq_diff > bandwidth)) { const Vector3f reset_angular_velocity{GetResetAngularVelocity()}; nf.reset(reset_angular_velocity(axis)); + perf_count(_dynamic_notch_filter_fft_reset_perf); } _dynamic_notch_fft_available = true; @@ -564,6 +575,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) // disable this notch filter (if it isn't already) if (force || !reset) { nf.setParameters(0, 0, 0); + perf_count(_dynamic_notch_filter_fft_disable_perf); } } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 115af7ad4f..5cca681610 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -157,8 +157,14 @@ private: hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {}; perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr}; - perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_esc_rpm_reset_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr}; perf_counter_t _dynamic_notch_filter_esc_rpm_perf{nullptr}; + + perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_fft_reset_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_fft_perf{nullptr}; bool _dynamic_notch_esc_rpm_available{false}; @@ -178,7 +184,8 @@ private: DEFINE_PARAMETERS( #if !defined(CONSTRAINED_FLASH) - (ParamInt) _param_imu_gyro_dyn_nf, + (ParamInt) _param_imu_gyro_dnf_en, + (ParamFloat) _param_imu_gyro_dnf_bw, #endif // !CONSTRAINED_FLASH (ParamFloat) _param_imu_gyro_cutoff, (ParamFloat) _param_imu_gyro_nf_freq, 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 0a24c4003b..ccff54b888 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -136,4 +136,16 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); * @bit 0 ESC RPM * @bit 1 FFT */ -PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0); +PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0); + +/** +* IMU gyro ESC notch filter bandwidth +* +* Bandwidth per notch filter when using dynamic notch filtering with ESC RPM. +* +* @group Sensors +* @category Developer +* @min 5 +* @max 30 +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 8.f);