AP_NavEKF2: prevent potential arithmetic error in declination fusion

2016-05-24 11:56:52 +10:00 · 2016-05-24 11:56:52 +10:00 · de15928921
commit de15928921
parent 722eb0d706
1 changed files with 3 additions and 0 deletions
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
@ -900,6 +900,9 @@ void NavEKF2_core::FuseDeclination()
    float t10 = t9-t14;
    float t15 = t23*t10;
    float t11 = R_DECL+t8-t15; // innovation variance
+    if (t11 < R_DECL) {
+        return;
+    }
    float t12 = 1.0f/t11;

    float H_MAG[24];