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

ekf2: run simplified GNSS checks after initial fix

Browse Source 
This prevents stopping GNSS fusion on slightly degraded solution when
tight checks are set
This commit is contained in:
bresch 2025-05-23 09:35:26 +02:00 committed by Matthias Grob
parent ff87361ffa
commit 457ce90541
3 changed files with 39 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp
View File

@ -50,7 +50,7 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us)
bool passed = false;
if (_initial_checks_passed) {
if (runInitialFixChecks(gnss)) {
if (runSimplifiedChecks(gnss)) {
_time_last_pass_us = time_us;
passed = isTimedOut(_time_last_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10));
@ -79,6 +79,34 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us)
return passed;
}
bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)
{
_check_fail_status.flags.fix = (gnss.fix_type < 3);
// Check the reported horizontal and vertical position accuracy
_check_fail_status.flags.hacc = (gnss.hacc > 50.f);
_check_fail_status.flags.vacc = (gnss.vacc > 50.f);
// Check the reported speed accuracy
_check_fail_status.flags.sacc = (gnss.sacc > 10.f);
_check_fail_status.flags.spoofed = gnss.spoofed;
bool passed = true;
if (
_check_fail_status.flags.fix ||
(_check_fail_status.flags.hacc && isCheckEnabled(GnssChecksMask::kHacc)) ||
(_check_fail_status.flags.vacc && isCheckEnabled(GnssChecksMask::kVacc)) ||
(_check_fail_status.flags.sacc && isCheckEnabled(GnssChecksMask::kSacc)) ||
(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
) {
passed = false;
}
return passed;
}
bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
{
// Check the fix type

1
src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp
View File

@ -112,6 +112,7 @@ private:
bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast<int32_t>(check)); }
bool runSimplifiedChecks(const gnssSample &gnss);
bool runInitialFixChecks(const gnssSample &gnss);
void runOnGroundGnssChecks(const gnssSample &gnss);

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

@ -84,12 +84,19 @@ TEST_F(EkfGpsTest, gpsTimeout)
// WHEN: the number of satellites drops below the minimum
_sensor_simulator._gps.setNumberOfSatellites(3);
// THEN: the GNSS fusion does not stop because other metrics are good enough
_sensor_simulator.runSeconds(8);
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
// WHEN: the fix type drops
_sensor_simulator._gps.setFixType(0);
// THEN: the GNSS fusion stops after some time
_sensor_simulator.runSeconds(8);
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
// BUT WHEN: the number of satellites is good again
_sensor_simulator._gps.setNumberOfSatellites(16);
// BUT WHEN: the fix type is good again
_sensor_simulator._gps.setFixType(3);
// THEN: the GNSS fusion restarts
_sensor_simulator.runSeconds(6);