Mag cal: allow 6, 3 and 2 side calibrations (and anything in-between with bitfields)

src/modules/commander/mag_calibration.cpp

@@ -69,11 +69,11 @@ static const int ERROR = -1;
 static const char *sensor_name = "mag";
 static constexpr unsigned max_mags = 3;
 static constexpr float mag_sphere_radius = 0.2f;
-static constexpr unsigned int calibration_sides = 3;			///< The total number of sides
+static unsigned int calibration_sides = 6;			///< The total number of sides
 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.75f;	///< 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.
+static constexpr float MAG_MAX_OFFSET_LEN = 1.3f;	///< The maximum measurement range is ~1.9 Ga, the earth field is ~0.6 Ga, so an offset larger than ~1.3 Ga means the mag will saturate in some directions.
 
 int32_t	device_ids[max_mags];
 bool internal[max_mags];
@@ -447,40 +447,32 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 	worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
 
-	// Collect: Right-side up, Left Side, Nose down
+	// Collect: As defined by configuration
-	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
+	// start with a full mask, all six bits set
-	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
+	uint32_t cal_mask = (1 << 6) - 1;
-	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
+	param_get(param_find("CAL_MAG_SIDES"), &cal_mask);
-	worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
-	worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
-	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
 
-	calibration_log_info(mavlink_log_pub,
+	calibration_sides = 0;
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_TAIL_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_UPSIDE_DOWN));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
-	usleep(100000);
-	calibration_log_info(mavlink_log_pub,
-		"[cal] %s side done, rotate to a different side",
-		detect_orientation_str(DETECT_ORIENTATION_RIGHT));
-	usleep(100000);
 
-	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+	for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) /
+		sizeof(worker_data.side_data_collected[0])); i++) {
+
+		if ((cal_mask & (1 << i)) > 0) {
+			// mark as missing
+			worker_data.side_data_collected[i] = false;
+			calibration_sides++;
+		} else {
+			// mark as completed from the beginning
+			worker_data.side_data_collected[i] = true;
+
+			calibration_log_info(mavlink_log_pub,
+				"[cal] %s side done, rotate to a different side",
+				detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
+			usleep(100000);
+		}
+	}
+
+	for (size_t cur_mag = 0; cur_mag<max_mags; cur_mag++) {
 		// Initialize to no subscription
 		worker_data.sub_mag[cur_mag] = -1;
 
@@ -606,8 +598,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 				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_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
+					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, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
+					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);
 					result = calibrate_return_ok;
 				}

src/modules/sensors/sensor_params.c

@@ -687,6 +687,25 @@ PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
  */
 PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
 
+/**
+ * Bitfield selecting mag sides for calibration
+ *
+ * DETECT_ORIENTATION_TAIL_DOWN = 1
+ * DETECT_ORIENTATION_NOSE_DOWN = 2
+ * DETECT_ORIENTATION_LEFT = 4
+ * DETECT_ORIENTATION_RIGHT = 8
+ * DETECT_ORIENTATION_UPSIDE_DOWN = 16
+ * DETECT_ORIENTATION_RIGHTSIDE_UP = 32
+ *
+ * @min 34
+ * @max 63
+ * @value 34 Two side calibration
+ * @value 38 Three side calibration
+ * @value 63 Six side calibration
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63);
+
 /**
  * Primary baro ID
  *

src/modules/sensors/sensors.cpp

@@ -672,6 +672,7 @@ Sensors::Sensors() :
 	(void)param_find("CAL_MAG0_ROT");
 	(void)param_find("CAL_MAG1_ROT");
 	(void)param_find("CAL_MAG2_ROT");
+	(void)param_find("CAL_MAG_SIDES");
 	(void)param_find("SYS_PARAM_VER");
 	(void)param_find("SYS_AUTOSTART");
 	(void)param_find("SYS_AUTOCONFIG");