commander: mag calibration use expected WMM sphere radius to reject samples if GPS is available

src/modules/commander/mag_calibration.cpp

@@ -57,6 +57,7 @@
 #include <lib/systemlib/err.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionBlocking.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -135,9 +136,9 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 }
 
 static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count,
-			  unsigned max_count)
+			  unsigned max_count, float mag_sphere_radius)
 {
-	float min_sample_dist = fabsf(5.4f * MAG_SPHERE_RADIUS_DEFAULT / sqrtf(max_count)) / 3.0f;
+	float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
 
 	for (size_t i = 0; i < count; i++) {
 		float dx = sx - x[i];
@@ -222,6 +223,28 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
 	return calibrate_return_ok;
 }
 
+static float get_sphere_radius()
+{
+	// if GPS is available use real field intensity from world magnetic model
+	uORB::SubscriptionMultiArray<vehicle_gps_position_s> gps_subs{ORB_ID::vehicle_gps_position};
+
+	for (auto &gps_sub : gps_subs) {
+		vehicle_gps_position_s gps;
+
+		if (gps_sub.copy(&gps)) {
+			if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
+				const double lat = gps.lat / 1.e7;
+				const double lon = gps.lon / 1.e7;
+
+				// magnetic field data returned by the geo library using the current GPS position
+				return get_mag_strength_gauss(lat, lon);
+			}
+		}
+	}
+
+	return MAG_SPHERE_RADIUS_DEFAULT;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data)
 {
 	const hrt_abstime calibration_started = hrt_absolute_time();
@@ -229,6 +252,8 @@ 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();
+
 	// notify user to start rotating
 	set_tune(TONE_SINGLE_BEEP_TUNE);
 
@@ -334,7 +359,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 						bool reject = reject_sample(mag.x, mag.y, mag.z,
 									    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);
+									    worker_data->calibration_sides * worker_data->calibration_points_perside,
+									    mag_sphere_radius);
 
 						if (!reject) {
 							new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
@@ -487,33 +513,16 @@ 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();
+
 	for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
-		sphere_radius[cur_mag] = MAG_SPHERE_RADIUS_DEFAULT;
+		sphere_radius[cur_mag] = mag_sphere_radius;
 		sphere[cur_mag].zero();
 		diag[cur_mag] = Vector3f{1.f, 1.f, 1.f};
 		offdiag[cur_mag].zero();
 	}
 
 	if (result == calibrate_return_ok) {
-
-		// if GPS is available use real field intensity from world magnetic model
-		uORB::SubscriptionData<vehicle_gps_position_s> gps_sub{ORB_ID(vehicle_gps_position)};
-		const vehicle_gps_position_s &gps = gps_sub.get();
-
-		if (hrt_elapsed_time(&gps.timestamp) < 10_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
-			const double lat = gps.lat / 1.e7;
-			const double lon = gps.lon / 1.e7;
-
-			// magnetic field data returned by the geo library using the current GPS position
-			const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
-
-			PX4_INFO("[cal] using current GPS for field strength: %.4f Gauss", (double)mag_strength_gps);
-
-			for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
-				sphere_radius[cur_mag] = mag_strength_gps;
-			}
-		}
-
 		// Sphere fit the data to get calibration values
 		for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 			if (worker_data.calibration[cur_mag].device_id() != 0) {