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); -}