Browse Source

AP_NavEKF3: Fix GPS < 3D empty PreArm: msg-as EKF2

master
Jaaaky 6 years ago committed by Andrew Tridgell
parent
commit
f21e4d833c
  1. 3
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

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

Loading…
Cancel
Save