Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use new get_earth_field_ga() API

This commit is contained in:
Andrew Tridgell 2019-06-08 08:08:38 +10:00
parent 7814a3f2e3
commit 74805870b8
1 changed files with 4 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Compass/Compass_learn.cpp
View File

@ -37,16 +37,8 @@ void CompassLearn::update(void)
return; return;
} }
// setup the expected earth field at this location // setup the expected earth field in mGauss at this location
float declination_deg=0, inclination_deg=0, intensity_gauss=0; mag_ef = AP_Declination::get_earth_field_ga(loc) * 1000;
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;
sem = hal.util->new_semaphore(); sem = hal.util->new_semaphore();
@ -68,8 +60,9 @@ void CompassLearn::update(void)
} }
// set initial error to field intensity // set initial error to field intensity
float intensity = mag_ef.length();
for (uint16_t i=0; i<num_sectors; i++) { 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)); hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));