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: EKF2 GPS requirement 20 sec after 3D lock

Browse Source
This commit is contained in:
Paul Riseborough
2017-09-20 09:32:29 +10:00
committed by Daniel Agar
parent 2495f8942b
commit 2a4336b6ef
1 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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