diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 10dfcdc6b4..5ba9029dbf 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -30,6 +30,12 @@ static struct { // should be called at 10hz void Copter::ekf_check() { + // exit immediately if ekf has no origin yet - this assumes the origin can never become unset + Location temp_loc; + if (!ahrs.get_NavEKF_const().getOriginLLH(temp_loc)) { + return; + } + // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected if (!motors.armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) { ekf_check_state.fail_count = 0;