AP_NavEKF3: Make bad IMU status more persistent

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -689,9 +689,15 @@ void NavEKF3_core::FuseVelPosNED()
             const ftype velDErr = stateStruct.velocity.z - velPosObs[2];
             // check if they are the same sign and both more than 3-sigma out of bounds
             if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) {
+                badIMUdata_ms = imuSampleTime_ms;
+            } else {
+                goodIMUdata_ms = imuSampleTime_ms;
+            }
+            if (imuSampleTime_ms - badIMUdata_ms < 10000) {
                 badIMUdata = true;
             } else {
                 badIMUdata = false;
+
             }
         }
 
@@ -1217,7 +1223,8 @@ void NavEKF3_core::selectHeightForFusion()
     // If we haven't fused height data for a while, then declare the height data as being timed out
     // set timeout period based on whether we have vertical GPS velocity available to constrain drift
     hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
-    if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
+    if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms ||
+        (badIMUdata && (imuSampleTime_ms - goodIMUdata_ms < BAD_IMU_DATA_TIMEOUT_MS))) {
         hgtTimeout = true;
     } else {
         hgtTimeout = false;

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -233,6 +233,8 @@ void NavEKF3_core::InitialiseVariables()
     hgtTimeout = true;
     tasTimeout = true;
     badIMUdata = false;
+    badIMUdata_ms = 0;
+    goodIMUdata_ms = 0;
     vertVelVarClipCounter = 0;
     finalInflightYawInit = false;
     dtIMUavg = ins.get_loop_delta_t();
@@ -1135,7 +1137,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
         ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);
         daxVar = dayVar = dazVar = sq(dt*_gyrNoise);
     }
-    ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, 10.0f);
+    ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, BAD_IMU_DATA_ACC_P_NSE);
     dvxVar = dvyVar = dvzVar = sq(dt*_accNoise);
 
     if (!inhibitDelVelBiasStates) {

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -96,8 +96,12 @@
 #define EK3_POSXY_STATE_LIMIT 1.0e6
 #endif
 
+// IMU acceleration process noise in m/s/s used when bad vibration affected IMU accel is detected
 #define BAD_IMU_DATA_ACC_P_NSE 5.0f
 
+// Number of milliseconds of bad IMU data before a reset to vertical position and velocity height sources is performed
+#define BAD_IMU_DATA_TIMEOUT_MS 1000
+
 class NavEKF3_core : public NavEKF_core_common
 {
 public:
@@ -960,6 +964,8 @@ private:
     bool magTimeout;                // boolean true if magnetometer measurements have failed for too long and have timed out
     bool tasTimeout;                // boolean true if true airspeed measurements have failed for too long and have timed out
     bool badIMUdata;                // boolean true if the bad IMU data is detected
+    uint32_t badIMUdata_ms;         // time stamp bad IMU data was last detected
+    uint32_t goodIMUdata_ms;        // time stamp good IMU data was last detected
     uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
 
     ftype gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts