Skip to content

Commit

Permalink
simplify world_magnetic_model interface (degrees in, degrees out)
Browse files Browse the repository at this point in the history 
 - this hopefully helps avoid accidental mis-use
 - try to clarify units everywhere
  • Loading branch information
@dagar
dagar committed May 31, 2024
1 parent ccbae90 commit 8dc431c
Show file tree
Hide file tree
Showing 8 changed files with 52 additions and 72 deletions.
11 changes: 4 additions & 7 deletions src/examples/fake_magnetometer/FakeMagnetometer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,12 @@ void FakeMagnetometer::Run()
if (_vehicle_gps_position_sub.copy(&gps)) {
if (gps.eph < 1000) {

const double lat = gps.latitude_deg;
const double lon = gps.longitude_deg;

// magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(lat, lon);
const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
const float declination_rad = math::radians(get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg));
const float inclination_rad = math::radians(get_mag_inclination_degrees(gps.latitude_deg, gps.longitude_deg));
const float field_strength_gauss = get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);

_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);

_mag_earth_available = true;
}
Expand Down
47 changes: 19 additions & 28 deletions src/lib/world_magnetic_model/geo_mag_declination.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,21 +66,21 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma
return static_cast<unsigned>((-(min) + *val) / SAMPLING_RES);
}

static constexpr float get_table_data(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM])
static constexpr float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM])
{
lat = math::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
latitude_deg = math::constrain(latitude_deg, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);

if (lon > SAMPLING_MAX_LON) {
lon -= 360;
if (longitude_deg > SAMPLING_MAX_LON) {
longitude_deg -= 360.f;
}

if (lon < SAMPLING_MIN_LON) {
lon += 360;
if (longitude_deg < SAMPLING_MIN_LON) {
longitude_deg += 360.f;
}

/* round down to nearest sampling resolution */
float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES;
float min_lat = floorf(latitude_deg / SAMPLING_RES) * SAMPLING_RES;
float min_lon = floorf(longitude_deg / SAMPLING_RES) * SAMPLING_RES;

/* find index of nearest low sampling point */
unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
Expand All @@ -92,45 +92,36 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[
const float data_nw = table[min_lat_index + 1][min_lon_index];

/* perform bilinear interpolation on the four grid corners */
const float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f);
const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f);
const float lat_scale = constrain((latitude_deg - min_lat) / SAMPLING_RES, 0.f, 1.f);
const float lon_scale = constrain((longitude_deg - min_lon) / SAMPLING_RES, 0.f, 1.f);

const float data_min = lon_scale * (data_se - data_sw) + data_sw;
const float data_max = lon_scale * (data_ne - data_nw) + data_nw;

return lat_scale * (data_max - data_min) + data_min;
}

float get_mag_declination_radians(float lat, float lon)
{
return math::radians(get_mag_declination_degrees(lat, lon));
}

float get_mag_declination_degrees(float lat, float lon)
float get_mag_declination_degrees(float latitude_deg, float longitude_deg)
{
// table stored as scaled degrees
return get_table_data(lat, lon, declination_table) * WMM_DECLINATION_SCALE_TO_DEGREES;
}

float get_mag_inclination_radians(float lat, float lon)
{
return math::radians(get_mag_inclination_degrees(lat, lon));
return get_table_data(latitude_deg, longitude_deg, declination_table) * WMM_DECLINATION_SCALE_TO_DEGREES;
}

float get_mag_inclination_degrees(float lat, float lon)
float get_mag_inclination_degrees(float latitude_deg, float longitude_deg)
{
// table stored as scaled degrees
return get_table_data(lat, lon, inclination_table) * WMM_INCLINATION_SCALE_TO_DEGREES;
return get_table_data(latitude_deg, longitude_deg, inclination_table) * WMM_INCLINATION_SCALE_TO_DEGREES;
}

float get_mag_strength_gauss(float lat, float lon)
float get_mag_strength_gauss(float latitude_deg, float longitude_deg)
{
// 1 Gauss = 1e4 Tesla
return get_mag_strength_tesla(lat, lon) * 1e4f;
return get_mag_strength_tesla(latitude_deg, longitude_deg) * 1e4f;
}

float get_mag_strength_tesla(float lat, float lon)
float get_mag_strength_tesla(float latitude_deg, float longitude_deg)
{
// table stored as scaled nanotesla
return get_table_data(lat, lon, totalintensity_table) * WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA * 1e-9f;
return get_table_data(latitude_deg, longitude_deg, totalintensity_table)
* WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA * 1e-9f;
}
14 changes: 6 additions & 8 deletions src/lib/world_magnetic_model/geo_mag_declination.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,12 @@

#pragma once

// Return magnetic declination in degrees or radians
float get_mag_declination_degrees(float lat, float lon);
float get_mag_declination_radians(float lat, float lon);
// Return magnetic declination in degrees
float get_mag_declination_degrees(float latitude_deg, float longitude_deg);

// Return magnetic field inclination in degrees or radians
float get_mag_inclination_degrees(float lat, float lon);
float get_mag_inclination_radians(float lat, float lon);
// Return magnetic field inclination in degrees
float get_mag_inclination_degrees(float latitude_deg, float longitude_deg);

// return magnetic field strength in Gauss or Tesla
float get_mag_strength_gauss(float lat, float lon);
float get_mag_strength_tesla(float lat, float lon);
float get_mag_strength_gauss(float latitude_deg, float longitude_deg);
float get_mag_strength_tesla(float latitude_deg, float longitude_deg);
4 changes: 2 additions & 2 deletions src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,8 +227,8 @@ void AttitudeEstimatorQ::update_gps_position()
if (_vehicle_gps_position_sub.update(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
// set magnetic declination automatically
update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg,
(float)gps.longitude_deg));
float mag_decl_deg = get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg);
update_mag_declination(math::radians(mag_decl_deg));
}
}
}
Expand Down
25 changes: 11 additions & 14 deletions src/modules/commander/mag_calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,11 +237,8 @@ static float get_sphere_radius()

if (gps_sub.copy(&gps)) {
if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
const double lat = gps.latitude_deg;
const double lon = gps.longitude_deg;

// magnetic field data returned by the geo library using the current GPS position
return get_mag_strength_gauss(lat, lon);
return get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);
}
}
}
Expand Down Expand Up @@ -954,13 +951,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
return result;
}

int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude)
int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians,
float latitude_deg, float longitude_deg)
{
// magnetometer quick calibration
// if GPS available use world magnetic model to zero mag offsets
bool mag_earth_available = false;

if (PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)) {
if (PX4_ISFINITE(latitude_deg) && PX4_ISFINITE(longitude_deg)) {
mag_earth_available = true;

} else {
Expand All @@ -969,8 +967,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian

if (vehicle_gps_position_sub.copy(&gps)) {
if ((gps.timestamp != 0) && (gps.eph < 1000)) {
latitude = (float)gps.latitude_deg;
longitude = (float)gps.longitude_deg;
latitude_deg = (float)gps.latitude_deg;
longitude_deg = (float)gps.longitude_deg;
mag_earth_available = true;
}
}
Expand All @@ -981,14 +979,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
return PX4_ERROR;

} else {

// magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);
const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
const float field_strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);

const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps,
0, 0);
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad))
* Vector3f(field_strength_gauss, 0, 0);

uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
vehicle_attitude_s attitude{};
Expand Down
11 changes: 4 additions & 7 deletions src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,15 +99,12 @@ void Ekf::collect_gps(const gnssSample &gps)
if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {

// If we have good GPS data set the origin's WGS-84 position to the last gps fix
const double lat = gps.lat;

#if defined(CONFIG_EKF2_MAGNETOMETER)
const double lon = gps.lon;

// set the magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(lat, lon);
const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
const float mag_declination_gps = math::radians(get_mag_declination_degrees(gps.lat, gps.lon));
const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(gps.lat, gps.lon));
const float mag_strength_gps = get_mag_strength_gauss(gps.lat, gps.lon);

if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {

Expand All @@ -130,7 +127,7 @@ void Ekf::collect_gps(const gnssSample &gps)
}
#endif // CONFIG_EKF2_MAGNETOMETER

_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
}

_wmm_gps_time_last_checked = _time_delayed_us;
Expand Down
4 changes: 2 additions & 2 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,8 +96,8 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
_gps_alt_ref = altitude;

#if defined(CONFIG_EKF2_MAGNETOMETER)
const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
const float mag_declination_gps = math::radians(get_mag_declination_degrees(latitude, longitude));
const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(latitude, longitude));
const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);

if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
Expand Down
8 changes: 4 additions & 4 deletions src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,11 +113,11 @@ void SensorMagSim::Run()
if (gpos.eph < 1000) {

// magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(gpos.lat, gpos.lon);
const float mag_inclination_gps = get_mag_inclination_radians(gpos.lat, gpos.lon);
const float mag_strength_gps = get_mag_strength_gauss(gpos.lat, gpos.lon);
const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon));
const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon));
const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon);

_mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0);
_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);

_mag_earth_available = true;
}
Expand Down

0 comments on commit 8dc431c

Please sign in to comment.