dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensors/vehicle_magnetometer: add mag calibration PX4_DEBUG output

This commit is contained in:
Daniel Agar 2023-07-26 12:30:12 -04:00
parent d65ddbf810
commit 82962acd5c
1 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -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));
}
}
}