diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp index 70798b09ae..0eed5cb7f6 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp @@ -107,25 +107,20 @@ public: TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion) { // WHEN: simulate being 5m above ground - // By default, both rng and flow aiding are active const float simulated_distance_to_ground = 1.f; - _sensor_simulator._rng.setData(simulated_distance_to_ground, 100); - _sensor_simulator._rng.setLimits(0.1f, 9.f); - _sensor_simulator.startRangeFinder(); - _ekf->set_in_air_status(true); - _sensor_simulator.runSeconds(1.5f); + runFlowAndRngScenario(simulated_distance_to_ground, simulated_distance_to_ground); // THEN: By default, both rng and flow aiding are active EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion()); - EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion()); const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); - EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground); + EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 1e-4); // WHEN: rng fusion is disabled _ekf_wrapper.disableTerrainRngFusion(); - _sensor_simulator.runSeconds(0.2); + _sensor_simulator.runSeconds(5.); - // THEN: only rng fusion should be disabled + // THEN: rng fusion should be disabled and flow fusion should take over EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainRngFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion());