diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c5fb499b69..cf822e5fa6 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -70,7 +70,8 @@ static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable static constexpr uint64_t EV_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec) static constexpr uint64_t GNSS_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec) static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec) -static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) +static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) +static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) // bad accelerometer detection and mitigation static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 8f1cb33035..f77847a329 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -107,6 +107,10 @@ void Ekf::controlMagFusion() _aid_src_mag.timestamp_sample = mag_sample.time_us; mag_observation.copyTo(_aid_src_mag.observation); mag_innov.copyTo(_aid_src_mag.innovation); + + } else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) { + // No data anymore. Stop until it comes back. + stopMagFusion(); } }