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