Browse Source

AP_NavEKF2: clean up mag field learned logic

master
priseborough 9 years ago
parent
commit
8793c75da0
  1. 6
      libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

6
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@ -269,12 +269,8 @@ void NavEKF2_core::SelectMagFusion() @@ -269,12 +269,8 @@ void NavEKF2_core::SelectMagFusion()
// If the final yaw reset has been performed and the state variances are sufficiently low
// record that the earth field has been learned.
bool earthMagFieldConverged = false;
if (!magFieldLearned && finalInflightMagInit) {
earthMagFieldConverged = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
}
if (magFieldLearned || earthMagFieldConverged) {
magFieldLearned = true;
magFieldLearned = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
}
// record the last learned field variances

Loading…
Cancel
Save