fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag_calibration: Make sure calibration fails if any mag fails; More checks on calibration results

Browse Source
This commit is contained in:
pixhawk 2017-01-27 14:28:26 +01:00 committed by Lorenz Meier
parent fded02220b
commit ef2a5599a1
1 changed files with 59 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
src/modules/commander/mag_calibration.cpp
View File

@ -277,6 +277,55 @@ static unsigned progress_percentage(mag_worker_data_t *worker_data)
return 100 * ((float)worker_data->done_count) / calibration_sides;
}
// Returns calibrate_return_error if any parameter is not finite
// Logs if parameters are out of range
static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z,
float sphere_radius,
float diag_x, float diag_y, float diag_z,
float offdiag_x, float offdiag_y, float offdiag_z,
orb_advert_t *mavlink_log_pub, size_t cur_mag)
{
float must_be_finite[] = {offset_x, offset_y, offset_z,
sphere_radius,
diag_x, diag_y, diag_z,
offdiag_x, offdiag_y, offdiag_z};
float should_be_not_huge[] = {offset_x, offset_y, offset_z};
float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
// Make sure every parameter is finite
const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
for (unsigned i = 0; i < num_finite; ++i) {
if (!PX4_ISFINITE(must_be_finite[i])) {
calibration_log_emergency(mavlink_log_pub,
"ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
return calibrate_return_error;
}
}
// Notify if offsets are too large
const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
for (unsigned i = 0; i < num_not_huge; ++i) {
if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
// Notify if a parameter which should be positive is non-positive
const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
for (unsigned i = 0; i < num_positive; ++i) {
if (should_be_positive[i] <= 0.0f) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
return calibrate_return_ok;
}
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
{
calibrate_return result = calibrate_return_ok;
@ -632,26 +681,18 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag], &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag], &offdiag_x[cur_mag], &offdiag_y[cur_mag],
&offdiag_z[cur_mag]);
&sphere_radius[cur_mag],
&diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag],
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
}
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
sphere_radius[cur_mag],
diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
mavlink_log_pub, cur_mag);
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
calibration_log_info(mavlink_log_pub, "Scale: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)diag_x[cur_mag],
(double)diag_y[cur_mag], (double)diag_z[cur_mag], cur_mag);
calibration_log_info(mavlink_log_pub, "offdiag: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)offdiag_x[cur_mag],
(double)offdiag_y[cur_mag], (double)offdiag_z[cur_mag], cur_mag);
result = calibrate_return_ok;
if (result == calibrate_return_error) {
break;
}
}
}