Commander: Add range check to mag cal

src/modules/commander/mag_calibration.cpp

@@ -73,6 +73,8 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
+static constexpr float MAG_MAX_OFFSET_LEN = 0.6f;	///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions.
+
 int32_t	device_ids[max_mags];
 int device_prio_max = 0;
 int32_t device_id_primary = 0;
@@ -507,6 +509,13 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
 					result = calibrate_return_error;
 				}
+
+				if (sqrtf(sphere_x[cur_mag] * sphere_x[cur_mag] +
+					sphere_y[cur_mag] * sphere_y[cur_mag] + sphere_z[cur_mag] * sphere_z[cur_mag])
+					> MAG_MAX_OFFSET_LEN) {
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Excessive offset for mag #%u", cur_mag);
+					result = calibrate_return_error;
+				}
 			}
 		}
 	}