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

Introduce similar checks fo all other health checks in the filter

This commit is contained in:
Lorenz Meier 2014-08-07 07:36:59 +02:00
parent 13b9cd0cec
commit 20c3a329c7
1 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED()
// apply a 5-sigma threshold // 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.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; 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.velHealth = true;
current_ekf_state.velFailTime = millis(); 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 else
{ {
@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED()
{ {
current_ekf_state.posHealth = true; current_ekf_state.posHealth = true;
current_ekf_state.posFailTime = millis(); 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 else
{ {