fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix magnetometer innovation test ratio reporting bug

Browse Source
This commit is contained in:
Paul Riseborough 2016-10-12 16:21:12 +11:00 committed by Lorenz Meier
parent 394bfdc935
commit 79b1d3018d
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/mag_fusion.cpp
View File

@ -155,6 +155,9 @@ void Ekf::fuseMag()
}
}
// we are no longer using heading fusion so set the reported test level to zero
_yaw_test_ratio = 0.0f;
// if any axis fails, abort the mag fusion
if (!healthy) {
return;
@ -608,6 +611,9 @@ void Ekf::fuseHeading()
// innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
// we are no longer using 3-axis fusion so set the reported test levels to zero
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_yaw = true;