diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index abf067bb21..f1492d8794 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -37,16 +37,8 @@ void CompassLearn::update(void) return; } - // setup the expected earth field at this location - float declination_deg=0, inclination_deg=0, intensity_gauss=0; - AP_Declination::get_mag_field_ef(loc.lat*1.0e-7, loc.lng*1.0e-7, intensity_gauss, declination_deg, inclination_deg); - - // create earth field - mag_ef = Vector3f(intensity_gauss*1000, 0.0, 0.0); - Matrix3f R; - - R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg)); - mag_ef = R * mag_ef; + // setup the expected earth field in mGauss at this location + mag_ef = AP_Declination::get_earth_field_ga(loc) * 1000; sem = hal.util->new_semaphore(); @@ -68,8 +60,9 @@ void CompassLearn::update(void) } // set initial error to field intensity + float intensity = mag_ef.length(); for (uint16_t i=0; iregister_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));