From b15bf3243ed085409f77d211c7795ec9bc0ad8cc Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 30 Oct 2015 17:07:38 +1100
Subject: [PATCH] AP_NavEKF2: Remove potential for division by zero

---
 libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 22 ++++++++++++++++---
 .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp    | 14 ++++++++----
 2 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
index cc31369690..ca4fe93d8a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
@@ -479,7 +479,7 @@ void NavEKF2_core::FuseMagnetometer()
     // calculate the measurement innovation
     innovMag[obsIndex] = MagPred[obsIndex] - magDataDelayed.mag[obsIndex];
     // calculate the innovation test ratio
-    magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(frontend._magInnovGate) * varInnovMag[obsIndex]);
+    magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(max(frontend._magInnovGate,1)) * varInnovMag[obsIndex]);
     // check the last values from all components and set magnetometer health accordingly
     magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
     // Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
@@ -632,7 +632,23 @@ void NavEKF2_core::fuseCompass()
         }
         varInnov += H_MAG[rowIndex]*PH[rowIndex];
     }
-    float varInnovInv = 1.0f / varInnov;
+    float varInnovInv;
+    if (varInnov >= R_MAG) {
+        varInnovInv = 1.0f / varInnov;
+        // All three magnetometer components are used in this measurement, so we output health status on three axes
+        faultStatus.bad_xmag = false;
+        faultStatus.bad_ymag = false;
+        faultStatus.bad_zmag = false;
+    } else {
+        // the calculation is badly conditioned, so we cannot perform fusion on this step
+        // we reset the covariance matrix and try again next measurement
+        CovarianceInit();
+        // All three magnetometer components are used in this measurement, so we output health status on three axes
+        faultStatus.bad_xmag = true;
+        faultStatus.bad_ymag = true;
+        faultStatus.bad_zmag = true;
+        return;
+    }
     for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
         Kfusion[rowIndex] = 0.0f;
         for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
@@ -655,7 +671,7 @@ void NavEKF2_core::fuseCompass()
     }
 
     // calculate the innovation test ratio
-    yawTestRatio = sq(innovation) / (sq(frontend._magInnovGate) * varInnov);
+    yawTestRatio = sq(innovation) / (sq(max(frontend._magInnovGate,1)) * varInnov);
 
     // Declare the magnetometer unhealthy if the innovation test fails
     if (yawTestRatio > 1.0f) {
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index c8853326ac..81e0291836 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -423,7 +423,15 @@ void NavEKF2_core::StoreIMU_reset()
 void NavEKF2_core::RecallIMU()
 {
     imuDataDelayed = storedIMU[fifoIndexDelayed];
+    // make sure that the delta time used for the delta angles and velocities are is no less than 10% of dtIMUavg to prevent
+    // divide by zero problems when converting to rates or acceleration
+    float minDT = 0.1f*dtIMUavg;
+    imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT);
+    imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT);
 }
+
+// read the delta velocity and corresponding time interval from the IMU
+// return false if data is not available
 bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
     const AP_InertialSensor &ins = _ahrs->get_ins();
 
@@ -435,9 +443,6 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d
     return false;
 }
 
-
-
-
 /********************************************************
 *             Global Position Measurement               *
 ********************************************************/
@@ -632,7 +637,8 @@ bool NavEKF2_core::RecallGPS()
     }
 }
 
-
+// read the delta angle and corresponding time interval from the IMU
+// return false if data is not available
 bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
     const AP_InertialSensor &ins = _ahrs->get_ins();