fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/mathlib/math/filter/AlphaFilter.hpp
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)
|| !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;
}
/**

16
src/lib/mathlib/math/test/AlphaFilterTest.cpp
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)
{
AlphaFilter<float> _alpha_filter;

10
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -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