Skip to content

feat(mag_calibration): always use WMM if possible #26912

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
bresch wants to merge 2 commits into from
Closed

feat(mag_calibration): always use WMM if possible #26912

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5d766d1
chore: return source of expected mag strength for mag cal
bresch Mar 30, 2026
3532116
feat(mag cal): override sphere radius with WMM if available
bresch Mar 30, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 27 additions & 14 deletions src/modules/commander/mag_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,23 +227,30 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
return calibrate_return_ok;
}

static float get_sphere_radius()
enum class SphereRadiusSource { WMM, DEFAULT };

struct SphereRadius {
float value;
SphereRadiusSource source;
};

static SphereRadius get_sphere_radius()
{
// if GPS is available use real field intensity from world magnetic model
// if GNSS is available use real field intensity from the World Magnetic Model (WMM)
uORB::SubscriptionMultiArray<sensor_gps_s, 3> gps_subs{ORB_ID::vehicle_gps_position};

for (auto &gps_sub : gps_subs) {
sensor_gps_s gps;

if (gps_sub.copy(&gps)) {
if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
// magnetic field data returned by the geo library using the current GPS position
return get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg);
// magnetic field data returned by the geo library using the current GNSS position (WMM)
return {get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg), SphereRadiusSource::WMM};
}
}
}

return MAG_SPHERE_RADIUS_DEFAULT;
return {MAG_SPHERE_RADIUS_DEFAULT, SphereRadiusSource::DEFAULT};
}

static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data)
Expand All @@ -253,7 +260,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta

mag_worker_data_t *worker_data = (mag_worker_data_t *)(data);

float mag_sphere_radius = get_sphere_radius();
const SphereRadius mag_sphere_radius = get_sphere_radius();

// notify user to start rotating
set_tune(tune_control_s::TUNE_ID_SINGLE_BEEP);
Expand Down Expand Up @@ -386,7 +393,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
worker_data->calibration_counter_total[cur_mag],
worker_data->calibration_sides * worker_data->calibration_points_perside,
mag_sphere_radius);
mag_sphere_radius.value);

if (!reject) {
new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
Expand Down Expand Up @@ -615,10 +622,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
Vector3f offdiag[MAX_MAGS];
float sphere_radius[MAX_MAGS];

const float mag_sphere_radius = get_sphere_radius();
const SphereRadius mag_sphere_radius = get_sphere_radius();

for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
sphere_radius[cur_mag] = mag_sphere_radius;
sphere_radius[cur_mag] = mag_sphere_radius.value;
sphere[cur_mag].zero();
diag[cur_mag] = Vector3f{1.f, 1.f, 1.f};
offdiag[cur_mag].zero();
Expand All @@ -630,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));
Expand All @@ -650,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);

Expand Down
Loading