From dfb6282814fe63d7d009989e6fe86fd2ff9f4205 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 | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index e409061698bf2c..15fdae7f11f80e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -548,6 +548,26 @@ 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) { + printf("No GPS but valid origin\n"); + // print origin + printf("Origin: %f %f %f\n", EKF_origin.lat, EKF_origin.lng, EKF_origin.alt); + // 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;