diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index d126c09a52..d94b2a076a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -148,6 +148,11 @@ void EstimatorInterface::setGpsData(const gps_message &gps) } if ((gps.time_usec - _time_last_gps) > _min_obs_interval_us) { + + if (!gps.vel_ned_valid || (gps.fix_type == 0)) { + return; + } + _time_last_gps = gps.time_usec; gpsSample gps_sample_new; diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 436b52b3da..dcbc72fbe4 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -79,6 +79,7 @@ public: TEST_F(EkfGpsTest, gpsTimeout) { // GIVEN:EKF that fuses GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); // WHEN: setting the PDOP to high _sensor_simulator._gps.setNumberOfSatellites(3); @@ -90,6 +91,24 @@ TEST_F(EkfGpsTest, gpsTimeout) EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); } +TEST_F(EkfGpsTest, gpsFixLoss) +{ + // GIVEN:EKF that fuses GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + // WHEN: the fix is loss + _sensor_simulator._gps.setFixType(0); + + // THEN: after dead-reconing for a couple of seconds, the local position gets invalidated + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning); + EXPECT_FALSE(_ekf->local_position_is_valid()); + + // The control logic takes a bit more time to deactivate the GNSS fusion completely + _sensor_simulator.runSeconds(5); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); +} + TEST_F(EkfGpsTest, resetToGpsVelocity) { ResetLoggingChecker reset_logging_checker(_ekf);