diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 7e9e3fa2be4..d89eeeb4e92 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -233,7 +233,9 @@ bool Ekf::initialiseTilt() void Ekf::predictState(const imuSample &imu_delayed) { - if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) { + if (global_origin_valid() + && (!PX4_ISFINITE(_earth_rate_lat_ref_rad) + || (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)))) { _earth_rate_lat_ref_rad = _gpos.latitude_rad(); _earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad); _gravity = LatLonAlt::Wgs84::gravity(_earth_rate_lat_ref_rad); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8866ae5c9b8..bf5e7b4df46 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -541,7 +541,7 @@ private: LatLonAlt _last_known_gpos{}; Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s - double _earth_rate_lat_ref_rad{0.0}; ///< latitude at which the earth rate was evaluated (radians) + double _earth_rate_lat_ref_rad{static_cast(NAN)}; ///< latitude at which the earth rate was evaluated (radians) float _gravity{CONSTANTS_ONE_G}; ///< latitude-dependent gravity magnitude (m/s^2) diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 8d0da8f6cc3..d4b26d58582 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -193,17 +193,17 @@ TEST_F(EkfBasicsTest, gravityIsStandardBeforeGlobalPosition) EXPECT_FLOAT_EQ(_ekf->getGravityMss(), CONSTANTS_ONE_G); } -TEST_F(EkfBasicsTest, gravityStaysStandardNearEquator) +TEST_F(EkfBasicsTest, gravityUpdatesNearEquator) { // GIVEN: GPS fixed within 1 deg of the equator _sensor_simulator.setGpsLatitude(0.5); _sensor_simulator.startGps(); _sensor_simulator.runSeconds(11); - // THEN: GPS is fused but the latitude is within the recompute threshold of the - // 0-reference, so gravity remains the standard constant (matches legacy behavior) + // THEN: gravity is evaluated at the first known latitude, however close to the equator ASSERT_EQ(1, (int) _ekf->control_status_flags().gnss_pos); - EXPECT_FLOAT_EQ(_ekf->getGravityMss(), CONSTANTS_ONE_G); + EXPECT_FLOAT_EQ(_ekf->getGravityMss(), LatLonAlt::Wgs84::gravity(math::radians(0.5))); + EXPECT_LT(_ekf->getGravityMss(), CONSTANTS_ONE_G - 0.02f); } TEST_F(EkfBasicsTest, gravityUpdatesAtHighLatitude)