From 0a140ec59a621e5dd2b92037851141ab332210d4 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 22 Oct 2021 10:55:30 +0200 Subject: [PATCH] ekf2_test: add GPS height to baro fallback --- src/modules/ekf2/test/test_EKF_gps.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 4c23166ad3..a2821d94c8 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -139,3 +139,25 @@ TEST_F(EkfGpsTest, resetToGpsPosition) EXPECT_TRUE(isEqual(estimated_position, previous_position + simulated_position_change, 1e-2f)); } + +TEST_F(EkfGpsTest, gpsHgtToBaroFallback) +{ + // GIVEN: EKF that fuses GPS and flow, and in GPS height mode + _sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest()); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + + _ekf_wrapper.setGpsHeight(); + + _sensor_simulator.runSeconds(1); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); + + // WHEN: stopping GPS fusion + _sensor_simulator.stopGps(); + _sensor_simulator.runSeconds(11); + + // THEN: the height source should automatically change to baro + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); +}