From afe857dfe66752a1b500a6a59b307938f5bde51c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 29 Nov 2017 07:45:03 +1100 Subject: [PATCH] commander: rework preflight GPS checks Fix the bug allowing arming without GPS checks passed in the first 20 seconds after gaining GPS lock when COM_ARM_WO_GPS is set to 0 Allow 10 seconds after boot for EKF quality checks to pass before reporting failure to allow EKF to stabilise. Move GPS quality checking and reporting to after all innovation and bias checks. Make messages more informative. Add missing GPS speed accuracy check. --- src/modules/commander/PreflightCheck.cpp | 73 +++++++++++++----------- 1 file changed, 40 insertions(+), 33 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 25a5317190..5fb03515d1 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -504,6 +504,9 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ // Check if preflight check perfomred by estimator has failed if (status.pre_flt_fail) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF INTERNAL CHECKS"); + } success = false; goto out; } @@ -538,32 +541,6 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ goto out; } - // If GPS aiding is required, declare fault condition if the EKF is not using GPS - if (enforce_gps_required) { - bool ekf_gps_fusion = status.control_mode_flags & (1 << 2); - if (!ekf_gps_fusion) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); - } - success = false; - goto out; - } - } - - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (enforce_gps_required) { - if ((status.gps_check_fail_flags & ((1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR) - + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR))) > 0) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY CHECKS"); - } - success = false; - goto out; - } - } - // check magnetometer innovation test ratio param_get(param_find("COM_ARM_EKF_YAW"), &test_limit); if (status.mag_test_ratio > test_limit) { @@ -594,6 +571,40 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_ goto out; } + // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing + if (enforce_gps_required) { + bool ekf_gps_fusion = status.control_mode_flags & (1 << 2); + bool ekf_gps_check_fail = status.gps_check_fail_flags > 0; + if (!ekf_gps_fusion) { + // The EKF is not using GPS + if (report_fail) { + if (ekf_gps_check_fail) { + // Poor GPS qulaity is the likely cause + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + } else { + // Likely cause unknown + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); + } + } + success = false; + goto out; + } else { + // The EKF is using GPS so check for bad quality on key performance indicators + bool gps_quality_fail = ((status.gps_check_fail_flags & ((1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR) + + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0); + if (gps_quality_fail) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + } + success = false; + goto out; + } + } + } + out: orb_unsubscribe(sub); return success; @@ -774,15 +785,11 @@ 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) { 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 ---- */ @@ -790,10 +797,10 @@ 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) { - // 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) && checkGNSS; + // don't report ekf failures for the first 10 seconds to allow time for the filter to start + bool report_ekf_fail = (time_since_boot > 10 * 1000000); - if (!ekf2Check(mavlink_log_pub, true, reportFailures, enforce_gps_required)) { + if (!ekf2Check(mavlink_log_pub, true, report_ekf_fail, checkGNSS)) { failed = true; } }