fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: GNSS checks relax pos/vel req once passing

Browse Source
This commit is contained in:
Daniel Agar
2025-02-26 12:57:53 -05:00
parent 84134e5123
commit 204a7fb305
1 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+10 -5
View File
@@ -68,12 +68,17 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
// Check the position dilution of precision
_gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop);
// Check the reported horizontal and vertical position accuracy
_gps_check_fail_status.flags.hacc = (gps.hacc > _params.req_hacc);
_gps_check_fail_status.flags.vacc = (gps.vacc > _params.req_vacc);
// Check the reported horizontal position accuracy (relaxed once passing)
const float max_horiz_accuracy = _control_status.flags.gps ? 2.f * _params.req_hacc : _params.req_hacc;
_gps_check_fail_status.flags.hacc = (gps.hacc > max_horiz_accuracy);
// Check the reported speed accuracy
_gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc);
// Check the reported vertical position accuracy (relaxed once passing)
const float max_vert_accuracy = _control_status.flags.gps ? 2.f * _params.req_vacc : _params.req_vacc;
_gps_check_fail_status.flags.vacc = (gps.vacc > max_vert_accuracy);
// Check the reported speed accuracy (relaxed once passing)
const float max_speed_accuracy = _control_status.flags.gps ? 2.f * _params.req_sacc : _params.req_sacc;
_gps_check_fail_status.flags.sacc = (gps.sacc > max_speed_accuracy);
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
constexpr float filt_time_const = 10.0f;