diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index f1f1f4188b..2a9d38b42e 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -441,6 +441,10 @@ bool NavEKF::healthy(void) const // extremely unhealthy. return false; } + // Give the filter 10 seconds to settle before use + if ((imuSampleTime_ms - ekfStartTime_ms) < 10000) { + return false; + } // all OK return true; @@ -684,9 +688,9 @@ void NavEKF::UpdateFilter() // check if on ground SetFlightAndFusionModes(); - // determine vehicle arm status + // determine vehicle arm status and don't allow filter to arm until it has been running for long enough to stabilise prevVehicleArmed = vehicleArmed; - vehicleArmed = getVehicleArmStatus(); + vehicleArmed = (getVehicleArmStatus() && (imuSampleTime_ms - ekfStartTime_ms) > 10000); // check to see if arm status has changed and reset states if it has if (vehicleArmed != prevVehicleArmed) { @@ -4353,6 +4357,7 @@ void NavEKF::ZeroVariables() flowMeaTime_ms = imuSampleTime_ms; prevFlowFusionTime_ms = imuSampleTime_ms; rngMeaTime_ms = imuSampleTime_ms; + ekfStartTime_ms = imuSampleTime_ms; gpsNoiseScaler = 1.0f; velTimeout = true; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 125e117d58..6f94e75459 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -541,6 +541,7 @@ private: uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy + uint32_t ekfStartTime_ms; // time the EKF was started (msec) Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator Vector3f lastAccel1; // acceleration from previous IMU1 sample used for trapezoidal integrator Vector3f lastAccel2; // acceleration from previous IMU2 sample used for trapezoidal integrator