ekf2: add mag fusion timestamps

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,20 +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));
 
-		const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f);
+		bool rejected = (_mag_test_ratio(index) > 1.f);
 
-		if (innov_check_fail) {
-			all_innovation_checks_passed = false;
+		switch (index) {
+		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 (index == 0) {
-			_innov_check_fail_status.flags.reject_mag_x = innov_check_fail;
-
-		} else if (index == 1) {
-			_innov_check_fail_status.flags.reject_mag_y = innov_check_fail;
-
-		} else {
-			_innov_check_fail_status.flags.reject_mag_z = innov_check_fail;
+		if (rejected) {
+			all_innovation_checks_passed = false;
 		}
 	}
 
@@ -196,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;
@@ -284,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);
 
@@ -364,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);
@@ -427,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;
 }
 
 // update quaternion states and covariances using the yaw innovation and yaw observation variance
@@ -678,7 +685,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
 	return false;
 }
 
-void Ekf::fuseDeclination(float decl_sigma)
+bool Ekf::fuseDeclination(float decl_sigma)
 {
 	// assign intermediate state variables
 	const float magN = _state.mag_I(0);
@@ -693,7 +700,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);
@@ -712,7 +719,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 		HK9 = HK4/innovation_variance;
 	} else {
 		// variance calculation is badly conditioned
-		return;
+		return false;
 	}
 
 	// Calculate the observation Jacobian
@@ -745,6 +752,8 @@ void Ekf::fuseDeclination(float decl_sigma)
 	if (is_fused) {
 		limitDeclination();
 	}
+
+	return is_fused;
 }
 
 void Ekf::limitDeclination()