diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 0accaa268b..bc40fca749 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -329,16 +329,17 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : { AP_Param::setup_object_defaults(this, var_info); // Tuning parameters - _gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration + _gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration _gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - _gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration + _gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration _msecHgtDelay = 60; // Height measurement delay (msec) _msecMagDelay = 40; // Magnetometer measurement delay (msec) _msecTasDelay = 240; // Airspeed measurement delay (msec) _gpsRetryTimeUseTAS = 20000; // GPS retry time with airspeed measurements (msec) - _gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec) + _gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec) _hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec) _hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec) + _magFailTimeLimit = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) _magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate _gyroBiasNoiseScaler = 3.0f; // scale factor applied to gyro bias state process variance when on ground _msecGpsAvg = 200; // average number of msec between GPS measurements @@ -722,6 +723,18 @@ void NavEKF::SelectMagFusion() // check for and read new magnetometer measurements readMagData(); + // If we are using the compass and the magnetometer has been unhealthy for too long we declare it failed + if (magHealth) { + lastHealthyMagTime_ms = hal.scheduler->millis(); + } else { + if ((hal.scheduler->millis() - lastHealthyMagTime_ms) > uint32_t(_magFailTimeLimit) && use_compass()) { + magTimeout = true; + } else { + magTimeout = false; + } + } + + // determine if conditions are right to start a new fusion cycle bool dataReady = statesInitialised && use_compass() && newDataMag; if (dataReady) @@ -2149,12 +2162,19 @@ void NavEKF::FuseMagnetometer() innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex]; // calculate the innovation test ratio magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(_magInnovGate) * varInnovMag[obsIndex]); - // test the ratio before fusing data - if (magTestRatio[obsIndex] < 1.0f) + // check the last values from all componenets 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 + // In this case we might as well try using the magnetometer, but with a reduced weighting + if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !_ahrs->get_fly_forward() && magTimeout)) { // correct the state vector for (uint8_t j= 0; j<=indexLimit; j++) { + // If we are forced to use a bad compass, we reduce the weighting by a factor of 4 + if (!magHealth) { + Kfusion[j] *= 0.25f; + } states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } // normalise the quaternion states @@ -2724,8 +2744,8 @@ void NavEKF::OnGroundCheck() } else { onGround = true; } - // force a yaw alignment if exiting onGround without a compass - if (!onGround && prevOnGround && !use_compass()) { + // force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle + if (!onGround && prevOnGround && (!use_compass() || (magTimeout && _ahrs->get_fly_forward()))) { alignYawGPS(); } // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 828fbf1695..f14abdf10e 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -320,6 +320,7 @@ private: AP_Int16 _gpsRetryTimeNoTAS; // GPS retry time following innovation consistency fail if no TAS measurements are used (msec) AP_Int16 _hgtRetryTimeMode0; // height measurement retry time following innovation consistency fail if GPS fusion mode is = 0 (msec) AP_Int16 _hgtRetryTimeMode12; // height measurement retry time following innovation consistency fail if GPS fusion mode is > 0 (msec) + AP_Int16 _magFailTimeLimit; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) float _gyroBiasNoiseScaler; // scale factor applied to gyro bias state process variance when on ground float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate uint16_t _msecGpsAvg; // average number of msec between GPS measurements @@ -330,12 +331,14 @@ private: // Variables uint8_t skipCounter; // counter used to skip position and height corrections to achieve _skipRatio bool statesInitialised; // boolean true when filter states have been initialised - bool velHealth; // boolean true if velocity measurements have failed innovation consistency check - bool posHealth; // boolean true if position measurements have failed innovation consistency check - bool hgtHealth; // boolean true if height measurements have failed innovation consistency check + bool velHealth; // boolean true if velocity measurements have passed innovation consistency check + bool posHealth; // boolean true if position measurements have passed innovation consistency check + bool hgtHealth; // boolean true if height measurements have passed innovation consistency check + bool magHealth; // boolean true if magnetometer has passed innovation consistency check bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out - bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out + bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out + bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out Vector31 Kfusion; // Kalman gain vector Matrix22 KH; // intermediate result used for covariance updates @@ -422,6 +425,7 @@ private: uint32_t lastStateStoreTime_ms; // time of last state vector storage uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived 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 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