Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

AngularAcceleration: explicitly disable filter when cutoff <= 0

Browse Source
This commit is contained in:
bresch
2021-11-17 16:02:49 +01:00
committed by Mathieu Bresciani
parent 4a14a8bc7f
commit 43fb076828
3 changed files with 28 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/filter/AlphaFilter.hpp
+4 -5
View File
@@ -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) if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2.f)
|| !isFinite(sample_freq) || !isFinite(cutoff_freq)) { || !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
// No filtering // Invalid parameters
_alpha = 1.f; return false;
_cutoff_freq = 0.f;
return;
} }
setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq)); setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq));
_cutoff_freq = cutoff_freq; _cutoff_freq = cutoff_freq;
return true;
} }
/** /**
src/lib/mathlib/math/test/AlphaFilterTest.cpp
+16
View File
@@ -241,6 +241,22 @@ TEST(AlphaFilterTest, AlphaZeroTest)
} }
} }
TEST(AlphaFilterTest, SetFrequencyTest)
{
AlphaFilter<float> _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) TEST(AlphaFilterTest, ConvergenceTest)
{ {
AlphaFilter<float> _alpha_filter; AlphaFilter<float> _alpha_filter;
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+7 -1
View File
@@ -167,8 +167,14 @@ void VehicleAngularVelocity::ResetFilters()
_notch_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis)); _notch_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis));
// angular acceleration low pass // angular acceleration low pass
_lp_filter_acceleration[axis].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()); 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)); _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 // force reset notch filters on any scale change