From a1ff219380587801d7b795e1ad65bfc308d58a73 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Mar 2017 17:40:18 +1100 Subject: [PATCH] EKF: Extend range of conditions over which we run GPS quality checks Previously GPS quality checks were only run until the EKF origin was set. This meant that they could not be used by other pre-flight checks. This change ensures that checks will always be run when the vehicle on-ground or not using GPS to enable use by external preflight checks. --- EKF/gps_checks.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 9a96a8af40..94478c379c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -57,11 +57,12 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { - // If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix - if (!_NED_origin_initialised) { - // we have good GPS data so can now set the origin's WGS-84 position - if (gps_is_good(gps) && !_NED_origin_initialised) { - // Set the origin's WGS-84 position to the last gps fix + // Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS + // Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks + if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) { + bool gps_checks_pass = gps_is_good(gps); + if (!_NED_origin_initialised && gps_checks_pass) { + // If we have good GPS data set the origin's WGS-84 position to the last gps fix double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);