diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 0021f9f3b9..6bb7c5ef5b 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -275,10 +275,10 @@ void VehicleMagnetometer::UpdateMagCalibration() const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance}; const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) - && (estimator_sensor_bias.mag_device_id != 0) && - estimator_sensor_bias.mag_bias_valid && - estimator_sensor_bias.mag_bias_stable && - (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); + && (estimator_sensor_bias.mag_device_id != 0) + && estimator_sensor_bias.mag_bias_valid + && estimator_sensor_bias.mag_bias_stable + && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); if (valid) { // find corresponding mag calibration @@ -288,8 +288,20 @@ void VehicleMagnetometer::UpdateMagCalibration() _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; // readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias) - _mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]); + const Vector3f mag_cal_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + + _calibration_estimator_bias[mag_index]); + if ((mag_cal_offset - _mag_cal[i].offset).longerThan(0.001f)) { + const Vector3f mag_cal_orig{_calibration[mag_index].offset()}; + + PX4_DEBUG("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])", + mag_index, _calibration[mag_index].device_id(), i, + (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), + (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), + (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); + } + + _mag_cal[i].offset = mag_cal_offset; _mag_cal[i].variance = bias_variance; _in_flight_mag_cal_available = true; @@ -340,6 +352,14 @@ void VehicleMagnetometer::UpdateMagCalibration() _calibration_estimator_bias[mag_index].zero(); calibration_param_save_needed = true; + + } else { + // new offset not saved + PX4_DEBUG("%d (%" PRIu32 ") EST:%d rejected: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (full [%.3f, %.3f, %.3f])", + mag_index, _calibration[mag_index].device_id(), i, + (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), + (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), + (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); } } }