fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: stop GNSS fusion when fix is loss

Browse Source
This commit is contained in:
bresch 2023-07-20 15:42:02 +02:00 committed by Mathieu Bresciani
parent 5736a31a67
commit dcb4c182fe
2 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -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;

19
src/modules/ekf2/test/test_EKF_gps.cpp
View File

@ -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);