AP_NavEKF2: print reason for EKF2 arming failure when no GPS is available

This commit is contained in:
Samuel Tabor 2017-11-16 14:39:27 +00:00 committed by Randy Mackay
parent 012ddbefac
commit 1b4705242c

View File

@ -334,6 +334,9 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
{ {
// If we are a plane and don't have GPS lock then don't initialise // If we are a plane and don't have GPS lock then don't initialise
if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) { if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"EKF2 init failure: No GPS lock");
statesInitialised = false; statesInitialised = false;
return false; return false;
} }