mag_calibration: Make sure calibration fails if any mag fails; More checks on calibration results

src/modules/commander/mag_calibration.cpp

@@ -277,6 +277,55 @@ static unsigned progress_percentage(mag_worker_data_t *worker_data)
 	return 100 * ((float)worker_data->done_count) / calibration_sides;
 }
 
+// Returns calibrate_return_error if any parameter is not finite
+// Logs if parameters are out of range
+static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z,
+				float sphere_radius,
+				float diag_x, float diag_y, float diag_z,
+				float offdiag_x, float offdiag_y, float offdiag_z,
+				orb_advert_t *mavlink_log_pub, size_t cur_mag)
+{
+	float must_be_finite[] = {offset_x, offset_y, offset_z,
+							  sphere_radius,
+							  diag_x, diag_y, diag_z,
+							  offdiag_x, offdiag_y, offdiag_z};
+
+	float should_be_not_huge[] = {offset_x, offset_y, offset_z};
+	float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
+
+	// Make sure every parameter is finite
+	const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
+	for (unsigned i = 0; i < num_finite; ++i) {
+		if (!PX4_ISFINITE(must_be_finite[i])) {
+			calibration_log_emergency(mavlink_log_pub,
+							"ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
+			return calibrate_return_error;
+		}
+	}
+
+	// Notify if offsets are too large
+	const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
+	for (unsigned i = 0; i < num_not_huge; ++i) {
+		if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
+			calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
+							(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
+			break;
+		}
+	}
+
+	// Notify if a parameter which should be positive is non-positive
+	const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
+	for (unsigned i = 0; i < num_positive; ++i) {
+		if (should_be_positive[i] <= 0.0f) {
+			calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale",
+							(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
+			break;
+		}
+	}
+
+	return calibrate_return_ok;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
 {
 	calibrate_return result = calibrate_return_ok;
@@ -632,26 +681,18 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 							    worker_data.calibration_counter_total[cur_mag],
 							    100, 0.0f,
 							    &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
-							    &sphere_radius[cur_mag], &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag], &offdiag_x[cur_mag], &offdiag_y[cur_mag],
-							    &offdiag_z[cur_mag]);
-
-				if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
-					calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
-					result = calibrate_return_error;
-				}
-
-				if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
-				    fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
-				    fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
-					calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
-								 (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
-					calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
-							     (double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
-					calibration_log_info(mavlink_log_pub, "Scale: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)diag_x[cur_mag],
-							     (double)diag_y[cur_mag], (double)diag_z[cur_mag], cur_mag);
-					calibration_log_info(mavlink_log_pub, "offdiag: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)offdiag_x[cur_mag],
-							     (double)offdiag_y[cur_mag], (double)offdiag_z[cur_mag], cur_mag);
-					result = calibrate_return_ok;
+							    &sphere_radius[cur_mag],
+							    &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag],
+							    &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
+
+				result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
+							    sphere_radius[cur_mag],
+							    diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
+							    offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
+							    mavlink_log_pub, cur_mag);
+
+				if (result == calibrate_return_error) {
+					break;
 				}
 			}
 		}