fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 18:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Do not fuse height after innovation consistency check fail

Browse Source
This commit is contained in:
Lorenz Meier
2014-08-07 07:25:20 +02:00
parent 95fc701376
commit 13b9cd0cec
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+9 -1
View File
@@ -1070,10 +1070,18 @@ void AttPosEKF::FuseVelposNED()
// apply a 10-sigma threshold
current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
{
current_ekf_state.hgtHealth = true;
current_ekf_state.hgtFailTime = millis();
// if we just reset from a timeout, do not fuse
// the height data, but reset height and stored states
if (current_ekf_state.hgtTimeout) {
ResetHeight();
ResetStoredStates();
fuseHgtData = false;
}
}
else
{