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

add GPS fix type check bit to EKF2 GPS checks

Browse Source
This commit is contained in:
Ian Scholl 2025-07-14 13:43:08 +02:00
parent 4896099232
commit 9db9c066c3
7 changed files with 24 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -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<int32_t>(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;
};

1
src/modules/ekf2/EKF/common.h
View File

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

1
src/modules/ekf2/EKF/estimator_interface.h
View File

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

1
src/modules/ekf2/EKF2.cpp
View File

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

1
src/modules/ekf2/EKF2.hpp
View File

@ -533,6 +533,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

13
src/modules/ekf2/params_gnss.yaml
View File

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