Mag cal: automatically disable internal mags if external ones are available

src/lib/sensor_calibration/Magnetometer.hpp

@@ -85,6 +85,7 @@ class Magnetometer
 	int8_t calibration_index() const { return _calibration_index; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
+	void disable() { _priority = 0; }
 	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
 	const int32_t &priority() const { return _priority; }

src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp

@@ -47,9 +47,6 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter
 	int num_enabled_and_valid_calibration = 0;
 
 	for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) {
-		bool is_mag_fault = false;
-		const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault);
-
 		const bool exists = _sensor_mag_sub[instance].advertised();
 		bool is_valid = false;
 		bool is_calibration_valid = false;
@@ -80,7 +77,9 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter
 		}
 
 		// Do not raise errors if a mag is not required
-		if (!is_required) {
+		bool is_mag_fault = false;
+
+		if (!isMagRequired(instance, is_mag_fault)) {
 			continue;
 		}
 

src/modules/commander/mag_calibration.cpp

@@ -903,6 +903,15 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		bool param_save = false;
 		bool failed = true;
 
+		bool external_mag_available = false;
+
+		for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
+			if (worker_data.calibration[cur_mag].external() && worker_data.calibration[cur_mag].enabled()) {
+				external_mag_available = true;
+				break;
+			}
+		}
+
 		for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 
 			calibration::Magnetometer &current_cal = worker_data.calibration[cur_mag];
@@ -921,6 +930,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 					current_cal.set_offdiagonal(offdiag[cur_mag]);
 				}
 
+				if (external_mag_available && !current_cal.external()) {
+					// automatically disable the internal mags as they should not be used for navigation
+					current_cal.disable();
+				}
+
 				current_cal.PrintStatus();
 
 				if (current_cal.ParametersSave(cur_mag, true)) {

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -209,6 +209,19 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 		if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
 			bool parameters_notify = false;
 
+			bool external_mag_available = false;
+
+			for (unsigned mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+				if (_calibration[mag_index].external()
+				    && _calibration[mag_index].enabled()
+				    && mag_bias_est.valid[mag_index]
+				    && mag_bias_est.stable[mag_index]) {
+
+					external_mag_available = true;
+					break;
+				}
+			}
+
 			for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
 				if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) {
 
@@ -228,6 +241,11 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 						const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
 
 						if (_calibration[mag_index].set_offset(offset)) {
+							if (external_mag_available && !_calibration[mag_index].external()) {
+								// automatically disable the internal mags as they should not be used for navigation
+								_calibration[mag_index].disable();
+							}
+
 							// save parameters with preferred calibration slot to current sensor index
 							_calibration[mag_index].ParametersSave(mag_index);