fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2_test: let the GPS start before setting the new origin

Browse Source 
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
This commit is contained in:
bresch
2022-08-04 16:50:31 +02:00
committed by Mathieu Bresciani
parent 55f395a7e9
commit e34de53e2e
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_basics.cpp
+8 -3
View File
@@ -223,7 +223,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
_sensor_simulator.setGpsLatitude(_latitude_new); _sensor_simulator.setGpsLatitude(_latitude_new);
_sensor_simulator.setGpsLongitude(_longitude_new); _sensor_simulator.setGpsLongitude(_longitude_new);
_sensor_simulator.setGpsAltitude(_altitude_new); _sensor_simulator.setGpsAltitude(_altitude_new);
_sensor_simulator.runSeconds(2); _sensor_simulator.runSeconds(5);
_ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); _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_DOUBLE_EQ(_longitude, _longitude_new);
EXPECT_NEAR(_altitude, _altitude_new, 0.01f); EXPECT_NEAR(_altitude, _altitude_new, 0.01f);
_latitude_new = -15.0000005; // Note: we cannot reset too far since the local position is limited to 1e6m
_longitude_new = -115.0000005; _latitude_new = 14.0000005;
_longitude_new = 109.0000005;
_altitude_new = 1500.0; _altitude_new = 1500.0;
_ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); _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_DOUBLE_EQ(_longitude, _longitude_new);
EXPECT_FLOAT_EQ(_altitude, _altitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new);
_sensor_simulator.runSeconds(1);
float hpos = 0.f; float hpos = 0.f;
float vpos = 0.f; float vpos = 0.f;
float hvel = 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_DOUBLE_EQ(_longitude, _longitude_new);
EXPECT_FLOAT_EQ(_altitude, _altitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new);
_sensor_simulator.runSeconds(1);
float hpos = 0.f; float hpos = 0.f;
float vpos = 0.f; float vpos = 0.f;
float hvel = 0.f; float hvel = 0.f;