Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use new get_earth_field_ga() API

Browse Source
This commit is contained in:
Andrew Tridgell 2019-06-01 21:01:43 +10:00
parent be62bf32cb
commit 7206258587
1 changed files with 4 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Compass/Compass_learn.cpp
View File

@ -42,17 +42,8 @@ void CompassLearn::update(void)
// remember primary mag
primary_mag = compass.get_primary();
// 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-7f, loc.lng*1.0e-7f, 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;
have_earth_field = true;
// form eliptical correction matrix and invert it. This is
@ -71,8 +62,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;
}
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field");