From 3a6362de8d0a481b5b1e99c2c47431d4b77a0d87 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 22 Nov 2023 13:59:02 -0300 Subject: [PATCH] EKF3: allow earth-frame fields to be estimated with an origin but no GPS --- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) 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;