ekf2: push mag cal reset to delayed time horizon

src/modules/ekf2/EKF/common.h

@@ -205,6 +205,7 @@ struct gpsSample {
 struct magSample {
 	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
 	Vector3f    mag{};      ///< NED magnetometer body frame measurements (Gauss)
+	bool        reset{false}; ///< magnetometer changed (different sensor or calibration change)
 };
 
 struct baroSample {

src/modules/ekf2/EKF/ekf.h

@@ -239,10 +239,6 @@ public:
 	void resetGyroBias();
 	void resetAccelBias();
 
-	// Reset all magnetometer bias states and covariances to initial alignment values.
-	// Requests full mag yaw reset (if using mag)
-	void resetMagBiasAndYaw();
-
 	Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
 
 	Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); }

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -735,27 +735,6 @@ void Ekf::resetAccelBias()
 	_prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag();
 }
 
-void Ekf::resetMagBiasAndYaw()
-{
-	// Zero the magnetometer bias states
-	_state.mag_B.zero();
-
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	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.zero();
-
-	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		_mag_yaw_reset_req = true;
-	}
-
-	_control_status.flags.mag_fault = false;
-
-	_mag_counter = 0;
-}
-
 // get EKF innovation consistency check status information comprising of:
 // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
 // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
@@ -1112,9 +1091,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 
 void Ekf::stopMagFusion()
 {
-	stopMag3DFusion();
-	stopMagHdgFusion();
-	clearMagCov();
+	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+		ECL_INFO("stopping mag fusion");
+		stopMag3DFusion();
+		stopMagHdgFusion();
+		clearMagCov();
+	}
 }
 
 void Ekf::stopMag3DFusion()

src/modules/ekf2/EKF/mag_control.cpp

@@ -49,8 +49,28 @@ void Ekf::controlMagFusion()
 		mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample);
 
 		if (mag_data_ready) {
-			_mag_lpf.update(mag_sample.mag);
-			_mag_counter++;
+
+			// sensor or calibration has changed, clear any mag bias and reset low pass filter
+			if (mag_sample.reset) {
+				// Zero the magnetometer bias states
+				_state.mag_B.zero();
+
+				// Zero the corresponding covariances and set
+				// variances to the values use for initial alignment
+				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.zero();
+
+				_control_status.flags.mag_fault = false;
+
+				_mag_lpf.reset(mag_sample.mag);
+				_mag_counter = 1;
+
+			} else {
+				_mag_lpf.update(mag_sample.mag);
+				_mag_counter++;
+			}
 
 			// 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.
@@ -134,7 +154,7 @@ void Ekf::controlMagFusion()
 
 		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
 
-		if (!mag_enabled_previously && mag_enabled) {
+		if ((!mag_enabled_previously && mag_enabled) || mag_sample.reset) {
 			_mag_yaw_reset_req = true;
 		}
 

src/modules/ekf2/EKF2.cpp

@@ -1972,7 +1972,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 		// check if magnetometer has changed
 		if (magnetometer.device_id != _device_id_mag) {
 			if (_device_id_mag != 0) {
-				PX4_WARN("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id);
+				PX4_DEBUG("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id);
 			}
 
 			reset = true;
@@ -1984,7 +1984,6 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 
 		if (reset) {
-			_ekf.resetMagBiasAndYaw();
 			_device_id_mag = magnetometer.device_id;
 			_mag_calibration_count = magnetometer.calibration_count;
 
@@ -1992,7 +1991,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 			_mag_cal = {};
 		}
 
-		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
+		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
 
 		ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);