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

ekf2: selector use hysteresis for healthy flag

Browse Source 
- require that an instance is healthy continuously for >= 1s before
allowing it to be used
This commit is contained in:
Daniel Agar
2021-12-13 10:04:33 -05:00
parent eb69f15d3a
commit 3fba7288af
3 changed files with 21 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2Selector.cpp
+14 -11
View File
@@ -82,7 +82,7 @@ void EKF2Selector::PrintInstanceChange(const uint8_t old_instance, uint8_t new_i
} else if (_accel_fault_detected) {
old_reason = " (accel fault)";
} else if (!_instance[_selected_instance].healthy && (_instance[_selected_instance].healthy_count > 0)) {
} else if (!_instance[_selected_instance].healthy.get_state() && (_instance[_selected_instance].healthy_count > 0)) {
// skipped if previous instance was never healthy in the first place (eg initialization)
old_reason = " (unhealthy)";
}
@@ -255,7 +255,7 @@ bool EKF2Selector::UpdateErrorScores()
// calculate individual error scores
for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) {
const bool prev_healthy = _instance[i].healthy;
const bool prev_healthy = _instance[i].healthy.get_state();
estimator_status_s status;
@@ -293,7 +293,10 @@ 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);
const bool healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f);
_instance[i].healthy.set_state_and_update(healthy, status.timestamp);
_instance[i].warning = (combined_test_ratio >= 1.f);
_instance[i].filter_fault = (status.filter_fault_flags != 0);
_instance[i].timeout = false;
@@ -307,21 +310,21 @@ bool EKF2Selector::UpdateErrorScores()
}
} else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout)) {
_instance[i].healthy = false;
_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
_instance[i].timeout = true;
}
// if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay
if (_gyro_fault_detected && (faulty_gyro_id != 0) && (_instance[i].gyro_device_id == faulty_gyro_id)) {
_instance[i].healthy = false;
_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
}
// if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay
if (_accel_fault_detected && (faulty_accel_id != 0) && (_instance[i].accel_device_id == faulty_accel_id)) {
_instance[i].healthy = false;
_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
}
if (prev_healthy != _instance[i].healthy) {
if (prev_healthy != _instance[i].healthy.get_state()) {
updated = true;
_selector_status_publish = true;
@@ -705,7 +708,7 @@ void EKF2Selector::Run()
// (has relative error less than selected instance and has not been the selected instance for at least 10 seconds
// OR
// selected instance has stopped updating
if (_instance[i].healthy && (i != _selected_instance)) {
if (_instance[i].healthy.get_state() && (i != _selected_instance)) {
const float test_ratio = _instance[i].combined_test_ratio;
const float relative_error = _instance[i].relative_test_ratio;
@@ -731,7 +734,7 @@ void EKF2Selector::Run()
}
}
if (!_instance[_selected_instance].healthy) {
if (!_instance[_selected_instance].healthy.get_state()) {
// 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
@@ -798,7 +801,7 @@ void EKF2Selector::PublishEstimatorSelectorStatus()
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
selector_status.healthy[i] = _instance[i].healthy;
selector_status.healthy[i] = _instance[i].healthy.get_state();
}
for (int i = 0; i < IMU_STATUS_SIZE; i++) {
@@ -824,7 +827,7 @@ void EKF2Selector::PrintStatus()
PX4_INFO("%" PRIu8 ": ACC: %" PRIu32 ", GYRO: %" PRIu32 ", MAG: %" PRIu32 ", %s, test ratio: %.7f (%.5f) %s",
inst.instance, inst.accel_device_id, inst.gyro_device_id, inst.mag_device_id,
inst.healthy ? "healthy" : "unhealthy",
inst.healthy.get_state() ? "healthy" : "unhealthy",
(double)inst.combined_test_ratio, (double)inst.relative_test_ratio,
(_selected_instance == i) ? "*" : "");
}