ekf2: add mag fusion timestamps

src/modules/ekf2/EKF/ekf.h

@@ -386,6 +386,8 @@ private:
 	uint64_t _time_last_hgt_fuse{0};	///< time the last fusion of vertical position measurements was performed (uSec)
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
+	uint64_t _time_last_heading_fuse{0};
+
 	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
@@ -394,6 +396,8 @@ private:
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
 	uint64_t _time_last_gps_yaw_data{0};	///< time the last GPS yaw measurement was available (uSec)
+	uint64_t _time_last_mag_heading_fuse{0};
+	uint64_t _time_last_mag_3d_fuse{0};
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
@@ -581,10 +585,10 @@ private:
 	void predictCovariance();
 
 	// ekf sequential fusion of magnetometer measurements
-	void fuseMag(const Vector3f &mag);
+	bool fuseMag(const Vector3f &mag, bool update_all_states = true);
 
 	// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
-	void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
+	bool fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
 
 	// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
 	// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
@@ -615,7 +619,7 @@ private:
 
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
-	void fuseDeclination(float decl_sigma);
+	bool fuseDeclination(float decl_sigma);
 
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();
@@ -1025,7 +1029,7 @@ private:
 	// yaw : Euler yaw angle (rad)
 	// yaw_variance : yaw error variance (rad^2)
 	// update_buffer : true if the state change should be also applied to the output observer buffer
-	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer);
+	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true);
 
 	// Declarations used to control use of the EKF-GSF yaw estimator
 

src/modules/ekf2/EKF/mag_control.cpp

@@ -356,12 +356,18 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 		// the angle of the projection onto the horizontal gives the yaw angle
 		float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
-		fuseHeading(measured_hdg, sq(_params.mag_heading_noise));
+		if (fuseHeading(measured_hdg, sq(_params.mag_heading_noise))) {
+			_time_last_mag_heading_fuse = _time_last_imu;
+		}
 	}
 }
 
 void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
+	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
+	// before they are used to constrain heading drift
+	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
+
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
 		// covariances need to be corrected to incorporate knowledge of the declination
@@ -369,13 +375,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag(mag);
+		fuseMag(mag, update_all_states);
 
 	} else {
 		// The normal sequence is to fuse the magnetometer data first before fusing
 		// declination angle at a higher uncertainty to allow some learning of
 		// declination angle over time.
-		fuseMag(mag);
+		fuseMag(mag, update_all_states);
 
 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -45,7 +45,7 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::fuseMag(const Vector3f &mag)
+bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
 {
 	// assign intermediate variables
 	const float &q0 = _state.quat_nominal(0);
@@ -96,7 +96,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 		// we need to re-initialise covariances and abort this fusion step
 		resetMagRelatedCovariances();
 		ECL_ERR("magX %s", numerical_error_covariance_reset_string);
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_mag_x = false;
@@ -138,7 +138,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 		// we need to re-initialise covariances and abort this fusion step
 		resetMagRelatedCovariances();
 		ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_mag_y = false;
@@ -150,7 +150,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 		// we need to re-initialise covariances and abort this fusion step
 		resetMagRelatedCovariances();
 		ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-		return;
+		return false;
 	}
 
 	_fault_status.flags.bad_mag_z = false;
@@ -174,12 +174,24 @@ void Ekf::fuseMag(const Vector3f &mag)
 	for (uint8_t index = 0; index <= 2; index++) {
 		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
 
-		if (_mag_test_ratio(index) > 1.0f) {
+		bool rejected = (_mag_test_ratio(index) > 1.f);
-			all_innovation_checks_passed = false;
-			_innov_check_fail_status.value |= (1 << (index + 3));
 
-		} else {
+		switch (index) {
-			_innov_check_fail_status.value &= ~(1 << (index + 3));
+		case 0:
+			_innov_check_fail_status.flags.reject_mag_x = rejected;
+			break;
+
+		case 1:
+			_innov_check_fail_status.flags.reject_mag_y = rejected;
+			break;
+
+		case 2:
+			_innov_check_fail_status.flags.reject_mag_z = rejected;
+			break;
+		}
+
+		if (rejected) {
+			all_innovation_checks_passed = false;
 		}
 	}
 
@@ -188,13 +200,9 @@ void Ekf::fuseMag(const Vector3f &mag)
 
 	// if any axis fails, abort the mag fusion
 	if (!all_innovation_checks_passed) {
-		return;
+		return false;
 	}
 
-	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
-	// before they are used to constrain heading drift
-	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
-
 	// Observation jacobian and Kalman gain vectors
 	SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
 	Vector24f Kfusion;
@@ -276,7 +284,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 				// we need to re-initialise covariances and abort this fusion step
 				resetMagRelatedCovariances();
 				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
-				return;
+				return false;
 			}
 			const float HKY24 = 1.0F/_mag_innov_var(1);
 
@@ -356,7 +364,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 				// we need to re-initialise covariances and abort this fusion step
 				resetMagRelatedCovariances();
 				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
-				return;
+				return false;
 			}
 
 			const float HKZ24 = 1.0F/_mag_innov_var(2);
@@ -419,6 +427,13 @@ void Ekf::fuseMag(const Vector3f &mag)
 			limitDeclination();
 		}
 	}
+
+	if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) {
+		_time_last_mag_3d_fuse = _time_last_imu;
+		return true;
+	}
+
+	return false;
 }
 
 bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
@@ -715,13 +730,15 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
 		// apply the state corrections
 		fuse(Kfusion, _heading_innov);
 
+		_time_last_heading_fuse = _time_last_imu;
+
 		return true;
 	}
 
 	return false;
 }
 
-void Ekf::fuseHeading(float measured_hdg, float obs_var)
+bool Ekf::fuseHeading(float measured_hdg, float obs_var)
 {
 	// observation variance
 	float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
@@ -769,14 +786,14 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
 	}
 
 	if (shouldUse321RotationSequence(_R_to_earth)) {
-		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
+		return fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
 
 	} else {
-		fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
+		return fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
 	}
 }
 
-void Ekf::fuseDeclination(float decl_sigma)
+bool Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
 	const float &magN = _state.mag_I(0);
@@ -791,7 +808,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 	// Calculate intermediate variables
 	if (fabsf(magN) < sq(N_field_min)) {
 		// calculation is badly conditioned close to +-90 deg declination
-		return;
+		return false;
 	}
 
 	const float HK0 = ecl::powf(magN, -2);
@@ -810,7 +827,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 		HK9 = HK4/innovation_variance;
 	} else {
 		// variance calculation is badly conditioned
-		return;
+		return false;
 	}
 
 	// Calculate the observation Jacobian
@@ -843,6 +860,8 @@ void Ekf::fuseDeclination(float decl_sigma)
 	if (is_fused) {
 		limitDeclination();
 	}
+
+	return is_fused;
 }
 
 void Ekf::limitDeclination()