mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: use new get_earth_field_ga() API
This commit is contained in:
parent
7814a3f2e3
commit
74805870b8
|
@ -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; i<num_sectors; i++) {
|
||||
errors[i] = intensity_gauss*1000;
|
||||
errors[i] = intensity;
|
||||
}
|
||||
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));
|
||||
|
|
Loading…
Reference in New Issue