Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

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

Browse Source 
* 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
2 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2Selector.cpp
+11 -2
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); 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].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].filter_fault = (status.filter_fault_flags != 0);
_instance[i].timeout = false; _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)) { if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) {
_instance[i].relative_test_ratio = 0; _instance[i].relative_test_ratio = 0;
} }
@@ -733,7 +738,11 @@ void EKF2Selector::Run()
SelectInstance(best_ekf); 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 // 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 // better instance and would like to switch to it even if the current primary is healthy
SelectInstance(best_ekf_alternate); SelectInstance(best_ekf_alternate);
src/modules/ekf2/EKF2Selector.hpp
+2
View File
@@ -125,11 +125,13 @@ private:
uint32_t mag_device_id{0}; uint32_t mag_device_id{0};
hrt_abstime time_last_selected{0}; hrt_abstime time_last_selected{0};
hrt_abstime time_last_no_warning{0};
float combined_test_ratio{NAN}; float combined_test_ratio{NAN};
float relative_test_ratio{NAN}; float relative_test_ratio{NAN};
bool healthy{false}; bool healthy{false};
bool warning{false};
bool filter_fault{false}; bool filter_fault{false};
bool timeout{false}; bool timeout{false};