diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 8d328e7a7d11..5f9dd72d4368 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/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; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 6716a8e06ad2..ac35a6180325 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/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; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a72f0fcadb9c..7aeadeab5511 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/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 ¤t_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)) { diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index df90ffdf9006..a0ba14bcfdbb 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/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);