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 handle uninitalized test ratios

Browse Source 
- choose the best instance regardless of tilt or yaw alignment
This commit is contained in:
Daniel Agar
2021-06-03 10:57:48 -04:00
parent ed62ae0e4f
commit 578c003c13
1 changed files with 14 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -282,18 +282,23 @@ bool EKF2Selector::UpdateErrorScores()
primary_updated = true;
}
const bool tilt_align = status.control_mode_flags & (1 << estimator_status_s::CS_TILT_ALIGN);
const bool yaw_align = status.control_mode_flags & (1 << estimator_status_s::CS_YAW_ALIGN);
float combined_test_ratio = 0;
if (tilt_align && yaw_align) {
combined_test_ratio = fmaxf(0.f, 0.5f * (status.vel_test_ratio + status.pos_test_ratio));
combined_test_ratio = fmaxf(combined_test_ratio, status.hgt_test_ratio);
// test ratios are invalid when 0, >= 1 is a failure
if (status.vel_test_ratio <= 0.f) {
status.vel_test_ratio = 1.f;
}
if (status.pos_test_ratio <= 0.f) {
status.pos_test_ratio = 1.f;
}
if (status.hgt_test_ratio <= 0.f) {
status.hgt_test_ratio = 1.f;
}
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 = tilt_align && yaw_align && (status.filter_fault_flags == 0);
_instance[i].healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f);
_instance[i].filter_fault = (status.filter_fault_flags != 0);
_instance[i].timeout = false;