EKF: use GPS to lookup declination from WMM before full GPS checks pass

EKF/control.cpp

@@ -99,10 +99,11 @@ void Ekf::controlFusionModes()
 	if (_mag_data_ready) {
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
-		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) {
+		if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised || ISFINITE(_mag_declination_gps))) {
 			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
 			_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
 			_control_status.flags.synthetic_mag_z = true;
+
 		} else {
 			_control_status.flags.synthetic_mag_z = false;
 		}

EKF/ekf_helper.cpp

@@ -524,7 +524,7 @@ float Ekf::getMagDeclination()
 
 	} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
-		if (_NED_origin_initialised) {
+		if (_NED_origin_initialised || ISFINITE(_mag_declination_gps)) {
 			return _mag_declination_gps;
 
 		} else {

EKF/estimator_interface.h

@@ -569,9 +569,9 @@ protected:
 	// free buffer memory
 	void unallocate_buffers();
 
-	float _mag_declination_gps{0.0f};         // magnetic declination returned by the geo library using the last valid GPS position (rad)
-	float _mag_inclination_gps{0.0f};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
-	float _mag_strength_gps{0.0f};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
+	float _mag_declination_gps{NAN};         // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
 
 	// this is the current status of the filter control modes
 	filter_control_status_u _control_status{};

EKF/gps_checks.cpp

@@ -60,6 +60,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 {
 	// Run GPS checks always
 	_gps_checks_passed = gps_is_good(gps);
+
 	if (!_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
 		const double lat = gps.lat * 1.0e-7;
@@ -79,6 +80,8 @@ bool Ekf::collect_gps(const gps_message &gps)
 		_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
 		_last_gps_origin_time_us = _time_last_imu;
 
+		const bool declination_was_valid = ISFINITE(_mag_declination_gps);
+
 		// set the magnetic field data returned by the geo library using the current GPS position
 		_mag_declination_gps = get_mag_declination_radians(lat, lon);
 		_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
@@ -89,9 +92,13 @@ bool Ekf::collect_gps(const gps_message &gps)
 			// try to reset the yaw using the EKF-GSF yaw esitimator
 			_do_ekfgsf_yaw_reset = true;
 			_ekfgsf_yaw_reset_time = 0;
+
 		} else {
-			_mag_yaw_reset_req = true;
+			if (!declination_was_valid) {
+				_mag_yaw_reset_req = true;
+			}
 		}
+
 		// save the horizontal and vertical position uncertainty of the origin
 		_gps_origin_eph = gps.eph;
 		_gps_origin_epv = gps.epv;
@@ -101,7 +108,33 @@ bool Ekf::collect_gps(const gps_message &gps)
 		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
 			startGpsHgtFusion();
 		}
+
 		ECL_INFO_TIMESTAMPED("GPS checks passed");
+
+	} else if (!_NED_origin_initialised) {
+		// a rough 2D fix is still sufficient to lookup declination
+		if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
+
+			const bool declination_was_valid = ISFINITE(_mag_declination_gps);
+
+			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
+			const double lat = gps.lat * 1.0e-7;
+			const double lon = gps.lon * 1.0e-7;
+
+			// set the magnetic field data returned by the geo library using the current GPS position
+			_mag_declination_gps = get_mag_declination_radians(lat, lon);
+			_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
+			_mag_strength_gps = get_mag_strength_tesla(lat, lon);
+
+			// request mag yaw reset if there's a mag declination for the first time
+			if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) {
+				if (!declination_was_valid && ISFINITE(_mag_declination_gps)) {
+					_mag_yaw_reset_req = true;
+				}
+			}
+
+			_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
+		}
 	}
 
 	// start collecting GPS if there is a 3D fix and the NED origin has been set
@@ -135,7 +168,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 
 	// check if GPS quality is degraded
 	_gps_error_norm = fmaxf((gps.eph / _params.req_hacc), (gps.epv / _params.req_vacc));
-	_gps_error_norm = fmaxf(_gps_error_norm , (gps.sacc / _params.req_sacc));
+	_gps_error_norm = fmaxf(_gps_error_norm, (gps.sacc / _params.req_sacc));
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	constexpr float filt_time_const = 10.0f;
@@ -145,6 +178,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	// The following checks are only valid when the vehicle is at rest
 	const double lat = gps.lat * 1.0e-7;
 	const double lon = gps.lon * 1.0e-7;
+
 	if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Calculate position movement since last measurement
 		float delta_pos_n = 0.0f;
@@ -232,6 +266,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		(_gps_check_fail_status.flags.vspeed  && (_params.gps_check_mask & MASK_GPS_VSPD))
 	) {
 		_last_gps_fail_us = _time_last_imu;
+
 	} else {
 		_last_gps_pass_us = _time_last_imu;
 	}

EKF/mag_fusion.cpp

@@ -899,9 +899,10 @@ void Ekf::limitDeclination()
 	float h_field_min = 0.001f;
 	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
 		// use parameter value until GPS is available, then use value returned by geo library
-		if (_NED_origin_initialised) {
+		if (_NED_origin_initialised || ISFINITE(_mag_declination_gps)) {
 			decl_reference = _mag_declination_gps;
 			h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
+
 		} else {
 			decl_reference = math::radians(_params.mag_declination_deg);
 		}