Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

commander: mag calibration tolerate fit failure if sensor disabled

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+23 -8
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); worker_data.calibration_counter_total[cur_mag], sphere_data, true);
if (ellipsoid_ret == PX4_OK) { 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; sphere_radius[cur_mag] = sphere_data.radius;
for (int i = 0; i < 3; i++) { 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); 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), result = check_calibration_result(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2),
sphere_radius[cur_mag], sphere_radius[cur_mag],
diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2), 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); mavlink_log_pub, cur_mag);
if (result == calibrate_return_error) { 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();
}
} }
} }
} }