Skip to content

ekf2: GNSS checks relax pos/vel req once passing #24420

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+10 −5
Open

ekf2: GNSS checks relax pos/vel req once passing #24420

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 10 additions & 5 deletions src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading