ekf2: extract WMM update logic

Peize-Liu · Aug 20, 2024 · 0931179 · 0931179
1 parent 98eae3c
commit 0931179
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 45 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
@@ -98,37 +98,7 @@ void Ekf::collect_gps(const gnssSample &gps)
 		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
 
 		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
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-
-			// set the magnetic field data returned by the geo library using the current GPS position
-			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)) {
-
-				const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
-				const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
-
-				if ((_wmm_gps_time_last_set == 0)
-				    || !PX4_ISFINITE(_mag_declination_gps)
-				    || !PX4_ISFINITE(_mag_inclination_gps)
-				    || !PX4_ISFINITE(_mag_strength_gps)
-				    || mag_declination_changed
-				    || mag_inclination_changed
-				   ) {
-					_mag_declination_gps = mag_declination_gps;
-					_mag_inclination_gps = mag_inclination_gps;
-					_mag_strength_gps = mag_strength_gps;
-
-					_wmm_gps_time_last_set = _time_delayed_us;
-				}
-			}
-
-#endif // CONFIG_EKF2_MAGNETOMETER
-
+			updateWmm(gps.lat, gps.lon);
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
 		}
 

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -259,6 +259,7 @@ class Ekf final : public EstimatorInterface
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
+	void updateWmm(double lat, double lon);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;

diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -96,20 +96,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
 		_gps_alt_ref = altitude;
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-		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)) {
-			_mag_declination_gps = mag_declination_gps;
-			_mag_inclination_gps = mag_inclination_gps;
-			_mag_strength_gps = mag_strength_gps;
-
-			_wmm_gps_time_last_set = _time_delayed_us;
-		}
-
-#endif // CONFIG_EKF2_MAGNETOMETER
+		updateWmm(current_lat, current_lon);
 
 		_gpos_origin_eph = eph;
 		_gpos_origin_epv = epv;
@@ -144,6 +131,39 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 	return false;
 }
 
+void Ekf::updateWmm(const double lat, const double lon)
+{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+
+	// set the magnetic field data returned by the geo library using the current GPS position
+	const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon));
+	const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon));
+	const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
+
+	if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
+
+		const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
+		const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
+
+		if ((_wmm_gps_time_last_set == 0)
+		    || !PX4_ISFINITE(_mag_declination_gps)
+		    || !PX4_ISFINITE(_mag_inclination_gps)
+		    || !PX4_ISFINITE(_mag_strength_gps)
+		    || mag_declination_changed
+		    || mag_inclination_changed
+		   ) {
+			_mag_declination_gps = mag_declination_gps;
+			_mag_inclination_gps = mag_inclination_gps;
+			_mag_strength_gps = mag_strength_gps;
+
+			_wmm_gps_time_last_set = _time_delayed_us;
+		}
+	}
+
+#endif // CONFIG_EKF2_MAGNETOMETER
+}
+
+
 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
 	float eph = INFINITY;