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

Author: kamilritz

EKF/control.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/covariance.cpp

@@ -331,7 +331,7 @@ class Ekf : public EstimatorInterface
 	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 @@ class Ekf : public EstimatorInterface
 	// 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.h

@@ -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)
-				|| ((_time_last_imu - _time_last_hor_vel_fuse) <= _params.no_aid_timeout_max)
-				|| ((_time_last_imu - _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 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);
+			    && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
+				|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
+				|| isRecent(_time_last_delpos_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 &&
+			     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/ekf_helper.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/estimator_interface.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/gps_checks.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)
-					      && !_control_status.flags.opt_flow;
+	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
+						    && !_control_status.flags.opt_flow;
 
 	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
 }

EKF/terrain_estimator.cpp

@@ -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/CMakeLists.txt

@@ -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.cpp

@@ -47,6 +47,19 @@ class EkfWrapper
 	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/ekf_wrapper.h

@@ -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.cpp

@@ -57,6 +57,9 @@ class Gps: public Sensor
 	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/gps.h

@@ -78,6 +78,9 @@ class SensorSimulator
 	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(); }