dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: use explicit flags instead of bitmask position 

This prevents bitmask mismatch when a new flag is inserted
This commit is contained in:
bresch 2022-05-23 17:44:53 +02:00 committed by Daniel Agar
parent d67cddae7d
commit 1f399e5b33
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
2 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -174,12 +174,20 @@ void Ekf::fuseMag(const Vector3f &mag)
for (uint8_t index = 0; index <= 2; index++) { 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)); _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; 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 { } else {
_innov_check_fail_status.value &= ~(1 << (index + 3)); _innov_check_fail_status.flags.reject_mag_z = innov_check_fail;
} }
} }

18
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -236,25 +236,29 @@ void Ekf::fuseOptFlow()
// run the innovation consistency check and record result // run the innovation consistency check and record result
bool flow_fail = false; bool all_innovation_checks_passed = true;
float test_ratio[2]; 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[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)); 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]); _optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (test_ratio[obs_index] > 1.0f) { const bool innov_check_fail = (test_ratio[obs_index] > 1.0f);
flow_fail = true;
_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 { } 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 either axis fails we abort the fusion
if (flow_fail) { if (!all_innovation_checks_passed) {
return; return;
} }