From 2a4336b6ef30d9a1250f796ab12cae45f49594c1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 20 Sep 2017 09:32:29 +1000 Subject: [PATCH] commander: EKF2 GPS requirement 20 sec after 3D lock --- src/modules/commander/PreflightCheck.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 250a8d2168..01b5c5cb20 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -453,9 +453,10 @@ out: return success; } -static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail) +static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool &lock_detected) { bool success = true; + lock_detected = false; int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); //Wait up to 2000ms to allow the driver to detect a GNSS receiver module @@ -472,6 +473,8 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail) if ((OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || (hrt_elapsed_time(&gps.timestamp) > 1000000)) { success = false; + } else if (gps.fix_type >= 3) { + lock_detected = true; } } @@ -765,10 +768,15 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check } /* ---- Global Navigation Satellite System receiver ---- */ + static hrt_abstime _time_last_no_gps_lock = 0; if (checkGNSS) { - if (!gnssCheck(mavlink_log_pub, reportFailures)) { + bool lock_detected = false; + if (!gnssCheck(mavlink_log_pub, reportFailures, lock_detected)) { failed = true; } + if (!lock_detected) { + _time_last_no_gps_lock = time_since_boot; + } } /* ---- Navigation EKF ---- */ @@ -776,8 +784,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check int32_t estimator_type; param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type); if (estimator_type == 2 && checkGNSS) { - // don't require GPS for the first 20s - bool enforce_gps_required = (time_since_boot > 20 * 1000000); + // don't fail if not using GPS for the first 20s after gaining 3D lock because quality checks take time to pass + bool enforce_gps_required = (_time_last_no_gps_lock > 20 * 1000000); if (!ekf2Check(mavlink_log_pub, true, reportFailures, enforce_gps_required)) { failed = true;