fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 07:23:33 +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 08:51:45 +02:00
committed by Lorenz Meier
parent 394bfdc935
commit 79b1d3018d
1 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+6
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 any axis fails, abort the mag fusion
if (!healthy) { if (!healthy) {
return; return;
@@ -608,6 +611,9 @@ void Ekf::fuseHeading()
// innovation test ratio // innovation test ratio
_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var); _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 // set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) { if (_yaw_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_yaw = true; _innov_check_fail_status.flags.reject_yaw = true;