From d47126fc6b06b2229d79c5993ccfef3c7cf5c699 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 27 Nov 2019 09:13:14 +1100 Subject: [PATCH] AP_NavEKF2: don't use WMM tables unless we have a compass scale factor set --- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index d504e1d2f2..b8a2fac16d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -577,13 +577,16 @@ void NavEKF2_core::readGpsData() } if (gpsGoodToAlign && !have_table_earth_field) { - table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc); - table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, - gpsloc.lng*1.0e-7)); - have_table_earth_field = true; - if (frontend->_mag_ef_limit > 0) { - // initialise earth field from tables - stateStruct.earth_magfield = table_earth_field_ga; + const Compass *compass = _ahrs->get_compass(); + if (compass && compass->have_scale_factor(magSelectIndex)) { + table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc); + table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, + gpsloc.lng*1.0e-7)); + have_table_earth_field = true; + if (frontend->_mag_ef_limit > 0) { + // initialise earth field from tables + stateStruct.earth_magfield = table_earth_field_ga; + } } }