AP_NavEKF2: Remove potential for division by zero

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) {

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();