From 6cab14fc5d0ba3cea0cf11ae69e0d08ed72bf0ec Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 24 Oct 2019 10:50:14 +0200 Subject: [PATCH] ekf2: Fix heading not stable issue. The prblem was that a large dt could drive the alpha filter crazy because of the small dt approximation during the computation of the coefficient. - Remove unused bitmask packing of the innovation checks --- src/modules/ekf2/Utility/InnovationLpf.hpp | 2 +- src/modules/ekf2/Utility/PreFlightChecker.cpp | 6 ------ src/modules/ekf2/Utility/PreFlightChecker.hpp | 6 ------ src/modules/ekf2/Utility/PreFlightCheckerTest.cpp | 15 --------------- 4 files changed, 1 insertion(+), 28 deletions(-) diff --git a/src/modules/ekf2/Utility/InnovationLpf.hpp b/src/modules/ekf2/Utility/InnovationLpf.hpp index a542f6aa4a..adf897406e 100644 --- a/src/modules/ekf2/Utility/InnovationLpf.hpp +++ b/src/modules/ekf2/Utility/InnovationLpf.hpp @@ -71,7 +71,7 @@ public: */ static float computeAlphaFromDtAndTauInv(float dt, float tau_inv) { - return dt * tau_inv; + return math::constrain(dt * tau_inv, 0.f, 1.f); } private: diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index 36dc3e695e..732870ea8f 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -117,12 +117,6 @@ bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov, const Vector2f || innov.norm_squared() > sq(2.0f * test_limit); } -uint8_t PreFlightChecker::prefltFailBoolToBitMask(const bool heading_failed, const bool horiz_vel_failed, - const bool vert_vel_failed, const bool height_failed) -{ - return heading_failed | (horiz_vel_failed << 1) | (vert_vel_failed << 2) | (height_failed << 3); -} - void PreFlightChecker::reset() { _is_using_gps_aiding = false; diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index 0bff50dafe..6d6da18c3a 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -124,12 +124,6 @@ public: */ static bool checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, float test_limit); - /* - * Packs the boolean flags into a bit field - */ - static uint8_t prefltFailBoolToBitMask(bool heading_failed, bool horiz_vel_failed, bool vert_vel_failed, - bool height_failed); - static constexpr float sq(float var) { return var * var; } private: diff --git a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp b/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp index 03ae5a942a..eda7042c36 100644 --- a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp +++ b/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp @@ -91,18 +91,3 @@ TEST_F(PreFlightCheckerTest, testInnov2dFailed) EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], 1.42)); } } - -TEST_F(PreFlightCheckerTest, testBitMask) -{ - PreFlightChecker preflt_checker; - - const bool heading_failed = true; - const bool horiz_vel_failed = true; - const bool down_vel_failed = false; - const bool height_failed = true; - - int bitmask = PreFlightChecker::prefltFailBoolToBitMask(heading_failed, horiz_vel_failed, down_vel_failed, - height_failed); - - EXPECT_EQ(bitmask, 0b1011); -}