Robustify timestamp checks (#729)

* Robustify timestamp checks

* Remove lowerbound on sensor timestamp

* Add tests for fusion timeouts

* Inline and const time check functions

* Rename dead_reckoning time variable

EKF/control.cpp

@@ -86,10 +86,10 @@ void Ekf::controlFusionModes()
 
 	// check for intermittent data (before pop_first_older_than)
 	const baroSample &baro_init = _baro_buffer.get_newest();
-	_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+	_baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
 
 	const gpsSample &gps_init = _gps_buffer.get_newest();
-	_gps_hgt_intermittent = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
+	_gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL);
 
 	// check for arrival of new sensor data at the fusion time horizon
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
@@ -143,7 +143,7 @@ void Ekf::controlFusionModes()
 	// check if we should fuse flow data for terrain estimation
 	if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
 		// only fuse flow for terrain if range data hasn't been fused for 5 seconds
-		_flow_for_terrain_data_ready = (_time_last_imu - _time_last_hagl_fuse) > 5 * 1000 * 1000;
+		_flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
 		// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
 		_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
 	}
@@ -192,7 +192,7 @@ void Ekf::controlExternalVisionFusion()
 		if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
 
 			// check for a external vision measurement that has fallen behind the fusion time horizon
-			if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
+			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 				// turn on use of external vision measurements for position
 				if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
 					_control_status.flags.ev_pos = true;
@@ -212,7 +212,7 @@ void Ekf::controlExternalVisionFusion()
 		// external vision yaw aiding selection logic
 		if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
 			// don't start using EV data unless daa is arriving frequently
-			if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
+			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 				// reset the yaw angle to the value from the observation quaternion
 				// get the roll, pitch, yaw estimates from the quaternion states
 				Eulerf euler_init(_state.quat_nominal);
@@ -325,9 +325,10 @@ void Ekf::controlExternalVisionFusion()
 				ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 0), sq(0.01f));
 
 				// check if we have been deadreckoning too long
-				if ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) {
+				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
-					// don't reset velocity if we have another source of aiding constraining it
+					// only reset velocity if we have no another source of aiding constraining it
-					if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_hor_vel_fuse) > (uint64_t)1E6)) {
+					if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
+					    isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
 						resetVelocity();
 					}
 
@@ -366,9 +367,10 @@ void Ekf::controlExternalVisionFusion()
 			_ev_vel_innov = _state.vel - vel_aligned;
 
 			// check if we have been deadreckoning too long
-			if ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max) {
+			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
-				// don't reset velocity if we have another source of aiding constraining it
+				// only reset velocity if we have no another source of aiding constraining it
-				if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_hor_pos_fuse) > (uint64_t)1E6)) {
+				if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
+				    isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
 					resetVelocity();
 				}
 			}
@@ -387,8 +389,7 @@ void Ekf::controlExternalVisionFusion()
 		}
 
 	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
-		   && (_time_last_imu >= _time_last_ext_vision)
+		   && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
-		   && ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) {
 
 		// Turn off EV fusion mode if no data has been received
 		stopEvFusion();
@@ -466,7 +467,7 @@ void Ekf::controlOpticalFlowFusion()
 				stopFlowFusion();
 				_time_last_of_fuse = 0;
 
-			} else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) {
+			} else if (isTimedOut(_time_last_of_fuse, (uint64_t)_params.reset_timeout_max)) {
 				stopFlowFusion();
 
 			}
@@ -508,7 +509,7 @@ void Ekf::controlOpticalFlowFusion()
 		// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 		if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
 
-			bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
+			bool do_reset = isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
 
 			if (do_reset) {
 				resetVelocity();
@@ -539,7 +540,7 @@ void Ekf::controlOpticalFlowFusion()
 	if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
 		// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 		// but use a relaxed time criteria to enable it to coast through bad range finder data
-		if (_control_status.flags.opt_flow && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)10e6)) {
+		if (_control_status.flags.opt_flow && isTimedOut(_time_last_hagl_fuse, (uint64_t)10e6)) {
 			fuseOptFlow();
 			_last_known_posNE(0) = _state.pos(0);
 			_last_known_posNE(1) = _state.pos(1);
@@ -559,7 +560,7 @@ void Ekf::controlGpsFusion()
 				&& ISFINITE(_gps_sample_delayed.yaw)
 				&& _control_status.flags.tilt_align
 				&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
-				&& ((_time_last_imu - _time_last_gps) < (2 * GPS_MAX_INTERVAL))) {
+				&& isRecent(_time_last_gps, 2 * GPS_MAX_INTERVAL)) {
 
 			if (resetGpsAntYaw()) {
 				// flag the yaw as aligned
@@ -584,8 +585,8 @@ void Ekf::controlGpsFusion()
 
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
-		bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
+		bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
-		bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6);
+		bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 		if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
 			if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
 				// If the heading is not aligned, reset the yaw and magnetic field states
@@ -649,13 +650,13 @@ void Ekf::controlGpsFusion()
 		if (_control_status.flags.gps) {
 			// We are relying on aiding to constrain drift so after a specified time
 			// with no aiding we need to do something
-			bool do_reset = ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max)
+			bool do_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-					&& ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max)
+					&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
-					&& ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max)
+					&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-					&& ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
+					&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
 
 			// We haven't had an absolute position fix for a longer time so need to do something
-			do_reset = do_reset || ((_time_last_imu - _time_last_hor_pos_fuse) > (2 * _params.reset_timeout_max));
+			do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
 			if (do_reset) {
 				// use GPS velocity data to check and correct yaw angle if a FW vehicle
@@ -772,17 +773,17 @@ void Ekf::controlHeightSensorTimeouts()
 	// declare a bad vertical acceleration measurement and make the declaration persist
 	// for a minimum of 10 seconds
 	if (_bad_vert_accel_detected) {
-		_bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
+		_bad_vert_accel_detected = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
 
 	} else {
 		_bad_vert_accel_detected = bad_vert_accel;
 	}
 
 	// check if height is continuously failing because of accel errors
-	bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us);
+	bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
 
 	// check if height has been inertial deadreckoning for too long
-	bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6);
+	bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
 
 	if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
 
@@ -795,10 +796,10 @@ void Ekf::controlHeightSensorTimeouts()
 			const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
 
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const bool baro_data_available = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
 
 			// check for inertial sensing errors in the last 10 seconds
-			const bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION);
+			const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
 
 			// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
 			const bool reset_to_gps = !_gps_hgt_intermittent &&
@@ -833,7 +834,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 			// check the baro height source for consistency and freshness
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			const bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const bool baro_data_fresh = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
 			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
 			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate);
 
@@ -865,7 +866,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 			// check if baro data is available
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const bool baro_data_available = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
 
 			if (_rng_hgt_valid) {
 
@@ -890,11 +891,11 @@ void Ekf::controlHeightSensorTimeouts()
 		if (_control_status.flags.ev_hgt) {
 			// check if vision data is available
 			const extVisionSample &ev_init = _ext_vision_buffer.get_newest();
-			const bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
+			const bool ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL);
 
 			// check if baro data is available
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			const bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+			const bool baro_data_available = isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL);
 
 			if (ev_data_available) {
 				setControlEVHeight();
@@ -960,7 +961,7 @@ void Ekf::controlHeightFusion()
 
 			// Turn off ground effect compensation if it times out
 			if (_control_status.flags.gnd_effect) {
-				if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT)) {
+				if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 
 					_control_status.flags.gnd_effect = false;
 				}
@@ -1056,7 +1057,7 @@ void Ekf::controlHeightFusion()
 	if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
 
 		// don't start using EV data unless data is arriving frequently
-		if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
+		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 			_fuse_height = true;
 			setControlEVHeight();
 			resetHeight();
@@ -1090,7 +1091,7 @@ void Ekf::controlHeightFusion()
 		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
 	}
 
-	if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt
+	if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt
 	    && (!_range_data_ready || !_rng_hgt_valid)) {
 
 		// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
@@ -1218,8 +1219,8 @@ void Ekf::controlAirDataFusion()
 	// control activation and initialisation/reset of wind states required for airspeed fusion
 
 	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
-	const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
+	const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
-	const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
+	const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
 
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
@@ -1261,8 +1262,8 @@ void Ekf::controlBetaFusion()
 	// control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion
 
 	// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
-	const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6);
+	const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
-	const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6);
+	const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
 
 	if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
 		_control_status.flags.wind = false;
@@ -1271,7 +1272,7 @@ void Ekf::controlBetaFusion()
 	// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
 
 	// Sufficient time has lapsed sice the last fusion
-	bool beta_fusion_time_triggered = ((_time_last_imu - _time_last_beta_fuse) > _params.beta_avg_ft_us);
+	bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
 
 	if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
 		// If starting wind state estimation, reset the wind states and covariances before fusing any data
@@ -1325,14 +1326,14 @@ void Ekf::controlFakePosFusion()
 		_using_synthetic_position = true;
 
 		// Fuse synthetic position observations every 200msec
-		if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) {
+		if (isTimedOut(_time_last_fake_pos, (uint64_t)2e5) || _fuse_height) {
 
 			Vector3f fake_pos_obs_var;
 			Vector2f fake_pos_innov_gate;
 
 
 			// Reset position and velocity states if we re-commence this aiding method
-			if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) {
+			if (isTimedOut(_time_last_fake_pos, (uint64_t)4e5)) {
 				resetPosition();
 				resetVelocity();
 				_fuse_hpos_as_odom = false;

EKF/covariance.cpp

@@ -811,7 +811,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 		// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
 		// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
-		if (_time_last_imu - _time_acc_bias_check > (uint64_t)7e6) {
+		if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {
 
 			P.uncorrelateCovariance<3>(13);
 

EKF/ekf.h

@@ -331,7 +331,7 @@ private:
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
-	uint64_t _time_ins_deadreckon_start{0};	///< amount of time we have been doing inertial only deadreckoning (uSec)
+	uint64_t _time_last_aiding{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_hor_pos_fuse{0};	///< time the last fusion of horizontal position measurements was performed (uSec)
@@ -785,6 +785,16 @@ private:
 	// sensor measurement
 	float calculate_synthetic_mag_z_measurement(const Vector3f& mag_meas, const Vector3f& mag_earth_predicted);
 
+	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
+	{
+		return last_sensor_timestamp + timeout_period < _time_last_imu;
+	}
+
+	bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const
+	{
+		return sensor_timestamp + acceptance_interval > _time_last_imu;
+	}
+
 	void stopGpsFusion();
 
 	void stopGpsPosFusion();

EKF/ekf_helper.cpp

@@ -246,7 +246,7 @@ void Ekf::resetHeight()
 		// initialize vertical position with newest baro measurement
 		const baroSample &baro_newest = _baro_buffer.get_newest();
 
-		if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
+		if (isRecent(baro_newest.time_us, 2 * BARO_MAX_INTERVAL)) {
 			_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
 
 			// the state variance is the same as the observation
@@ -260,7 +260,7 @@ void Ekf::resetHeight()
 
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
-		if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
+		if (isRecent(gps_newest.time_us, 2 * GPS_MAX_INTERVAL)) {
 			_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
 
 			// the state variance is the same as the observation
@@ -296,7 +296,7 @@ void Ekf::resetHeight()
 	}
 
 	// reset the vertical velocity state
-	if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
+	if (_control_status.flags.gps && isRecent(gps_newest.time_us, 2 * GPS_MAX_INTERVAL)) {
 		// If we are using GPS, then use it to reset the vertical velocity
 		_state.vel(2) = gps_newest.vel(2);
 
@@ -1386,22 +1386,23 @@ bool Ekf::global_position_is_valid()
 void Ekf::update_deadreckoning_status()
 {
 	bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
-			    && (((_time_last_imu - _time_last_hor_pos_fuse) <= _params.no_aid_timeout_max)
+			    && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-				|| ((_time_last_imu - _time_last_hor_vel_fuse) <= _params.no_aid_timeout_max)
+				|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
-				|| ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max));
+				|| isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
-	bool optFlowAiding = _control_status.flags.opt_flow && ((_time_last_imu - _time_last_of_fuse) <= _params.no_aid_timeout_max);
+	bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
-	bool airDataAiding = _control_status.flags.wind && ((_time_last_imu - _time_last_arsp_fuse) <= _params.no_aid_timeout_max) && ((_time_last_imu - _time_last_beta_fuse) <= _params.no_aid_timeout_max);
+	bool airDataAiding = _control_status.flags.wind &&
+			     isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
+			     isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
 
 	_is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
 	_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
 
-	// record the time we start inertial dead reckoning
 	if (!_is_dead_reckoning) {
-		_time_ins_deadreckon_start = _time_last_imu - _params.no_aid_timeout_max;
+		_time_last_aiding = _time_last_imu - _params.no_aid_timeout_max;
 	}
 
 	// report if we have been deadreckoning for too long
-	_deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max);
+	_deadreckon_time_exceeded = isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max);
 }
 
 // calculate the inverse rotation matrix from a quaternion rotation
@@ -1748,7 +1749,6 @@ float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) c
 	return t;
 }
 
-
 void Ekf::stopGpsFusion()
 {
 	stopGpsPosFusion();

EKF/estimator_interface.cpp

@@ -196,7 +196,6 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 
 		gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000;
 		gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
-		gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
 
 		gps_sample_new.vel = gps.vel_ned;
 
@@ -267,7 +266,6 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 		baro_sample_new.time_us = 1000 * (_baro_timestamp_sum / _baro_sample_count);
 		baro_sample_new.time_us -= _params.baro_delay_ms * 1000;
 		baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
-		baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
 
 		_baro_buffer.push(baro_sample_new);
 

EKF/gps_checks.cpp

@@ -137,7 +137,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	const float filt_time_const = 10.0f;
-	const float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const);
+	const float dt = fminf(fmaxf(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f), filt_time_const);
 	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest
@@ -232,5 +232,5 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	}
 
 	// continuous period without fail of x seconds required to return a healthy status
-	return _time_last_imu - _last_gps_fail_us > (uint64_t)_min_gps_health_time_us;
+	return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us);
 }

EKF/terrain_estimator.cpp

@@ -57,7 +57,7 @@ bool Ekf::initHagl()
 		initialized = true;
 
 	} else if (_rng_hgt_valid
-		   && (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5
+		   && isRecent(latest_measurement.time_us, (uint64_t)2e5)
 		   && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
 		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
@@ -168,7 +168,7 @@ void Ekf::fuseHagl()
 
 		} else {
 			// If we have been rejecting range data for too long, reset to measurement
-			if ((_time_last_imu - _time_last_hagl_fuse) > (uint64_t)10E6) {
+			if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
 				_terrain_vpos = _state.pos(2) + meas_hagl;
 				_terrain_var = obs_variance;
 
@@ -290,12 +290,12 @@ bool Ekf::isTerrainEstimateValid() const
 void Ekf::updateTerrainValidity()
 {
 	// we have been fusing range finder measurements in the last 5 seconds
-	const bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6;
+	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);
 
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
-	const bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6)
+	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
-					      && !_control_status.flags.opt_flow;
+						    && !_control_status.flags.opt_flow;
 
 	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
 }

test/CMakeLists.txt

@@ -44,6 +44,7 @@ set(SRCS
 	test_AlphaFilter.cpp
 	test_EKF_fusionLogic.cpp
 	test_EKF_initialization.cpp
+	test_EKF_gps.cpp
 	test_EKF_externalVision.cpp
 	test_EKF_airspeed.cpp
    )

test/sensor_simulator/ekf_wrapper.cpp

@@ -10,6 +10,54 @@ EkfWrapper::~EkfWrapper()
 {
 }
 
+void EkfWrapper::setBaroHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_BARO;
+}
+
+bool EkfWrapper::isIntendingBaroHeightFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.baro_hgt;
+}
+
+void EkfWrapper::setGpsHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_GPS;
+}
+
+bool EkfWrapper::isIntendingGpsHeightFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.gps_hgt;
+}
+
+void EkfWrapper::setRangeHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_RANGE;
+}
+
+bool EkfWrapper::isIntendingRangeHeightFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.rng_hgt;
+}
+
+void EkfWrapper::setVisionHeight()
+{
+	_ekf_params->vdist_sensor_type = VDIST_SENSOR_EV;
+}
+
+bool EkfWrapper::isIntendingVisionHeightFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.ev_hgt;
+}
+
 void EkfWrapper::enableGpsFusion()
 {
 	_ekf_params->fusion_mode |= MASK_USE_GPS;

test/sensor_simulator/ekf_wrapper.h

@@ -47,6 +47,19 @@ public:
 	EkfWrapper(std::shared_ptr<Ekf> ekf);
 	~EkfWrapper();
 
+
+	void setBaroHeight();
+	bool isIntendingBaroHeightFusion() const;
+
+	void setGpsHeight();
+	bool isIntendingGpsHeightFusion() const;
+
+	void setRangeHeight();
+	bool isIntendingRangeHeightFusion() const;
+
+	void setVisionHeight();
+	bool isIntendingVisionHeightFusion() const;
+
 	void enableGpsFusion();
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;

test/sensor_simulator/gps.cpp

@@ -44,6 +44,21 @@ void Gps::setVelocity(const Vector3f& vel)
 	_gps_data.vel_ned = vel;
 }
 
+void Gps::setFixType(int n)
+{
+	_gps_data.fix_type = n;
+}
+
+void Gps::setNumberOfSatellites(int n)
+{
+	_gps_data.nsats = n;
+}
+
+void Gps::setPdop(float pdop)
+{
+	_gps_data.pdop = pdop;
+}
+
 void Gps::stepHeightByMeters(float hgt_change)
 {
 	_gps_data.alt += hgt_change * 1e3f;

test/sensor_simulator/gps.h

@@ -57,6 +57,9 @@ public:
 	void setLatitude(int32_t lat);
 	void setLongitude(int32_t lon);
 	void setVelocity(const Vector3f& vel);
+	void setFixType(int n);
+	void setNumberOfSatellites(int n);
+	void setPdop(float pdop);
 
 	gps_message getDefaultGpsData();
 

test/sensor_simulator/sensor_simulator.h

@@ -78,6 +78,9 @@ public:
 	void runSeconds(float duration_seconds);
 	void runMicroseconds(uint32_t duration);
 
+	void startBaro(){ _baro.start(); }
+	void stopBaro(){ _baro.stop(); }
+
 	void startGps(){ _gps.start(); }
 	void stopGps(){ _gps.stop(); }
 

test/test_EKF_fusionLogic.cpp

@@ -68,6 +68,22 @@ class EkfFusionLogicTest : public ::testing::Test {
 };
 
 
+TEST_F(EkfFusionLogicTest, doNoFusion)
+{
+	// GIVEN: a tilt and heading aligned filter
+	// WHEN: having no aiding source EKF should not have a valid position estimate
+
+	// TODO: for the first 5 second it still has some valid local position
+	//       that needs to change
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: Local and global position should not be valid
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+}
+
 TEST_F(EkfFusionLogicTest, doGpsFusion)
 {
 	// GIVEN: a tilt and heading aligned filter
@@ -153,7 +169,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
 	_sensor_simulator.startFlow();
 	_sensor_simulator.runSeconds(4);
 
-	// THEN: EKF should intend to fuse flow measurements
+	// THEN: EKF should not intend to fuse flow measurements
 	EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
 	// THEN: Local and global position should not be valid
 	EXPECT_FALSE(_ekf->local_position_is_valid());
@@ -171,7 +187,7 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
 	EXPECT_FALSE(_ekf->local_position_is_valid());
 	EXPECT_FALSE(_ekf->global_position_is_valid());
 
-	// WHEN: Flow data is  send and we enable flow fusion
+	// WHEN: Flow data is sent and we enable flow fusion
 	_sensor_simulator.startFlow();
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.runSeconds(10);
@@ -181,4 +197,172 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
 	// THEN: Local and global position should be valid
 	EXPECT_TRUE(_ekf->local_position_is_valid());
 	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: Stop sending flow data
+	_sensor_simulator.stopFlow();
+	_sensor_simulator.runSeconds(10);
+
+	// THEN: EKF should not intend to fuse flow measurements
+	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); // TODO: change to false
+	// THEN: Local and global position should not be valid
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+}
+
+TEST_F(EkfFusionLogicTest, doVisionPositionFusion)
+{
+	// WHEN: allow vision position to be fused and we send vision data
+	_ekf_wrapper.enableExternalVisionPositionFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: EKF should intend to fuse vision position estimate
+	//       and we have a valid local position estimate
+	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: stop sending vision data
+	_sensor_simulator.stopExternalVision();
+	_sensor_simulator.runSeconds(7);
+
+	// THEN: EKF should stop to intend to fuse vision position estimate
+	//       and EKF should not have a valid local position estimate anymore
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+}
+
+TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
+{
+	// WHEN: allow vision position to be fused and we send vision data
+	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: EKF should intend to fuse vision position estimate
+	//       and we have a valid local position estimate
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: stop sending vision data
+	_sensor_simulator.stopExternalVision();
+	_sensor_simulator.runSeconds(7);
+
+	// THEN: EKF should stop to intend to fuse vision position estimate
+	//       and EKF should not have a valid local position estimate anymore
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+}
+
+TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
+{
+	// WHEN: allow vision position to be fused and we send vision data
+	_ekf_wrapper.enableExternalVisionHeadingFusion();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runSeconds(4);
+
+	// THEN: EKF should intend to fuse vision heading estimates
+	//       and we should not have a valid local position estimate
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: stop sending vision data
+	_sensor_simulator.stopExternalVision();
+	_sensor_simulator.runSeconds(6);
+
+	// THEN: EKF should stop to intend to fuse vision position estimate
+	//       and EKF should not have a valid local position estimate anymore
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
+	// TODO: it is still intending to fuse ev_yaw. There should be some fallback to mag if possible
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+}
+
+TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
+{
+	// GIVEN: EKF that receives baro data
+
+	// THEN: EKF should intend to fuse baro by default
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+
+	// WHEN: stop sending baro data
+	_sensor_simulator.stopBaro();
+	_sensor_simulator.runSeconds(6);
+
+	// THEN: EKF should stop to intend to use baro hgt
+	// TODO: We have no fall back in balce
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); // TODO: Needs to change
+}
+
+
+TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
+{
+	// WHEN: commanding GPS height and sending GPS data
+	_ekf_wrapper.setGpsHeight();
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(11);
+
+	// THEN: EKF should intend to fuse gps height
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+
+	// WHEN: stop sending gps data
+	_sensor_simulator.stopGps();
+	_sensor_simulator.runSeconds(11); // TODO: We have to wait way too long
+
+	// THEN: EKF should stop to intend to use gps height
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+}
+
+
+TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
+{
+	// WHEN: commanding range height and sending range data
+	_ekf_wrapper.setRangeHeight();
+	_sensor_simulator.startRangeFinder();
+	_sensor_simulator.runSeconds(2);
+
+	// THEN: EKF should intend to fuse range height
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
+
+	// WHEN: stop sending range data
+	_sensor_simulator.stopRangeFinder();
+	_sensor_simulator.runSeconds(2);
+
+	// THEN: EKF should stop to intend to use range height
+	EXPECT_FALSE(_ekf_wrapper.isIntendingRangeHeightFusion());
+}
+
+TEST_F(EkfFusionLogicTest, doVisionHeightFusion)
+{
+	// WHEN: commanding vision height and sending vision data
+	_ekf_wrapper.setVisionHeight();
+	_sensor_simulator.startExternalVision();
+	_sensor_simulator.runSeconds(2);
+
+	// THEN: EKF should intend to fuse vision height
+	EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion());
+
+	// WHEN: stop sending vision data
+	_sensor_simulator.stopExternalVision();
+	_sensor_simulator.runSeconds(12);
+
+	// THEN: EKF should stop to intend to use vision height
+	// TODO: This is not happening
+	EXPECT_TRUE(_ekf_wrapper.isIntendingVisionHeightFusion()); // TODO: Needs to change
 }

test/test_EKF_gps.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test the gps fusion
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "sensor_simulator/sensor_simulator.h"
+#include "sensor_simulator/ekf_wrapper.h"
+
+
+class EkfGpsTest : public ::testing::Test {
+ public:
+
+	EkfGpsTest(): ::testing::Test(),
+	_ekf{std::make_shared<Ekf>()},
+	_sensor_simulator(_ekf),
+	_ekf_wrapper(_ekf) {};
+
+	std::shared_ptr<Ekf> _ekf;
+	SensorSimulator _sensor_simulator;
+	EkfWrapper _ekf_wrapper;
+
+	// Setup the Ekf with synthetic measurements
+	void SetUp() override
+	{
+		_ekf->init(0);
+		_sensor_simulator.runSeconds(2);
+		_ekf_wrapper.enableGpsFusion();
+		_sensor_simulator.startGps();
+		_sensor_simulator.runSeconds(11);
+	}
+
+	// Use this method to clean up any memory, network etc. after each test
+	void TearDown() override
+	{
+	}
+};
+
+TEST_F(EkfGpsTest, gpsTimeout)
+{
+	// GIVEN:EKF that fuses GPS
+
+	// WHEN: setting the PDOP to high
+	_sensor_simulator._gps.setNumberOfSatellites(3);
+
+	// THEN: EKF should stop fusing GPS
+	_sensor_simulator.runSeconds(20);
+
+	// TODO: this is not happening as expected
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+}