diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 73c890bec4..8f01a80f51 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -174,12 +174,20 @@ void Ekf::fuseMag(const Vector3f &mag) for (uint8_t index = 0; index <= 2; index++) { _mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index)); - if (_mag_test_ratio(index) > 1.0f) { + const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f); + + if (innov_check_fail) { all_innovation_checks_passed = false; - _innov_check_fail_status.value |= (1 << (index + 3)); + } + + if (index == 0) { + _innov_check_fail_status.flags.reject_mag_x = innov_check_fail; + + } else if (index == 1) { + _innov_check_fail_status.flags.reject_mag_y = innov_check_fail; } else { - _innov_check_fail_status.value &= ~(1 << (index + 3)); + _innov_check_fail_status.flags.reject_mag_z = innov_check_fail; } } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index d158f90523..69470e0778 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -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); - } else { - _innov_check_fail_status.value &= ~(1 << (obs_index + 10)); + 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.flags.reject_optflow_Y = innov_check_fail; } } // if either axis fails we abort the fusion - if (flow_fail) { + if (!all_innovation_checks_passed) { return; }