Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-09 22:08:56 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

feat(mag cal): override sphere radius with WMM if available

Browse Source
This commit is contained in:
bresch
2026-03-30 15:54:25 +02:00
parent 5d766d14ef
commit 3532116ed5
1 changed files with 11 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+11 -5
View File
@@ -637,11 +637,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
if (worker_data.calibration[cur_mag].device_id() != 0) {
// Mag in this slot is available and we should have values for it to calibrate
// Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained
// enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration
// is already close)
bool sphere_fit_only = worker_data.calibration_sides <= 2;
sphere_params sphere_data;
sphere_data.radius = sphere_radius[cur_mag];
sphere_data.offset = matrix::Vector3f(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2));
@@ -657,7 +652,18 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
sphere_fit_success = true;
PX4_INFO("Mag: %" PRIu8 " sphere radius: %.4f", cur_mag, (double)sphere_data.radius);
// Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained
// enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration
// is already close)
const bool sphere_fit_only = worker_data.calibration_sides <= 2;
if (!sphere_fit_only) {
if (mag_sphere_radius.source == SphereRadiusSource::WMM) {
// Use WMM instead of estimated radius as this lead to better
// estimates of true scale factors
sphere_data.radius = mag_sphere_radius.value;
}
int ellipsoid_ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total[cur_mag], sphere_data, true);