diff --git a/EKF/control.cpp b/EKF/control.cpp index a6729eeb8d..d6c4a00809 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -156,6 +156,10 @@ void Ekf::controlFusionModes() // Reset states to the last GPS measurement resetPosition(); resetVelocity(); + + // Reset the timeout counters + _time_last_pos_fuse = _time_last_imu; + _time_last_vel_fuse = _time_last_imu; } } } @@ -241,6 +245,9 @@ void Ekf::controlFusionModes() // Reset vertical position and velocity states to the last measurement resetHeight(); + + // Reset the timout timer + _time_last_hgt_fuse = _time_last_imu; } // handle the case when we are relying on optical flow fusion and lose it diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b5e8df7487..dd14b991c6 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -488,6 +488,13 @@ bool Ekf::initialiseFilter(void) // initialise the terrain estimator initHagl(); + // reset the essential fusion timeout counters + _time_last_hgt_fuse = _time_last_imu; + _time_last_pos_fuse = _time_last_imu; + _time_last_vel_fuse = _time_last_imu; + _time_last_hagl_fuse = _time_last_imu; + _time_last_of_fuse = _time_last_imu; + return true; } }