fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:03: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/mag_fusion.cpp
+11 -3
View File
@@ -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;
}
}