diff --git a/libraries/AP_Declination/AP_Declination.cpp b/libraries/AP_Declination/AP_Declination.cpp index f48e881dd8..37dd1ed51c 100644 --- a/libraries/AP_Declination/AP_Declination.cpp +++ b/libraries/AP_Declination/AP_Declination.cpp @@ -127,3 +127,19 @@ float AP_Declination::get_declination(float latitude_deg, float longitude_deg) return declination_deg; } +/* + get earth field as a Vector3f in Gauss given a Location +*/ +Vector3f AP_Declination::get_earth_field_ga(const Location &loc) +{ + float declination_deg=0, inclination_deg=0, intensity_gauss=0; + get_mag_field_ef(loc.lat*1.0e-7f, loc.lng*1.0e-7f, intensity_gauss, declination_deg, inclination_deg); + + // create earth field + Vector3f mag_ef = Vector3f(intensity_gauss, 0.0, 0.0); + Matrix3f R; + + R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg)); + mag_ef = R * mag_ef; + return mag_ef; +} diff --git a/libraries/AP_Declination/AP_Declination.h b/libraries/AP_Declination/AP_Declination.h index 107f930e34..a9dec3fea0 100644 --- a/libraries/AP_Declination/AP_Declination.h +++ b/libraries/AP_Declination/AP_Declination.h @@ -1,5 +1,7 @@ #pragma once +#include + /* magnetic data derived from WMM */ @@ -16,6 +18,11 @@ public: */ static bool get_mag_field_ef(float latitude_deg, float longitude_deg, float &intensity_gauss, float &declination_deg, float &inclination_deg); + /* + get earth field as a Vector3f in Gauss given a Location + */ + static Vector3f get_earth_field_ga(const Location &loc); + /* get declination in degrees for a given latitude_deg and longitude_deg */