fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 15:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
bresch
2025-02-14 16:12:20 +01:00
committed by Mathieu Bresciani
parent 430be08131
commit e3fd50667d
3 changed files with 33 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+18
View File
@@ -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);