ekf2: return saved mag bias variance when not in 3d magnetometer fusion

- the estimated mag bias was requiring > 30 seconds of continuous 3d
mag fusion to be reported stable (and saved back to mag cal), this
restores the original intent requiring 30 seconds of accumulated valid
3d mag fusion

src/modules/ekf2/EKF/ekf.h

@@ -246,7 +246,15 @@ public:
 
 	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
 	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
-	Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; }
+
+	Vector3f getMagBiasVariance() const
+	{
+		if (_control_status.flags.mag_3D) {
+			return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
+		}
+
+		return _saved_mag_bf_variance;
+	}
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
 
@@ -518,8 +526,9 @@ private:
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
-	float _saved_mag_bf_variance[4] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
-	Matrix2f _saved_mag_ef_covmat{};		///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
+	Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
+	Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
+	float _saved_mag_ef_d_variance{};   ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -879,10 +879,7 @@ void Ekf::resetMagBias()
 	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
 	// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
-	// _saved_mag_bf_variance[0] is the the D earth axis
-	_saved_mag_bf_variance[1] = 0;
-	_saved_mag_bf_variance[2] = 0;
-	_saved_mag_bf_variance[3] = 0;
+	_saved_mag_bf_variance.zero();
 }
 
 // get EKF innovation consistency check status information comprising of:
@@ -1487,24 +1484,30 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 // save covariance data for re-use when auto-switching between heading and 3-axis fusion
 void Ekf::saveMagCovData()
 {
-	// save variances for the D earth axis and XYZ body axis field
-	for (uint8_t index = 0; index <= 3; index ++) {
-		_saved_mag_bf_variance[index] = P(index + 18, index + 18);
-	}
+	// save variances for XYZ body axis field
+	_saved_mag_bf_variance(0) = P(19, 19);
+	_saved_mag_bf_variance(1) = P(20, 20);
+	_saved_mag_bf_variance(2) = P(21, 21);
 
 	// save the NE axis covariance sub-matrix
-	_saved_mag_ef_covmat = P.slice<2, 2>(16, 16);
+	_saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16);
+
+	// save variance for the D earth axis
+	_saved_mag_ef_d_variance = P(18, 18);
 }
 
 void Ekf::loadMagCovData()
 {
-	// re-instate variances for the D earth axis and XYZ body axis field
-	for (uint8_t index = 0; index <= 3; index ++) {
-		P(index + 18, index + 18) = _saved_mag_bf_variance[index];
-	}
+	// re-instate variances for the XYZ body axis field
+	P(19, 19) = _saved_mag_bf_variance(0);
+	P(20, 20) = _saved_mag_bf_variance(1);
+	P(21, 21) = _saved_mag_bf_variance(2);
 
 	// re-instate the NE axis covariance sub-matrix
-	P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
+	P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat;
+
+	// re-instate the D earth axis variance
+	P(18, 18) = _saved_mag_ef_d_variance;
 }
 
 void Ekf::startAirspeedFusion()