Browse Source

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

c415-sdk
Andrew Tridgell 5 years ago
parent
commit
fce4d6952f
  1. 3
      libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

3
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@ -577,6 +577,8 @@ void NavEKF2_core::readGpsData() @@ -577,6 +577,8 @@ void NavEKF2_core::readGpsData()
}
if (gpsGoodToAlign && !have_table_earth_field) {
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));
@ -586,6 +588,7 @@ void NavEKF2_core::readGpsData() @@ -586,6 +588,7 @@ void NavEKF2_core::readGpsData()
stateStruct.earth_magfield = table_earth_field_ga;
}
}
}
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) {

Loading…
Cancel
Save