Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: relocate perf monitor to count all three mag components

This commit is contained in:
priseborough 2017-01-26 08:02:31 +11:00 committed by Randy Mackay
parent a63eac8073
commit 2c36da2b21
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -260,15 +260,16 @@ void NavEKF3_core::SelectMagFusion()
FuseDeclination(0.34f); FuseDeclination(0.34f);
} }
// fuse the three magnetometer componenents sequentially // fuse the three magnetometer componenents sequentially
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
hal.util->perf_begin(_perf_test[0]); hal.util->perf_begin(_perf_test[0]);
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) {
FuseMagnetometer(); FuseMagnetometer();
hal.util->perf_end(_perf_test[0]);
// don't continue fusion if unhealthy // don't continue fusion if unhealthy
if (!magHealth) { if (!magHealth) {
hal.util->perf_end(_perf_test[0]);
break; break;
} }
} }
hal.util->perf_end(_perf_test[0]);
// zero the test ratio output from the inactive simple magnetometer yaw fusion // zero the test ratio output from the inactive simple magnetometer yaw fusion
yawTestRatio = 0.0f; yawTestRatio = 0.0f;
} }