fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 04:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: selector handle NAN test ratios as uninitilaized

Browse Source 
- requires https://github.com/PX4/PX4-ECL/pull/1020
This commit is contained in:
Daniel Agar
2021-06-29 10:22:04 -04:00
parent f08a22934e
commit 982692f5ed
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2Selector.cpp
+4 -4
View File
@@ -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()
_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;
}