From e34de53e2e85770560e46bf931bbbf5568b82f28 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 4 Aug 2022 15:12:30 +0200 Subject: [PATCH] ekf2_test: let the GPS start before setting the new origin fix test by reducing the distance to the new origin: the maximum size of the local position origin is a cube of 1e6m. If the origin is moved further than this, the state is clipped to that maximum value --- src/modules/ekf2/test/test_EKF_basics.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 7b3cf096eb..5167eb9de7 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -223,7 +223,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) _sensor_simulator.setGpsLatitude(_latitude_new); _sensor_simulator.setGpsLongitude(_longitude_new); _sensor_simulator.setGpsAltitude(_altitude_new); - _sensor_simulator.runSeconds(2); + _sensor_simulator.runSeconds(5); _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); @@ -231,8 +231,9 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_NEAR(_altitude, _altitude_new, 0.01f); - _latitude_new = -15.0000005; - _longitude_new = -115.0000005; + // Note: we cannot reset too far since the local position is limited to 1e6m + _latitude_new = 14.0000005; + _longitude_new = 109.0000005; _altitude_new = 1500.0; _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); @@ -242,6 +243,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + _sensor_simulator.runSeconds(1); + float hpos = 0.f; float vpos = 0.f; float hvel = 0.f; @@ -276,6 +279,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + _sensor_simulator.runSeconds(1); + float hpos = 0.f; float vpos = 0.f; float hvel = 0.f;