EKF: prevent race condition between global position validity and eph reporting

EKF/control.cpp

@@ -1356,9 +1356,12 @@ void Ekf::controlVelPosFusion()
 	if (!_control_status.flags.gps &&
 			!_control_status.flags.opt_flow &&
 			!_control_status.flags.ev_pos &&
-			!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) &&
+			!(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) {
-			((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height))
+		// We now need to use a synthetic positon observation to prevent unconstrained drift of the INS states.
-	{
+		_using_synthetic_position = true;
+
+		// Fuse synthetic position observations every 200msec
+		if ((_time_last_imu - _time_last_fake_gps > (uint64_t)2e5) || _fuse_height) {
 			// Reset position and velocity states if we re-commence this aiding method
 			if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
 				resetPosition();
@@ -1390,6 +1393,9 @@ void Ekf::controlVelPosFusion()
 			_posInnovGateNE = 100.0f;
 
 		}
+	} else {
+		_using_synthetic_position = false;
+	}
 
 	// Fuse available NED velocity and position data into the main filter
 	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {

EKF/ekf.h

@@ -292,6 +292,7 @@ private:
 
 	uint64_t _time_last_fake_gps{0};	///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)
 	uint64_t _time_ins_deadreckon_start{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
+	bool _using_synthetic_position{false};	///< true if we are using a synthetic position to constrain drift
 
 	uint64_t _time_last_pos_fuse{0};	///< time the last fusion of horizontal position measurements was performed (uSec)
 	uint64_t _time_last_delpos_fuse{0};	///< time the last fusion of incremental horizontal position measurements was performed (uSec)

EKF/ekf_helper.cpp

@@ -1291,8 +1291,9 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first,
 
 bool Ekf::global_position_is_valid()
 {
-	// return true if we are not doing unconstrained free inertial navigation and the origin is set
+	// return true if the origin is set we are not doing unconstrained free inertial navigation
-	return (_NED_origin_initialised && !_deadreckon_time_exceeded);
+	// and have not started using synthetic position observations to constrain drift
+	return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
 }
 
 // return true if we are totally reliant on inertial dead-reckoning for position