fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: use explicit flags instead of bitmask position

Browse Source 
This prevents bitmask mismatch when a new flag is inserted
This commit is contained in:
bresch
2022-05-23 14:43:02 -04:00
committed by Daniel Agar
parent 62edcc7a57
commit c57c575cfe
2 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/optflow_fusion.cpp
+11 -7
View File
@@ -236,25 +236,29 @@ void Ekf::fuseOptFlow()
// run the innovation consistency check and record result
bool flow_fail = false;
bool all_innovation_checks_passed = true;
float test_ratio[2];
test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (test_ratio[obs_index] > 1.0f) {
flow_fail = true;
_innov_check_fail_status.value |= (1 << (obs_index + 10));
const bool innov_check_fail = (test_ratio[obs_index] > 1.0f);
if (innov_check_fail) {
all_innovation_checks_passed = false;
}
if (obs_index == 0) {
_innov_check_fail_status.flags.reject_optflow_X = innov_check_fail;
} else {
_innov_check_fail_status.value &= ~(1 << (obs_index + 10));
_innov_check_fail_status.flags.reject_optflow_Y = innov_check_fail;
}
}
// if either axis fails we abort the fusion
if (flow_fail) {
if (!all_innovation_checks_passed) {
return;
}