diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-07 07:36:59 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-07 07:36:59 +0200 |
commit | 20c3a329c72ca5950f7bb037473a812b4dd74b1b (patch) | |
tree | 051756ddbf97e3a82ffcfa14e8aa64fb0049bad8 /src | |
parent | 13b9cd0cec2d10d6252d0e8015d000f4fcc1517f (diff) | |
download | px4-firmware-20c3a329c72ca5950f7bb037473a812b4dd74b1b.tar.gz px4-firmware-20c3a329c72ca5950f7bb037473a812b4dd74b1b.tar.bz2 px4-firmware-20c3a329c72ca5950f7bb037473a812b4dd74b1b.zip |
Introduce similar checks fo all other health checks in the filter
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 21 |
1 files changed, 19 insertions, 2 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c619735c6..c7c7305b2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED() // apply a 5-sigma threshold current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { + if (current_ekf_state.velHealth || staticMode) { current_ekf_state.velHealth = true; current_ekf_state.velFailTime = millis(); + } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) { + // XXX check + current_ekf_state.velHealth = true; + ResetVelocity(); + ResetStoredStates(); + // do not fuse bad data + fuseVelData = false; } else { @@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED() { current_ekf_state.posHealth = true; current_ekf_state.posFailTime = millis(); + + if (current_ekf_state.posTimeout) { + ResetPosition(); + + // XXX cross-check the state reset + ResetStoredStates(); + + // do not fuse position data on this time + // step + fusePosData = false; + } } else { |