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

commander: mag calibration tolerate fit failure if sensor disabled

This commit is contained in:
Daniel Agar 2021-11-08 14:20:26 -05:00
parent 89374b2e1d
commit 980f696023
1 changed files with 23 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -604,11 +604,23 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
worker_data.calibration_counter_total[cur_mag], sphere_data, true);
if (ellipsoid_ret == PX4_OK) {
ellipsoid_fit_success = true;
ellipsoid_fit_success = true;
}
}
}
if (!sphere_fit_success && !ellipsoid_fit_success) {
if (worker_data.calibration[cur_mag].enabled()) {
calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag);
result = calibrate_return_error;
break;
} else {
calibration_log_info(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag);
continue;
}
}
sphere_radius[cur_mag] = sphere_data.radius;
for (int i = 0; i < 3; i++) {
@ -617,12 +629,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
offdiag[cur_mag](i) = sphere_data.offdiag(i);
}
if (!sphere_fit_success && !ellipsoid_fit_success) {
calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag);
result = calibrate_return_error;
break;
}
result = check_calibration_result(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2),
sphere_radius[cur_mag],
diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2),
@ -630,7 +636,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
mavlink_log_pub, cur_mag);
if (result == calibrate_return_error) {
break;
// tolerate mag cal failures if this sensor is disabled
if (worker_data.calibration[cur_mag].enabled()) {
break;
} else {
// reset
sphere[cur_mag].zero();
diag[cur_mag] = Vector3f{1.f, 1.f, 1.f};
offdiag[cur_mag].zero();
}
}
}
}