diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 425c09f3a7..56878ec985 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -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; } diff --git a/src/lib/world_magnetic_model/geo_mag_declination.cpp b/src/lib/world_magnetic_model/geo_mag_declination.cpp index 97105f4a11..64738426f5 100644 --- a/src/lib/world_magnetic_model/geo_mag_declination.cpp +++ b/src/lib/world_magnetic_model/geo_mag_declination.cpp @@ -66,21 +66,21 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma return static_cast((-(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); @@ -92,8 +92,8 @@ 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; @@ -101,36 +101,27 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[ 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; + return get_table_data(latitude_deg, longitude_deg, 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)); -} - -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; } diff --git a/src/lib/world_magnetic_model/geo_mag_declination.h b/src/lib/world_magnetic_model/geo_mag_declination.h index 790f3b353a..7e6f346706 100644 --- a/src/lib/world_magnetic_model/geo_mag_declination.h +++ b/src/lib/world_magnetic_model/geo_mag_declination.h @@ -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); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 7c47cfca30..f6b645397a 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -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)); } } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 02ab015e0f..4121549bd2 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -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); } } } @@ -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 { @@ -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; } } @@ -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{}; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 71018df1c7..abdbec64fc 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -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)) { @@ -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; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 40b6a4239d..7b6b0b0cac 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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)) { diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp index ff5e5e31aa..97c2626f75 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp @@ -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; }