yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: selector improve fallback selection when the primary becomes unhealthy

Browse Source 
- if an ekf instance becomes unhealthy prefer switching to the next best healthy instance on a different IMU
This commit is contained in:
Daniel Agar
2021-01-11 09:38:26 -05:00
committed by GitHub
parent 967d35a6b6
commit 04f9ada500
1 changed files with 24 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -508,7 +508,11 @@ void EKF2Selector::Run()
bool lower_error_available = false; bool lower_error_available = false;
float alternative_error = 0.f; // looking for instances that have error lower than the current primary float alternative_error = 0.f; // looking for instances that have error lower than the current primary
uint8_t best_ekf_instance = _selected_instance; float best_test_ratio = FLT_MAX;
uint8_t best_ekf = _selected_instance;
uint8_t best_ekf_alternate = INVALID_INSTANCE;
uint8_t best_ekf_different_imu = INVALID_INSTANCE;
// loop through all available instances to find if an alternative is available // loop through all available instances to find if an alternative is available
for (int i = 0; i < _available_instances; i++) { for (int i = 0; i < _available_instances; i++) {
@@ -519,48 +523,42 @@ void EKF2Selector::Run()
// OR // OR
// selected instance has stopped updating // selected instance has stopped updating
if (_instance[i].healthy && (i != _selected_instance)) { if (_instance[i].healthy && (i != _selected_instance)) {
const float test_ratio = _instance[i].combined_test_ratio;
const float relative_error = _instance[i].relative_test_ratio; const float relative_error = _instance[i].relative_test_ratio;
if (relative_error < alternative_error) { if (relative_error < alternative_error) {
best_ekf_alternate = i;
alternative_error = relative_error; alternative_error = relative_error;
best_ekf_instance = i;
// relative error less than selected instance and has not been the selected instance for at least 10 seconds // relative error less than selected instance and has not been the selected instance for at least 10 seconds
if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) { if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) {
lower_error_available = true; lower_error_available = true;
} }
} }
}
}
if (!_instance[_selected_instance].healthy && (best_ekf_instance == _selected_instance)) {
// force selection to best healthy instance
uint8_t best_instance = _selected_instance;
float best_test_ratio = FLT_MAX;
for (int i = 0; i < _available_instances; i++) {
if (_instance[i].healthy) {
const float test_ratio = _instance[i].combined_test_ratio;
if ((test_ratio > 0) && (test_ratio < best_test_ratio)) { if ((test_ratio > 0) && (test_ratio < best_test_ratio)) {
best_instance = i; best_ekf = i;
best_test_ratio = test_ratio; best_test_ratio = test_ratio;
// also check next best available ekf using a different IMU
if (_instance[i].status.accel_device_id != _instance[_selected_instance].status.accel_device_id) {
best_ekf_different_imu = i;
}
} }
} }
} }
SelectInstance(best_instance); if (!_instance[_selected_instance].healthy) {
// prefer the best healthy instance using a different IMU
if (!SelectInstance(best_ekf_different_imu)) {
// otherwise switch to the healthy instance with best overall test ratio
SelectInstance(best_ekf);
}
} else if (best_ekf_instance != _selected_instance) { } else if (lower_error_available && (hrt_elapsed_time(&_last_instance_change) > 10_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
// switch immediately if the current selected is no longer healthy SelectInstance(best_ekf_alternate);
if ((lower_error_available && hrt_elapsed_time(&_last_instance_change) > 10_s)
|| !_instance[_selected_instance].healthy) {
SelectInstance(best_ekf_instance);
}
} }
// publish selector status at ~1 Hz or immediately on any change // publish selector status at ~1 Hz or immediately on any change