fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Fix heading not stable issue. The prblem was that a large

Browse Source 
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
This commit is contained in:
bresch 2019-10-24 10:50:14 +02:00 committed by Daniel Agar
parent 45bc2c5882
commit 6cab14fc5d
4 changed files with 1 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/Utility/InnovationLpf.hpp
View File

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

6
src/modules/ekf2/Utility/PreFlightChecker.cpp
View File

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

6
src/modules/ekf2/Utility/PreFlightChecker.hpp
View File

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

15
src/modules/ekf2/Utility/PreFlightCheckerTest.cpp
View File

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