fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

simplify world_magnetic_model interface (degrees in, degrees out)

Browse Source 
- this hopefully helps avoid accidental mis-use
 - try to clarify units everywhere
This commit is contained in:
Daniel Agar
2024-06-04 09:14:36 -04:00
parent 5fa3b9d86a
commit 157f7cf40b
8 changed files with 52 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+4 -7
View File
@@ -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;
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -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)) {