dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Do not fuse height after innovation consistency check fail

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -1070,10 +1070,18 @@ void AttPosEKF::FuseVelposNED()
// apply a 10-sigma threshold // apply a 10-sigma threshold
current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; 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.hgtHealth = true;
current_ekf_state.hgtFailTime = millis(); 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 else
{ {