Browse Source

ekf2: selector handle NAN test ratios as uninitilaized

- requires https://github.com/PX4/PX4-ECL/pull/1020
release/1.12
Daniel Agar 4 years ago
parent
commit
982692f5ed
  1. 8
      src/modules/ekf2/EKF2Selector.cpp

8
src/modules/ekf2/EKF2Selector.cpp

@ -283,15 +283,15 @@ bool EKF2Selector::UpdateErrorScores() @@ -283,15 +283,15 @@ bool EKF2Selector::UpdateErrorScores()
}
// test ratios are invalid when 0, >= 1 is a failure
if (status.vel_test_ratio <= 0.f) {
if (!PX4_ISFINITE(status.vel_test_ratio) || (status.vel_test_ratio <= 0.f)) {
status.vel_test_ratio = 1.f;
}
if (status.pos_test_ratio <= 0.f) {
if (!PX4_ISFINITE(status.pos_test_ratio) || (status.pos_test_ratio <= 0.f)) {
status.pos_test_ratio = 1.f;
}
if (status.hgt_test_ratio <= 0.f) {
if (!PX4_ISFINITE(status.hgt_test_ratio) || (status.hgt_test_ratio <= 0.f)) {
status.hgt_test_ratio = 1.f;
}
@ -306,7 +306,7 @@ bool EKF2Selector::UpdateErrorScores() @@ -306,7 +306,7 @@ bool EKF2Selector::UpdateErrorScores()
_instance[i].relative_test_ratio = 0;
}
} else if (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout) {
} else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) {
_instance[i].healthy = false;
_instance[i].timeout = true;
}

Loading…
Cancel
Save