forked from Archive/PX4-Autopilot
sensors/vehicle_magnetometer: add mag calibration PX4_DEBUG output
This commit is contained in:
parent
d65ddbf810
commit
82962acd5c
|
@ -275,10 +275,10 @@ void VehicleMagnetometer::UpdateMagCalibration()
|
||||||
const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
|
const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
|
||||||
|
|
||||||
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
|
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
|
||||||
&& (estimator_sensor_bias.mag_device_id != 0) &&
|
&& (estimator_sensor_bias.mag_device_id != 0)
|
||||||
estimator_sensor_bias.mag_bias_valid &&
|
&& estimator_sensor_bias.mag_bias_valid
|
||||||
estimator_sensor_bias.mag_bias_stable &&
|
&& estimator_sensor_bias.mag_bias_stable
|
||||||
(bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
|
&& (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
|
||||||
|
|
||||||
if (valid) {
|
if (valid) {
|
||||||
// find corresponding mag calibration
|
// find corresponding mag calibration
|
||||||
|
@ -288,8 +288,20 @@ void VehicleMagnetometer::UpdateMagCalibration()
|
||||||
_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
|
_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
|
||||||
|
|
||||||
// readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias)
|
// 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;
|
_mag_cal[i].variance = bias_variance;
|
||||||
|
|
||||||
_in_flight_mag_cal_available = true;
|
_in_flight_mag_cal_available = true;
|
||||||
|
@ -340,6 +352,14 @@ void VehicleMagnetometer::UpdateMagCalibration()
|
||||||
_calibration_estimator_bias[mag_index].zero();
|
_calibration_estimator_bias[mag_index].zero();
|
||||||
|
|
||||||
calibration_param_save_needed = true;
|
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));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue