diff --git a/EKF/control.cpp b/EKF/control.cpp index 1f598c6bf4..1b7926120b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -601,6 +601,9 @@ void Ekf::controlGpsFusion() if (!_control_status.flags.in_air) { _time_last_on_ground_us = _time_last_imu; + + } else { + _time_last_in_air = _time_last_imu; } const bool recent_takeoff_nav_failure = _control_status.flags.in_air && diff --git a/EKF/ekf.h b/EKF/ekf.h index a8122ccebf..55b9de04b1 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -959,7 +959,8 @@ private: EKFGSF_yaw _yawEstimator; int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) - uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec) + uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec) + uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index add82b9dd5..2b5d73d1d5 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -646,13 +646,15 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f // if we are in air we don't want to fuse the measurement // we allow to use it when on the ground because the large innovation could be caused // by interference or a large initial gyro bias - if (_control_status.flags.in_air) { - return; - - } else { + if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6)) { // constrain the innovation to the maximum set by the gate + // we need to delay this forced fusion to avoid starting it + // immediately after touchdown, when the drone is still armed float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var)); _heading_innov = math::constrain(innovation, -gate_limit, gate_limit); + + } else { + return; } } else {