yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander: rework preflight GPS checks

Browse Source 
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.
This commit is contained in:
Paul Riseborough
2017-11-29 07:45:03 +11:00
committed by Lorenz Meier
parent c09eecbab1
commit afe857dfe6
1 changed files with 40 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
src/modules/commander/PreflightCheck.cpp
View File

@@ -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;
}
}