From 43fb076828754a61276ea5b1ebddcfd3f526c6ca Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 17 Nov 2021 16:02:49 +0100 Subject: [PATCH] AngularAcceleration: explicitly disable filter when cutoff <= 0 --- src/lib/mathlib/math/filter/AlphaFilter.hpp | 9 ++++----- src/lib/mathlib/math/test/AlphaFilterTest.cpp | 16 ++++++++++++++++ .../VehicleAngularVelocity.cpp | 10 ++++++++-- 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 677cfdd9ef..0635ae438c 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -73,19 +73,18 @@ public: } } - void setCutoffFreq(float sample_freq, float cutoff_freq) + bool setCutoffFreq(float sample_freq, float cutoff_freq) { if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2.f) || !isFinite(sample_freq) || !isFinite(cutoff_freq)) { - // No filtering - _alpha = 1.f; - _cutoff_freq = 0.f; - return; + // Invalid parameters + return false; } setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq)); _cutoff_freq = cutoff_freq; + return true; } /** diff --git a/src/lib/mathlib/math/test/AlphaFilterTest.cpp b/src/lib/mathlib/math/test/AlphaFilterTest.cpp index 1e0cbc1889..30661ea82b 100644 --- a/src/lib/mathlib/math/test/AlphaFilterTest.cpp +++ b/src/lib/mathlib/math/test/AlphaFilterTest.cpp @@ -241,6 +241,22 @@ TEST(AlphaFilterTest, AlphaZeroTest) } } +TEST(AlphaFilterTest, SetFrequencyTest) +{ + AlphaFilter _alpha_filter; + const float fs = .1f; + + EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, 0.f)); + EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, fs)); // Cutoff above Nyquist freq + EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, fs / 4.f)); + EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, 0.f)); + EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, fs / 4.f)); + EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, -fs / 4.f)); + EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, -fs / 4.f)); + + EXPECT_TRUE(_alpha_filter.setCutoffFreq(fs, fs / 4.f)); +} + TEST(AlphaFilterTest, ConvergenceTest) { AlphaFilter _alpha_filter; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index d61fa58c9d..facbb19a47 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -167,8 +167,14 @@ void VehicleAngularVelocity::ResetFilters() _notch_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis)); // angular acceleration low pass - _lp_filter_acceleration[axis].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()); - _lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis)); + if ((_param_imu_dgyro_cutoff.get() > 0.f) + && (_lp_filter_acceleration[axis].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()))) { + _lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis)); + + } else { + // disable filtering + _lp_filter_acceleration[axis].setAlpha(1.f); + } } // force reset notch filters on any scale change