EKF: ensure fusion timeout counters are reset when required

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

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;
 	}
 }