From 980f696023a577639fc5b2bcbd1e0f9240e7052f Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 8 Nov 2021 14:20:26 -0500
Subject: [PATCH] commander: mag calibration tolerate fit failure if sensor
 disabled

---
 src/modules/commander/mag_calibration.cpp | 31 +++++++++++++++++------
 1 file changed, 23 insertions(+), 8 deletions(-)

diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 6166e5e787..7255581ff3 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -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);
 
 						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;
 
 				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);
 				}
 
-				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),
 								  sphere_radius[cur_mag],
 								  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);
 
 				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();
+					}
 				}
 			}
 		}