New mag cal changes

- Use new calibrate_from_orientation worker routine to detect
orientaions
- Calibrate all mags at once
- Change to 3-side calibration mechanism

1 files changed, 325 insertions, 206 deletions

diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index a0aadab39..5f0754f98 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -49,6 +49,7 @@
 #include <math.h>
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_mag.h>
 #include <mavlink/mavlink_log.h>
@@ -62,283 +63,401 @@
 static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
+static const unsigned max_mags = 3;
+
+int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
+int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
+
+/// Data passed to calibration worker routine
+typedef struct  {
+	int		mavlink_fd;
+	unsigned	done_count;
+	int		sub_mag[max_mags];
+	unsigned int	calibration_points_perside;
+	unsigned int	calibration_interval_perside_seconds;
+	uint64_t	calibration_interval_perside_useconds;
+	unsigned int	calibration_counter_total;
+	bool		side_data_collected[detect_orientation_side_count];
+	float*		x[max_mags];
+	float*		y[max_mags];
+	float*		z[max_mags];
+} mag_worker_data_t;
 
-int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id);
 
 int do_mag_calibration(int mavlink_fd)
 {
-	const unsigned max_mags = 3;
-
-	int32_t device_id[max_mags];
 	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	sleep(1);
 
-	struct mag_scale mscale_null[max_mags] = {
-	{
+	struct mag_scale mscale_null = {
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
-	}
-	} ;
+	};
 
-	int res = ERROR;
+	int result = OK;
+	
+	// Determine which mags are available and reset each
 
+	int32_t	device_ids[max_mags];
 	char str[30];
 
-	unsigned calibrated_ok = 0;
-
-	for (unsigned s = 0; s < max_mags; s++) {
+	for (size_t i=0; i<max_mags; i++) {
+		device_ids[i] = 0; // signals no mag
+	}
+	
+	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+		// Reset mag id to mag not available
+		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
+		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));;
+		if (result != OK) {
+			mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
+			break;
+		}
 
-		/* erase old calibration */
-		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+		// Attempt to open mag
+		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 		int fd = open(str, O_RDONLY);
-
 		if (fd < 0) {
 			continue;
 		}
 
-		mavlink_and_console_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s);
-		sleep(3);
-
-		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+		// Get device id for this mag
+		device_ids[cur_mag] = ioctl(fd, DEVIOCGDEVICEID, 0);
 
-		/* ensure all scale fields are initialized tha same as the first struct */
-		(void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));
+		// Reset mag scale
+		result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
-		res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);
-
-		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+		if (result != OK) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
 		}
 
-		if (res == OK) {
+		if (result == OK) {
 			/* calibrate range */
-			res = ioctl(fd, MAGIOCCALIBRATE, fd);
+			result = ioctl(fd, MAGIOCCALIBRATE, fd);
 
-			if (res != OK) {
-				mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration");
+			if (result != OK) {
+				mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
 				/* this is non-fatal - mark it accordingly */
-				res = OK;
+				result = OK;
 			}
 		}
 
 		close(fd);
-
-		if (res == OK) {
-			res = calibrate_instance(mavlink_fd, s, device_id[s]);
-
-			if (res == OK) {
-				calibrated_ok++;
-			}
-		}
 	}
 
-	if (calibrated_ok) {
-
-		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
-		usleep(100000);
-		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-
+	if (result == OK) {
+		// Calibrate all mags at the same time
+		result = mag_calibrate_all(mavlink_fd, device_ids);
+	}
+	
+	if (result == OK) {
 		/* auto-save to EEPROM */
-		res = param_save_default();
-
-		if (res != OK) {
+		result = param_save_default();
+		if (result != OK) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 		}
+	}
+	
+	if (result == OK) {
+		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 	} else {
 		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 	}
 
-	return res;
+	return result;
 }
 
-int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
+int mag_calibration_worker(detect_orientation_return orientation, void* data)
 {
-	/* 45 seconds */
-	uint64_t calibration_interval = 25 * 1000 * 1000;
-
-	/* maximum 500 values */
-	const unsigned int calibration_maxcount = 240;
-	unsigned int calibration_counter;
-
-	float *x = new float[calibration_maxcount];
-	float *y = new float[calibration_maxcount];
-	float *z = new float[calibration_maxcount];
-
-	char str[30];
-	int res = OK;
+	int result = OK;
 	
-	/* allocate memory */
-	mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
+	unsigned int calibration_counter_side;
 
-	if (x == nullptr || y == nullptr || z == nullptr) {
-		mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
-
-		/* clean up */
-		if (x != nullptr) {
-			delete x;
+	mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
+	
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
+	sleep(2);
+	
+	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
+	unsigned poll_errcount = 0;
+	
+	calibration_counter_side = 0;
+	
+	while (hrt_absolute_time() < calibration_deadline &&
+	       calibration_counter_side < worker_data->calibration_points_perside) {
+		
+		// Wait clocking for new data on all mags
+		struct pollfd fds[max_mags];
+		size_t fd_count = 0;
+		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (worker_data->sub_mag[cur_mag] >= 0) {
+				fds[fd_count].fd = worker_data->sub_mag[cur_mag];
+				fds[fd_count].events = POLLIN;
+				fd_count++;
+			}
 		}
-
-		if (y != nullptr) {
-			delete y;
+		int poll_ret = poll(fds, fd_count, 1000);
+		
+		if (poll_ret > 0) {
+			for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+				if (worker_data->sub_mag[cur_mag] >= 0) {
+					struct mag_report mag;
+
+					orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
+					
+					worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
+					worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
+					worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
+					
+				}
+			}
+			
+			worker_data->calibration_counter_total++;
+			calibration_counter_side++;
+			
+			// Progress indicator for side
+			mavlink_and_console_log_info(worker_data->mavlink_fd,
+						     "%s %s side calibration: progress <%u>",
+						     sensor_name,
+						     detect_orientation_str(orientation),
+						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+		} else {
+			poll_errcount++;
 		}
-
-		if (z != nullptr) {
-			delete z;
+		
+		if (poll_errcount > worker_data->calibration_points_perside * 3) {
+			result = ERROR;
+			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			break;
 		}
-
-		res = ERROR;
-		return res;
 	}
+	
+	// Mark the opposite side as collected as well. No need to collect opposite side since it
+	// would generate similar points.
+	switch (orientation) {
+		case DETECT_ORIENTATION_TAIL_DOWN:
+			worker_data->side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = true;
+			break;
+		case DETECT_ORIENTATION_NOSE_DOWN:
+			worker_data->side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
+			break;
+		case DETECT_ORIENTATION_LEFT:
+			worker_data->side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
+			break;
+		case DETECT_ORIENTATION_RIGHT:
+			worker_data->side_data_collected[DETECT_ORIENTATION_LEFT] = true;
+			break;
+		case DETECT_ORIENTATION_UPSIDE_DOWN:
+			worker_data->side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = true;
+			break;
+		case DETECT_ORIENTATION_RIGHTSIDE_UP:
+			worker_data->side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
+			break;
+		case DETECT_ORIENTATION_ERROR:
+			warnx("Invalid orientation in mag_calibration_worker");
+			break;
+	}
+	
+	worker_data->done_count++;
+	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
+	
+	return result;
+}
 
-	if (res == OK) {
-		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
-
-		if (sub_mag < 0) {
-			mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort");
-			res = ERROR;
-		}  else {
-			struct mag_report mag;
-
-			/* limit update rate to get equally spaced measurements over time (in ms) */
-			orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-			/* calibrate offsets */
-			uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-			unsigned poll_errcount = 0;
-
-			mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
-
-			calibration_counter = 0U;
-
-			while (hrt_absolute_time() < calibration_deadline &&
-			       calibration_counter < calibration_maxcount) {
-
-				/* wait blocking for new data */
-				struct pollfd fds[1];
-				fds[0].fd = sub_mag;
-				fds[0].events = POLLIN;
-
-				int poll_ret = poll(fds, 1, 1000);
-
-				if (poll_ret > 0) {
-					orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-					x[calibration_counter] = mag.x;
-					y[calibration_counter] = mag.y;
-					z[calibration_counter] = mag.z;
-
-					calibration_counter++;
+int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
+{
+	int result = OK;
 
-					if (calibration_counter % (calibration_maxcount / 20) == 0) {
-						mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
-					}
+	mag_worker_data_t worker_data;
+	
+	worker_data.mavlink_fd = mavlink_fd;
+	worker_data.done_count = 0;
+	worker_data.calibration_counter_total = 0;
+	worker_data.calibration_points_perside = 80;
+	worker_data.calibration_interval_perside_seconds = 20;
+	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
+
+	// Initialize to collect all sides
+	for (size_t cur_side=0; cur_side<6; cur_side++) {
+		worker_data.side_data_collected[cur_side] = false;
+	}
+	
+	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		// Initialize to no subscription
+		worker_data.sub_mag[cur_mag] = -1;
+		
+		// Initialize to no memory allocated
+		worker_data.x[cur_mag] = NULL;
+		worker_data.y[cur_mag] = NULL;
+		worker_data.z[cur_mag] = NULL;
+	}
 
-				} else {
-					poll_errcount++;
-				}
+	const unsigned int calibration_sides = 3;
+	const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
+	
+	char str[30];
+	
+	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+		worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+		worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+		if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
+			mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
+			result = ERROR;
+		}
+	}
 
-				if (poll_errcount > 1000) {
-					mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
-					res = ERROR;
+	
+	// Setup subscriptions to mag sensors
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				// Mag in this slot is available
+				worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
+				if (worker_data.sub_mag[cur_mag] < 0) {
+					mavlink_and_console_log_critical(mavlink_fd, "Mag #%u not found, abort", cur_mag);
+					result = ERROR;
 					break;
 				}
 			}
-
-			close(sub_mag);
 		}
 	}
-
-	float sphere_x;
-	float sphere_y;
-	float sphere_z;
-	float sphere_radius;
-
-	if (res == OK && calibration_counter > (calibration_maxcount / 2)) {
-
-		/* sphere fit */
-		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
-		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
-
-		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
-			res = ERROR;
+	
+	// Limit update rate to get equally spaced measurements over time (in ms)
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				// Mag in this slot is available
+				unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
+				
+				//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
+				orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
+			}
 		}
+		
 	}
 
-	if (x != nullptr) {
-		delete x;
+	result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
+	
+	// Close subscriptions
+	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		if (worker_data.sub_mag[cur_mag] >= 0) {
+			close(worker_data.sub_mag[cur_mag]);
+		}
 	}
-
-	if (y != nullptr) {
-		delete y;
+	
+	// Calculate calibration values for each mag
+	
+	
+	float sphere_x[max_mags];
+	float sphere_y[max_mags];
+	float sphere_z[max_mags];
+	float sphere_radius[max_mags];
+	
+	// Sphere fit the data to get calibration values
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				// Mag in this slot is available and we should have values for it to calibrate
+				
+				sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
+							 worker_data.calibration_counter_total,
+							 100, 0.0f,
+							 &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
+							 &sphere_radius[cur_mag]);
+				
+				if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) {
+					mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
+					result = ERROR;
+				}
+			}
+		}
 	}
-
-	if (z != nullptr) {
-		delete z;
+	
+	// Data points are no longer needed
+	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+		free(worker_data.x[cur_mag]);
+		free(worker_data.y[cur_mag]);
+		free(worker_data.z[cur_mag]);
 	}
+	
+	if (result == OK) {
+		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+			if (device_ids[cur_mag] != 0) {
+				int fd_mag = -1;
+				struct mag_scale mscale;
+				
+				// Set new scale
+				
+				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
+				fd_mag = open(str, 0);
+				if (fd_mag < 0) {
+					mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
+					result = ERROR;
+				}
+				
+				if (result == OK) {
+					result = ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
+					if (result != OK) {
+						mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
+						result = ERROR;
+					}
+				}
 
-	if (res == OK) {
-		/* apply calibration and set parameters */
-		struct mag_scale mscale;
-		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
-		int fd = open(str, 0);
-		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
-
-		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
-		}
-
-		if (res == OK) {
-			mscale.x_offset = sphere_x;
-			mscale.y_offset = sphere_y;
-			mscale.z_offset = sphere_z;
-
-			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
-
-			if (res != OK) {
-				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
-			}
-		}
+				if (result == OK) {
+					mscale.x_offset = sphere_x[cur_mag];
+					mscale.y_offset = sphere_y[cur_mag];
+					mscale.z_offset = sphere_z[cur_mag];
 
-		close(fd);
+					result = ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
+					if (result != OK) {
+						mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
+						result = ERROR;
+					}
+				}
+				
+				// Mag device no longer needed
+				if (fd_mag >= 0) {
+					close(fd_mag);
+				}
 
-		if (res == OK) {
-
-			bool failed = false;
-			/* set parameters */
-			(void)sprintf(str, "CAL_MAG%u_ID", s);
-			failed |= (OK != param_set(param_find(str), &(device_id)));
-			(void)sprintf(str, "CAL_MAG%u_XOFF", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
-			(void)sprintf(str, "CAL_MAG%u_YOFF", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
-			(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
-			(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
-			(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
-			(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
-
-			if (failed) {
-				res = ERROR;
-				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+				if (result == OK) {
+					bool failed = false;
+					
+					/* set parameters */
+					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
+					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset)));
+					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
+					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
+					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
+					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
+					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
+
+					if (failed) {
+						mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
+						result = ERROR;
+					} else {
+						mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
+									     cur_mag,
+									     (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
+						mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
+									     cur_mag,
+									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
+					}
+				}
 			}
-
-			mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
 		}
-
-		mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-				 (double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-				 (double)mscale.y_scale, (double)mscale.z_scale);
 	}
 
-	return res;
+	return result;
 }