From 4812311c6c4792ac611ed74d2051608428414d45 Mon Sep 17 00:00:00 2001 From: Ian Scholl <91476087+ischollETH@users.noreply.github.com> Date: Fri, 8 Aug 2025 03:50:58 +0200 Subject: [PATCH] ekf2: Add GPS fix type check bit (#25215) --- .../ekf2/EKF/aid_sources/gnss/gnss_checks.cpp | 6 +++--- .../ekf2/EKF/aid_sources/gnss/gnss_checks.hpp | 11 ++++++----- src/modules/ekf2/EKF/common.h | 3 ++- 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 | 19 +++++++++++++++++-- 7 files changed, 31 insertions(+), 11 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..83f82da3ce 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -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..70b133ee70 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp @@ -44,10 +44,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, - 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}, + float &ekf2_req_sacc, 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_req_fix, ekf2_vel_lim, min_health_time_us}, _control_status(control_status) {}; @@ -108,7 +107,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 +151,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 805a6532f2..de4eb07ad6 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -341,7 +341,7 @@ struct parameters { // these parameters control the strictness of GPS quality checks used to determine if the GPS is // good enough to set a local origin and commence aiding - int32_t ekf2_gps_check{21}; ///< bitmask used to control which GPS quality checks are used + int32_t ekf2_gps_check{1045}; ///< bitmask used to control which GPS quality checks are used float ekf2_req_eph{5.0f}; ///< maximum acceptable horizontal position error (m) float ekf2_req_epv{8.0f}; ///< maximum acceptable vertical position error (m) float ekf2_req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) @@ -349,6 +349,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 35597d3803..64d3e4ffc3 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -415,6 +415,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 307f85fea8..76ab2aeab2 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -97,6 +97,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 43e497718e..c95e3c3623 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -535,6 +535,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 3fc87250c4..bcc186e0ba 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -125,9 +125,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 @@ -188,6 +189,20 @@ parameters: max: 1.5 decimal: 2 unit: m/s + EKF2_REQ_FIX: + description: + short: Required GPS fix + long: Minimum GPS fix type required for GPS usage. + type: enum + values: + 0: No fix required + 2: 2D fix + 3: 3D fix + 4: RTCM code differential + 5: RTK float + 6: RTK fixed + 8: Extrapolated + default: 3 EKF2_REQ_GPS_H: description: short: Required GPS health time on startup