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

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