diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-07 07:25:20 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-07 07:25:20 +0200 |
| commit | 13b9cd0cec2d10d6252d0e8015d000f4fcc1517f (patch) | |
| tree | 8d559f658049e1f27b5910520a1b6e8b3cdabde3 /src/modules/ekf_att_pos_estimator | |
| parent | 95fc701376b63cc8209f9e8ae4dfab0f3435d825 (diff) | |
| download | px4-firmware-13b9cd0cec2d10d6252d0e8015d000f4fcc1517f.tar.gz px4-firmware-13b9cd0cec2d10d6252d0e8015d000f4fcc1517f.tar.bz2 px4-firmware-13b9cd0cec2d10d6252d0e8015d000f4fcc1517f.zip | |
Do not fuse height after innovation consistency check fail
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 10 |
1 files changed, 9 insertions, 1 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index ffdd29a5b..c619735c6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -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 { |