diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index e409061698bf2c..958d75e0335c24 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -548,6 +548,23 @@ void NavEKF3_core::readGpsData() // check for new GPS data const auto &gps = dal.gps(); + Vector3f magNED; + getMagNED(magNED); + if (gps.status() == AP_DAL_GPS::NO_GPS && validOrigin) { + // if there is not gps, we should still align if we have a valid origin + alignMagStateDeclination(); + const auto &compass = dal.compass(); + if (compass.have_scale_factor(magSelectIndex) && + compass.auto_declination_enabled()) { + getEarthFieldTable(EKF_origin); + if (frontend->_mag_ef_limit > 0) { + // initialise earth field from tables + stateStruct.earth_magfield = table_earth_field_ga; + } + } + return; + } + // limit update rate to avoid overflowing the FIFO buffer if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) { return;