Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

lpf test: move to common function

Browse Source
This commit is contained in:
bresch
2021-02-26 16:13:07 +01:00
committed by Daniel Agar
parent 844602c961
commit 3277648959
1 changed files with 48 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp
+48 -54
View File
@@ -47,69 +47,63 @@ using matrix::Vector3f;
class LowPassFilter2pVector3fTest : public ::testing::Test class LowPassFilter2pVector3fTest : public ::testing::Test
{ {
public: public:
void runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg, const Vector3f &gain_db);
math::LowPassFilter2pVector3f _lpf{800.f, 30.f}; math::LowPassFilter2pVector3f _lpf{800.f, 30.f};
const float _sample_freq = 1000.f;
const float _cutoff_freq = 80.f;
const float _epsilon_near = 10e-3f; const float _epsilon_near = 10e-3f;
}; };
void LowPassFilter2pVector3fTest::runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg,
const Vector3f &gain_db)
{
const Vector3f phase_delay = phase_delay_deg * M_PI_F / 180.f;
const Vector3f omega = 2.f * M_PI_F * signal_freq_hz;
Vector3f gain;
for (int i = 0; i < 3; i++) {
gain(i) = powf(10.f, gain_db(i) / 20.f);
}
const float dt = 1.f / _lpf.get_sample_freq();
float t = 0.f;
for (int i = 0; i < 1000; i++) {
Vector3f input{0.f, sinf(omega(1) * t), -sinf(omega(2) * t)};
Vector3f output_expected{0.f,
gain(1) *sinf(omega(1) * t - phase_delay(1)),
-gain(2) *sinf(omega(2) * t - phase_delay(2))};
Vector3f out = _lpf.apply(input);
t = i * dt;
// Let some time for the filter to settle
if (i > 30) {
EXPECT_EQ(out(0), 0.f);
EXPECT_NEAR(out(1), output_expected(1), _epsilon_near);
EXPECT_NEAR(out(2), output_expected(2), _epsilon_near);
}
}
}
TEST_F(LowPassFilter2pVector3fTest, setGet) TEST_F(LowPassFilter2pVector3fTest, setGet)
{ {
_lpf.set_cutoff_frequency(_sample_freq, _cutoff_freq); const float sample_freq = 1000.f;
EXPECT_EQ(_lpf.get_sample_freq(), _sample_freq); const float cutoff_freq = 80.f;
EXPECT_EQ(_lpf.get_cutoff_freq(), _cutoff_freq);
_lpf.set_cutoff_frequency(sample_freq, cutoff_freq);
EXPECT_EQ(_lpf.get_sample_freq(), sample_freq);
EXPECT_EQ(_lpf.get_cutoff_freq(), cutoff_freq);
} }
TEST_F(LowPassFilter2pVector3fTest, belowCutoff) TEST_F(LowPassFilter2pVector3fTest, belowAndAboveCutoff)
{ {
_lpf.set_cutoff_frequency(_sample_freq, _cutoff_freq); const float sample_freq = 1000.f;
const float cutoff_freq = 80.f;
_lpf.set_cutoff_frequency(sample_freq, cutoff_freq);
const float signal_freq = 10.f; const Vector3f signal_freq_hz{0.f, 10.f, 100.f};
const float omega = 2.f * M_PI_F * signal_freq; const Vector3f phase_delay_deg = Vector3f{0.f, 10.4f, 108.5f}; // Given by simulation
const float phase_delay = 10.4f * M_PI_F / 180.f; // Given by simulation const Vector3f gain_db{0.f, 0.f, -5.66f}; // given by simulation
const float dt = 1.f / _sample_freq; runSimulatedFilter(signal_freq_hz, phase_delay_deg, gain_db);
float t = 0.f;
for (int i = 0; i < 1000; i++) {
float input = sinf(omega * t);
float output_expected = sinf(omega * t - phase_delay);
Vector3f out = _lpf.apply(Vector3f(0.f, input, -input));
t = i * dt;
// Let some time for the filter to settle
if (i > 30) {
EXPECT_EQ(out(0), 0.f);
EXPECT_NEAR(out(1), output_expected, _epsilon_near);
EXPECT_NEAR(out(2), -output_expected, _epsilon_near);
}
}
}
TEST_F(LowPassFilter2pVector3fTest, aboveCutoff)
{
_lpf.set_cutoff_frequency(_sample_freq, _cutoff_freq);
const float signal_freq = 100.f;
const float omega = 2.f * M_PI_F * signal_freq;
const float phase_delay = 108.5f * M_PI_F / 180.f; // Given by simulation
const float gain = 0.52f; // = -5.66 dB, given by simulation
const float dt = 1.f / _sample_freq;
float t = 0.f;
for (int i = 0; i < 1000; i++) {
float input = sinf(omega * t);
float output_expected = gain * sinf(omega * t - phase_delay);
Vector3f out = _lpf.apply(Vector3f(0.f, input, -input));
t = i * dt;
// Let some time for the filter to settle
if (i > 30) {
EXPECT_EQ(out(0), 0.f);
EXPECT_NEAR(out(1), output_expected, _epsilon_near);
EXPECT_NEAR(out(2), -output_expected, _epsilon_near);
}
}
} }