fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 09:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Add GPS fix type check bit (#25215)

Browse Source
This commit is contained in:
Ian Scholl
2025-08-07 21:50:58 -04:00
committed by GitHub
parent 9531262cfe
commit 4812311c6c
7 changed files with 31 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp
+3 -3
View File
@@ -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)) ||
src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp
+6 -5
View File
@@ -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<int32_t>(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;
};
src/modules/ekf2/EKF/common.h
+2 -1
View File
@@ -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
src/modules/ekf2/EKF/estimator_interface.h
+1
View File
@@ -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};
src/modules/ekf2/EKF2.cpp
+1
View File
@@ -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)
src/modules/ekf2/EKF2.hpp
+1
View File
@@ -535,6 +535,7 @@ private:
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>) _param_ekf2_req_pdop,
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _param_ekf2_req_hdrift,
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,
(ParamExtInt<px4::params::EKF2_REQ_FIX>) _param_ekf2_req_fix,
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h,
// Used by EKF-GSF experimental yaw estimator
src/modules/ekf2/params_gnss.yaml
+17 -2
View File
@@ -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