|
|
|
@ -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) { |
|
|
|
|