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

ekf2: selector treat combined test ratio > 1 as a warning 

* ekf2: selector treat combined test ratio > 1 as a warning
* ekf2: wait for 1s of constant warning to trigger an instance switch

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar 2021-12-01 20:22:34 -05:00 committed by GitHub
parent 89892d0d17
commit b7efd4f947
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/ekf2/EKF2Selector.cpp
View File

@ -293,10 +293,15 @@ bool EKF2Selector::UpdateErrorScores()
float combined_test_ratio = fmaxf(0.5f * (status.vel_test_ratio + status.pos_test_ratio), status.hgt_test_ratio);
_instance[i].combined_test_ratio = combined_test_ratio;
_instance[i].healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f) && (combined_test_ratio < 1.f);
_instance[i].healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f);
_instance[i].warning = (combined_test_ratio >= 1.f);
_instance[i].filter_fault = (status.filter_fault_flags != 0);
_instance[i].timeout = false;
if (!_instance[i].warning) {
_instance[i].time_last_no_warning = status.timestamp_sample;
}
if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) {
_instance[i].relative_test_ratio = 0;
}
@ -733,7 +738,11 @@ void EKF2Selector::Run()
SelectInstance(best_ekf);
}
} else if (lower_error_available && (hrt_elapsed_time(&_last_instance_change) > 10_s)) {
} else if (lower_error_available
&& ((hrt_elapsed_time(&_last_instance_change) > 10_s)
|| (_instance[_selected_instance].warning
&& (hrt_elapsed_time(&_instance[_selected_instance].time_last_no_warning) > 1_s)))) {
// if this instance has a significantly lower relative error to the active primary, we consider it as a
// better instance and would like to switch to it even if the current primary is healthy
SelectInstance(best_ekf_alternate);

2
src/modules/ekf2/EKF2Selector.hpp
View File

@ -125,11 +125,13 @@ private:
uint32_t mag_device_id{0};
hrt_abstime time_last_selected{0};
hrt_abstime time_last_no_warning{0};
float combined_test_ratio{NAN};
float relative_test_ratio{NAN};
bool healthy{false};
bool warning{false};
bool filter_fault{false};
bool timeout{false};