From 9db9c066c31cb2e5e5e3e00a8fe28bd91a024f2d Mon Sep 17 00:00:00 2001 From: Ian Scholl Date: Mon, 14 Jul 2025 13:43:08 +0200 Subject: [PATCH] add GPS fix type check bit to EKF2 GPS checks --- .../ekf2/EKF/aid_sources/gnss/gnss_checks.cpp | 8 ++++---- .../ekf2/EKF/aid_sources/gnss/gnss_checks.hpp | 8 +++++--- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/estimator_interface.h | 1 + src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/params_gnss.yaml | 13 +++++++++++-- 7 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp index b5f01fbde7..333acef7e0 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -81,7 +81,7 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us) bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss) { - _check_fail_status.flags.fix = (gnss.fix_type < 3); + _check_fail_status.flags.fix = (gnss.fix_type < _params.ekf2_req_fix); // Check the reported horizontal and vertical position accuracy _check_fail_status.flags.hacc = (gnss.hacc > 50.f); @@ -95,7 +95,7 @@ bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss) bool passed = true; if ( - _check_fail_status.flags.fix || + (_check_fail_status.flags.fix && isCheckEnabled(GnssChecksMask::kFix)) || (_check_fail_status.flags.hacc && isCheckEnabled(GnssChecksMask::kHacc)) || (_check_fail_status.flags.vacc && isCheckEnabled(GnssChecksMask::kVacc)) || (_check_fail_status.flags.sacc && isCheckEnabled(GnssChecksMask::kSacc)) || @@ -110,7 +110,7 @@ bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss) bool GnssChecks::runInitialFixChecks(const gnssSample &gnss) { // Check the fix type - _check_fail_status.flags.fix = (gnss.fix_type < 3); + _check_fail_status.flags.fix = (gnss.fix_type < _params.ekf2_req_fix); // Check the number of satellites _check_fail_status.flags.nsats = (gnss.nsats < _params.ekf2_req_nsats); @@ -143,7 +143,7 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss) // if any user selected checks have failed, record the fail time if ( - _check_fail_status.flags.fix || + (_check_fail_status.flags.fix && isCheckEnabled(GnssChecksMask::kFix)) || (_check_fail_status.flags.nsats && isCheckEnabled(GnssChecksMask::kNsats)) || (_check_fail_status.flags.pdop && isCheckEnabled(GnssChecksMask::kPdop)) || (_check_fail_status.flags.hacc && isCheckEnabled(GnssChecksMask::kHacc)) || diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp index f8476c7eee..f3c1c0915b 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp @@ -45,9 +45,9 @@ class GnssChecks final public: GnssChecks(int32_t &check_mask, int32_t &ekf2_req_nsats, float &ekf2_req_pdop, float &ekf2_req_eph, float &ekf2_req_epv, float &ekf2_req_sacc, - float &ekf2_req_hdrift, float &ekf2_req_vdrift, float &ekf2_vel_lim, uint32_t &min_health_time_us, + float &ekf2_req_hdrift, float &ekf2_req_vdrift, int32_t &ekf2_req_fix, float &ekf2_vel_lim, uint32_t &min_health_time_us, filter_control_status_u &control_status): - _params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_vel_lim, min_health_time_us}, + _params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_req_fix, ekf2_vel_lim, min_health_time_us}, _control_status(control_status) {}; @@ -108,7 +108,8 @@ private: kVdrift = (1 << 6), kHspd = (1 << 7), kVspd = (1 << 8), - kSpoofed = (1 << 9) + kSpoofed = (1 << 9), + kFix = (1 << 10) }; bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast(check)); } @@ -151,6 +152,7 @@ private: const float &ekf2_req_sacc; const float &ekf2_req_hdrift; const float &ekf2_req_vdrift; + const int32_t &ekf2_req_fix; const float &ekf2_vel_lim; const uint32_t &min_health_time_us; }; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 04f7409bfd..e2215386ac 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -343,6 +343,7 @@ struct parameters { float ekf2_req_pdop{2.0f}; ///< maximum acceptable position dilution of precision float ekf2_req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) float ekf2_req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) + int32_t ekf2_req_fix{3}; ///< minimum acceptable GPS fix type # if defined(CONFIG_EKF2_GNSS_YAW) // GNSS heading fusion diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 56e8334fba..8229d345f3 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -412,6 +412,7 @@ protected: _params.ekf2_req_sacc, _params.ekf2_req_hdrift, _params.ekf2_req_vdrift, + _params.ekf2_req_fix, _params.ekf2_vel_lim, _min_gps_health_time_us, _control_status}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 785f5565f4..e0f6d6f939 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -96,6 +96,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_req_pdop(_params->ekf2_req_pdop), _param_ekf2_req_hdrift(_params->ekf2_req_hdrift), _param_ekf2_req_vdrift(_params->ekf2_req_vdrift), + _param_ekf2_req_fix(_params->ekf2_req_fix), _param_ekf2_gsf_tas(_params->ekf2_gsf_tas), #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index dcae0b0cf3..d029ef5b49 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -533,6 +533,7 @@ private: (ParamExtFloat) _param_ekf2_req_pdop, (ParamExtFloat) _param_ekf2_req_hdrift, (ParamExtFloat) _param_ekf2_req_vdrift, + (ParamExtInt) _param_ekf2_req_fix, (ParamFloat) _param_ekf2_req_gps_h, // Used by EKF-GSF experimental yaw estimator diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index e21a8423e2..8e0cae15a1 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -115,9 +115,10 @@ parameters: 7: Horizontal speed offset (EKF2_REQ_HDRIFT) 8: Vertical speed offset (EKF2_REQ_VDRIFT) 9: Spoofing - default: 1023 + 10: GPS fix type (EKF2_REQ_FIX) + default: 2047 min: 0 - max: 1023 + max: 2047 EKF2_REQ_EPH: description: short: Required EPH to use GPS @@ -178,6 +179,14 @@ parameters: max: 1.5 decimal: 2 unit: m/s + EKF2_REQ_FIX: + description: + short: Required minimum GPS fix type to use GPS + long: Minimum GPS fix type required for GPS usage. 0-1 - no fix, 2 - 2D fix, 3 - 3D fix, 4 - RTCM code differential, 5 - Real-Time Kinematic, float, 6 - Real-Time Kinematic, fixed, 8 - Extrapolated. + type: int32 + default: 3 + min: 0 + max: 8 EKF2_REQ_GPS_H: description: short: Required GPS health time on startup