gps_hgt: add and improve unit tests

src/modules/ekf2/test/CMakeLists.txt

@@ -40,7 +40,7 @@ px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
-px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -40,6 +40,7 @@
 #include "EKF/ekf.h"
 #include "sensor_simulator/sensor_simulator.h"
 #include "sensor_simulator/ekf_wrapper.h"
+#include "test_helper/reset_logging_checker.h"
 
 
 class EkfFusionLogicTest : public ::testing::Test
@@ -344,7 +345,9 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
 
 TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
 {
-	// GIVEN: EKF that receives baro data
+	// GIVEN: EKF that receives baro and GPS data
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(11);
 
 	// THEN: EKF should intend to fuse baro by default
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -353,11 +356,49 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
 	_sensor_simulator.stopBaro();
 	_sensor_simulator.runSeconds(6);
 
-	// THEN: EKF should stop to intend to use baro hgt
+	// THEN: EKF should stop to intend to use baro hgt and use GPS as a fallback
-	// TODO: We have no fall back in balce
+	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
-	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); // TODO: Needs to change
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
 }
 
+TEST_F(EkfFusionLogicTest, doBaroHeightFusionTimeout)
+{
+	// GIVEN: EKF that receives baro data
+
+	// THEN: EKF should intend to fuse baro by default
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+
+	// WHEN: the baro data jumps by a lot
+	ResetLoggingChecker reset_logging_checker(_ekf);
+	reset_logging_checker.capturePreResetState();
+
+	_sensor_simulator._baro.setData(100.f);
+	_sensor_simulator.runSeconds(6);
+
+	reset_logging_checker.capturePostResetState();
+
+	// THEN: EKF should reset to the measurement
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1));
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
+
+	// BUT WHEN: GPS height data is also available
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(11);
+	reset_logging_checker.capturePostResetState();
+	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
+
+	// AND: the baro data jumps by a lot
+	_sensor_simulator._baro.setData(800.f);
+	_sensor_simulator.runSeconds(6);
+	reset_logging_checker.capturePostResetState();
+
+	// THEN: EKF should fallback to GPS height
+	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(3));
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
+}
 
 TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
 {
@@ -371,13 +412,13 @@ TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
 
 	// WHEN: stop sending gps data
 	_sensor_simulator.stopGps();
-	_sensor_simulator.runSeconds(11); // TODO: We have to wait way too long
+	_sensor_simulator.runSeconds(5);
 
 	// THEN: EKF should stop to intend to use gps height
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
 }
 
-
 TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
 {
 	// WHEN: commanding range height and sending range data