Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

commander: mag calibration improve console and debug output

Browse Source
This commit is contained in:
Daniel Agar
2020-08-17 14:25:56 -04:00
parent 2bb04f2261
commit b2e8f6839f
1 changed files with 11 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+11 -9
View File
@@ -339,9 +339,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
rejected = true; rejected = true;
} else { } else {
new_samples[cur_mag](0) = mag.x; new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
new_samples[cur_mag](1) = mag.y;
new_samples[cur_mag](2) = mag.z;
} }
} else { } else {
@@ -537,11 +535,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
&diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2), &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2),
&offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2)); &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2));
PX4_DEBUG("Mag: %d (%d/%d) sphere fit ret=%d, fitness: %.4f, lambda: %.4f", cur_mag, i, max_iterations, ret, PX4_DEBUG("Mag: %d (%d/%d) sphere fit ret=%d, fitness: %.5f, lambda: %.5f, radius: %.3f, offset: [%.3f, %.3f %.3f]",
(double)fitness, (double)sphere_lambda); cur_mag, i, max_iterations, ret, (double)fitness, (double)sphere_lambda, (double)sphere_radius[cur_mag],
(double)sphere[cur_mag](0), (double)sphere[cur_mag](1), (double)sphere[cur_mag](2));
} }
PX4_INFO("Mag: %d sphere fitness: %.4f radius: %.4f", cur_mag, (double)fitness, (double)sphere_radius[cur_mag]); PX4_INFO("Mag: %d sphere fitness: %.5f radius: %.4f", cur_mag, (double)fitness, (double)sphere_radius[cur_mag]);
} }
if (!sphere_fit_only) { if (!sphere_fit_only) {
@@ -557,11 +556,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
&diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2), &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2),
&offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2)); &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2));
PX4_DEBUG("Mag: %d (%d/%d) ellipsoid fit ret=%d, fitness: %.4f, lambda: %.4f", cur_mag, i, max_iterations, ret, PX4_DEBUG("Mag: %d (%d/%d) ellipsoid fit ret=%d, fitness: %.5f, lambda: %.5f, radius: %.3f, offset: [%.3f, %.3f %.3f]",
(double)fitness, (double)ellipsoid_lambda); cur_mag, i, max_iterations, ret, (double)fitness, (double)ellipsoid_lambda, (double)sphere_radius[cur_mag],
(double)sphere[cur_mag](0), (double)sphere[cur_mag](1), (double)sphere[cur_mag](2));
} }
PX4_INFO("Mag: %d ellipsoid fitness: %.4f", cur_mag, (double)fitness); PX4_INFO("Mag: %d ellipsoid fitness: %.5f", cur_mag, (double)fitness);
} }
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),
@@ -784,6 +784,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
current_cal.set_offdiagonal(offdiag[cur_mag]); current_cal.set_offdiagonal(offdiag[cur_mag]);
} }
current_cal.PrintStatus();
} else { } else {
current_cal.Reset(); current_cal.Reset();
} }