Browse Source

AP_NavEKF2: don't use WMM tables unless we have a compass scale factor set

celiu-4.0.17-rc8
Andrew Tridgell 5 years ago committed by Randy Mackay
parent
commit
d47126fc6b
  1. 17
      libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

17
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@ -577,13 +577,16 @@ void NavEKF2_core::readGpsData() @@ -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;
}
}
}

Loading…
Cancel
Save