Sensor cal reworkSensorCal

- cancel support
- versioned cal messages
- better still detection
- better messaging

9 files changed, 572 insertions, 430 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 409c2ea00..d0982b341 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -152,11 +152,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
-int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
-int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
+calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
+calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
-int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
-int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
+calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
 
 /// Data passed to calibration worker routine
 typedef struct  {
@@ -171,7 +170,7 @@ int do_accel_calibration(int mavlink_fd)
 	int fd;
 	int32_t device_id[max_accel_sens];
 
-	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	struct accel_scale accel_scale = {
 		0.0f,
@@ -202,7 +201,7 @@ int do_accel_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 		}
 	}
 
@@ -210,13 +209,21 @@ int do_accel_calibration(int mavlink_fd)
 	float accel_T[max_accel_sens][3][3];
 	unsigned active_sensors;
 
+	/* measure and calculate offsets & scales */
 	if (res == OK) {
-		/* measure and calculate offsets & scales */
-		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
+		calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
+		if (cal_return == calibrate_return_cancelled) {
+			// Cancel message already displayed, nothing left to do
+			return ERROR;
+		} else if (cal_return == calibrate_return_ok) {
+			res = OK;
+		} else {
+			res = ERROR;
+		}
 	}
 
 	if (res != OK || active_sensors == 0) {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+		mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
 		return ERROR;
 	}
 
@@ -263,7 +270,7 @@ int do_accel_calibration(int mavlink_fd)
 		failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
 		
 		if (failed) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
 			return ERROR;
 		}
 
@@ -271,7 +278,7 @@ int do_accel_calibration(int mavlink_fd)
 		fd = open(str, 0);
 
 		if (fd < 0) {
-			mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] sensor does not exist");
 			res = ERROR;
 		} else {
 			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -279,7 +286,7 @@ int do_accel_calibration(int mavlink_fd)
 		}
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
 		}
 	}
 
@@ -288,41 +295,44 @@ int do_accel_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 
-		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 
 	} else {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
+	
+	// This give a chance for the log messages to go out of the queue before someone else stomps on then
+	sleep(1);
 
 	return res;
 }
 
-int accel_calibration_worker(detect_orientation_return orientation, void* data)
+static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	const unsigned samples_num = 3000;
 	accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
 	
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
 	
 	read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
 	
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
 				     (double)worker_data->accel_ref[0][orientation][0],
 				     (double)worker_data->accel_ref[0][orientation][1],
 				     (double)worker_data->accel_ref[0][orientation][2]);
 	
 	worker_data->done_count++;
-	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
+	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
 	
-	return OK;
+	return calibrate_return_ok;
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
+calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 	
 	*active_sensors = 0;
 	
@@ -343,7 +353,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 	for (unsigned i = 0; i < max_accel_sens; i++) {
 		worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
 		if (worker_data.subs[i] < 0) {
-			result = ERROR;
+			result = calibrate_return_error;
 			break;
 		}
 		
@@ -353,8 +363,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 		timestamps[i] = arp.timestamp;
 	}
 
-	if (result == OK) {
-		result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
+	if (result == calibrate_return_ok) {
+		int cancel_sub = calibrate_cancel_subscribe();
+		result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
+		calibrate_cancel_unsubscribe(cancel_sub);
 	}
 
 	/* close all subscriptions */
@@ -370,13 +382,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 		}
 	}
 
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		/* calculate offsets and transform matrix */
 		for (unsigned i = 0; i < (*active_sensors); i++) {
 			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-			if (result != OK) {
-				mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
+			if (result != calibrate_return_ok) {
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
 				break;
 			}
 		}
@@ -388,7 +400,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
+calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
 {
 	struct pollfd fds[max_accel_sens];
 
@@ -432,7 +444,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
 		}
 
 		if (errcount > samples_num / 10) {
-			return ERROR;
+			return calibrate_return_error;
 		}
 	}
 
@@ -442,7 +454,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
 		}
 	}
 
-	return OK;
+	return calibrate_return_ok;
 }
 
 int mat_invert3(float src[3][3], float dst[3][3])
@@ -468,7 +480,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
 	return OK;
 }
 
-int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
+calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
 {
 	/* calculate offsets */
 	for (unsigned i = 0; i < 3; i++) {
@@ -490,7 +502,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
 	float mat_A_inv[3][3];
 
 	if (mat_invert3(mat_A, mat_A_inv) != OK) {
-		return ERROR;
+		return calibrate_return_error;
 	}
 
 	/* copy results to accel_T */
@@ -501,5 +513,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
 		}
 	}
 
-	return OK;
+	return calibrate_return_ok;
 }
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 747d915ff..837a3f1e8 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -38,6 +38,7 @@
 
 #include "airspeed_calibration.h"
 #include "calibration_messages.h"
+#include "calibration_routines.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
@@ -61,19 +62,20 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "dpress";
 
-#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
-
 static void feedback_calibration_failed(int mavlink_fd)
 {
 	sleep(5);
-	mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-	mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
+	mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 }
 
 int do_airspeed_calibration(int mavlink_fd)
 {
+	int result = OK;
+	unsigned calibration_counter = 0;
+	const unsigned maxcount = 3000;
+
 	/* give directions */
-	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	const unsigned calibration_count = 2000;
 
@@ -96,11 +98,13 @@ int do_airspeed_calibration(int mavlink_fd)
 			paramreset_successful = true;
 
 		} else {
-			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
+			mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
 		}
 
 		close(fd);
 	}
+    
+	int cancel_sub = calibrate_cancel_subscribe();
 
 	if (!paramreset_successful) {
 
@@ -108,26 +112,26 @@ int do_airspeed_calibration(int mavlink_fd)
 		float analog_scaling = 0.0f;
 		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
 		if (fabsf(analog_scaling) < 0.1f) {
-			mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
+			goto error_return;
 		}
 
 		/* set scaling offset parameter */
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			goto error_return;
 		}
 	}
 
-	unsigned calibration_counter = 0;
-
-	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+	mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
 	usleep(500 * 1000);
 
 	while (calibration_counter < calibration_count) {
 
+		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+			goto error_return;
+		}
+        
 		/* wait blocking for new data */
 		struct pollfd fds[1];
 		fds[0].fd = diff_pres_sub;
@@ -142,14 +146,13 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
+				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
 			}
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_fd);
-			close(diff_pres_sub);
-			return ERROR;
+			goto error_return;
 		}
 	}
 
@@ -161,16 +164,15 @@ int do_airspeed_calibration(int mavlink_fd)
 		airscale.offset_pa = diff_pres_offset;
 		if (fd_scale > 0) {
 			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
+				mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
 			}
 
 			close(fd_scale);
 		}
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			goto error_return;
 		}
 
 		/* auto-save to EEPROM */
@@ -178,30 +180,31 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
-			close(diff_pres_sub);
-			return ERROR;
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+			goto error_return;
 		}
 
 	} else {
 		feedback_calibration_failed(mavlink_fd);
-		close(diff_pres_sub);
-		return ERROR;
+		goto error_return;
 	}
 
-	mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
+	mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
 
 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);
 
-	mavlink_log_critical(mavlink_fd, "Create airflow now");
+	mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
 
 	calibration_counter = 0;
-	const unsigned maxcount = 3000;
 
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
 
+        if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+		goto error_return;
+        }
+        
 		/* wait blocking for new data */
 		struct pollfd fds[1];
 		fds[0].fd = diff_pres_sub;
@@ -216,7 +219,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
 				if (calibration_counter % 500 == 0) {
-					mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
+					mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
@@ -224,30 +227,26 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
+				mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
-				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
-				close(diff_pres_sub);
+				mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
 
 				diff_pres_offset = 0.0f;
 				if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-					mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-					close(diff_pres_sub);
-					return ERROR;
+					mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+					goto error_return;
 				}
 
 				/* save */
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
+				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
 				(void)param_save_default();
 
-				close(diff_pres_sub);
-
 				feedback_calibration_failed(mavlink_fd);
-				return ERROR;
+				goto error_return;
 			} else {
-				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
+				mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}
@@ -255,21 +254,30 @@ int do_airspeed_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_fd);
-			close(diff_pres_sub);
-			return ERROR;
+			goto error_return;
 		}
 	}
 
 	if (calibration_counter == maxcount) {
 		feedback_calibration_failed(mavlink_fd);
-		close(diff_pres_sub);
-		return ERROR;
+		goto error_return;
 	}
 
-	mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+	mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 
-	mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	tune_neutral(true);
+
+normal_return:
+	calibrate_cancel_unsubscribe(cancel_sub);
 	close(diff_pres_sub);
-	return OK;
+	
+	// This give a chance for the log messages to go out of the queue before someone else stomps on then
+	sleep(1);
+	
+	return result;
+    
+error_return:
+	result = ERROR;
+	goto normal_return;
 }
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
index 29f44dc36..2f6d02a72 100644
--- a/src/modules/commander/calibration_messages.h
+++ b/src/modules/commander/calibration_messages.h
@@ -42,17 +42,25 @@
 #ifndef CALIBRATION_MESSAGES_H_
 #define CALIBRATION_MESSAGES_H_
 
-#define CAL_STARTED_MSG	"%s calibration: started"
-#define CAL_DONE_MSG	"%s calibration: done"
-#define CAL_FAILED_MSG	"%s calibration: failed"
-#define CAL_PROGRESS_MSG	"%s calibration: progress <%u>"
+// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state
+// machine to provide visual feedback for calibration. As such, the text for them or semantics of when
+// they are displayed cannot be modified without causing QGC to break. If modifications are made, make
+// sure to bump the calibration version number which will cause QGC to perform log based calibration
+// instead of visual calibration until such a time as QGC is update to the new version.
 
-#define	CAL_FAILED_UNKNOWN_ERROR	"ERROR: unknown error"
-#define CAL_FAILED_SENSOR_MSG		"ERROR: failed reading sensor"
-#define CAL_FAILED_RESET_CAL_MSG	"ERROR: failed to reset calibration, sensor %u"
-#define CAL_FAILED_APPLY_CAL_MSG	"ERROR: failed to apply calibration, sensor %u"
-#define CAL_FAILED_SET_PARAMS_MSG	"ERROR: failed to set parameters, sensor %u"
-#define CAL_FAILED_SAVE_PARAMS_MSG	"ERROR: failed to save parameters"
-#define CAL_FAILED_ORIENTATION_TIMEOUT	"ERROR: timed out waiting for correct orientation"
+// The number in the cal started message is used to indicate the version stamp for the current calibration code.
+#define CAL_QGC_STARTED_MSG			"[cal] calibration started: 1 %s"
+#define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
+#define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
+#define CAL_QGC_CANCELLED_MSG			"[cal] calibration cancelled"
+#define CAL_QGC_PROGRESS_MSG			"[cal] progress <%u>"
+#define CAL_QGC_ORIENTATION_DETECTED_MSG	"[cal] %s orientation detected"
+#define CAL_QGC_SIDE_DONE_MSG			"[cal] %s side done, rotate to a different side"
+
+#define CAL_ERROR_SENSOR_MSG		"[cal] ERROR: failed reading sensor"
+#define CAL_ERROR_RESET_CAL_MSG		"[cal] ERROR: failed to reset calibration, sensor %u"
+#define CAL_ERROR_APPLY_CAL_MSG		"[cal] ERROR: failed to apply calibration, sensor %u"
+#define CAL_ERROR_SET_PARAMS_MSG	"[cal] ERROR: failed to set parameters, sensor %u"
+#define CAL_ERROR_SAVE_PARAMS_MSG	"[cal] ERROR: failed to save parameters"
 
 #endif /* CALIBRATION_MESSAGES_H_ */
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 192b8c727..a320c5c5b 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -48,6 +48,8 @@
 #include <geo/geo.h>
 #include <string.h>
 
+#include <uORB/topics/vehicle_command.h>
+
 #include "calibration_routines.h"
 #include "calibration_messages.h"
 #include "commander_helper.h"
@@ -230,23 +232,19 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 	return 0;
 }
 
-enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
+enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position)
 {
 	const unsigned ndim = 3;
 	
 	struct accel_report sensor;
-	/* exponential moving average of accel */
-	float accel_ema[ndim] = { 0.0f };
-	/* max-hold dispersion of accel */
-	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
-	/* EMA time constant in seconds*/
-	float ema_len = 0.5f;
-	/* set "still" threshold to 0.25 m/s^2 */
-	float still_thr2 = powf(0.25f, 2);
-	/* set accel error threshold to 5m/s^2 */
-	float accel_err_thr = 5.0f;
-	/* still time required in us */
-	hrt_abstime still_time = 2000000;
+	float		accel_ema[ndim] = { 0.0f };		// exponential moving average of accel
+	float		accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
+	float		ema_len = 0.5f;				// EMA time constant in seconds
+	const float	normal_still_thr = 0.25;		// normal still threshold
+	float		still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
+	float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
+	hrt_abstime	still_time = lenient_still_position ? 1000000 : 1500000;	// still time required in us
+    
 	struct pollfd fds[1];
 	fds[0].fd = accel_sub;
 	fds[0].events = POLLIN;
@@ -308,7 +306,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
+					mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
 					t_still = t;
 					t_timeout = t + timeout;
 					
@@ -325,7 +323,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 				   accel_disp[2] > still_thr2 * 4.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
+					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
 					sleep(3);
 					t_still = 0;
 				}
@@ -340,7 +338,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 		}
 		
 		if (poll_errcount > 1000) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
 			return DETECT_ORIENTATION_ERROR;
 		}
 	}
@@ -381,7 +379,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
 		return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
 	}
 	
-	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
 	
 	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
 }
@@ -401,28 +399,35 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
 	return rgOrientationStrs[orientation];
 }
 
-int calibrate_from_orientation(int	mavlink_fd,
-			       bool	side_data_collected[detect_orientation_side_count],
-			       calibration_from_orientation_worker_t calibration_worker,
-			       void*	worker_data)
+calibrate_return calibrate_from_orientation(int		mavlink_fd,
+					    int		cancel_sub,
+					    bool	side_data_collected[detect_orientation_side_count],
+					    calibration_from_orientation_worker_t calibration_worker,
+					    void*	worker_data,
+					    bool	lenient_still_position)
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 	
 	// Setup subscriptions to onboard accel sensor
 	
 	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
 	if (sub_accel < 0) {
-		mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort");
-		return ERROR;
+		mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: No onboard accel found");
+		return calibrate_return_error;
 	}
 	
 	unsigned orientation_failures = 0;
 	
-	// Rotate through all three main positions
+	// Rotate through all requested orientation
 	while (true) {
-		if (orientation_failures > 10) {
-			result = ERROR;
-			mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT);
+		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+			result = calibrate_return_cancelled;
+			break;
+		}
+		
+		if (orientation_failures > 4) {
+			result = calibrate_return_error;
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: timeout waiting for orientation");
 			break;
 		}
 		
@@ -450,32 +455,35 @@ int calibrate_from_orientation(int	mavlink_fd,
 				strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
 			}
 		}
-		mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
+		mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
 		
-		mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides");
-		enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel);
+		mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
+		enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
 		
 		if (orient == DETECT_ORIENTATION_ERROR) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
+			mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
 			continue;
 		}
 		
 		/* inform user about already handled side */
 		if (side_data_collected[orient]) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient));
-			mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side");
+			mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
+			mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
 			continue;
 		}
 		
-		mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient));
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
 		orientation_failures = 0;
 		
 		// Call worker routine
-		calibration_worker(orient, worker_data);
+		result = calibration_worker(orient, cancel_sub, worker_data);
+		if (result != calibrate_return_ok ) {
+			break;
+		}
 		
-		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
 		
 		// Note that this side is complete
 		side_data_collected[orient] = true;
@@ -487,7 +495,62 @@ int calibrate_from_orientation(int	mavlink_fd,
 		close(sub_accel);
 	}
 	
-	// FIXME: Do we need an orientation complete routine?
-	
 	return result;
 }
+
+int calibrate_cancel_subscribe(void)
+{
+	return orb_subscribe(ORB_ID(vehicle_command));
+}
+
+void calibrate_cancel_unsubscribe(int cmd_sub)
+{
+	orb_unsubscribe(cmd_sub);
+}
+
+static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+		case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive(true);
+			break;
+			
+		case VEHICLE_CMD_RESULT_DENIED:
+			mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command);
+			tune_negative(true);
+			break;
+			
+		default:
+			break;
+	}
+}
+
+bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
+{
+	struct pollfd fds[1];
+	fds[0].fd = cancel_sub;
+	fds[0].events = POLLIN;
+
+	if (poll(&fds[0], 1, 0) > 0) {
+		struct vehicle_command_s cmd;
+		memset(&cmd, 0, sizeof(cmd));
+		
+		orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
+		
+		if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
+		    (int)cmd.param1 == 0 &&
+		    (int)cmd.param2 == 0 &&
+		    (int)cmd.param3 == 0 &&
+		    (int)cmd.param4 == 0 &&
+		    (int)cmd.param5 == 0 &&
+		    (int)cmd.param6 == 0) {
+			calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+			mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG);
+			return true;
+		} else {
+			calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_DENIED);
+		}
+	}
+	
+	return false;
+}
\ No newline at end of file
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index 8f34f0204..b8232730a 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,12 +31,8 @@
  *
  ****************************************************************************/
 
-/**
- * @file calibration_routines.h
- * Calibration routines definitions.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
+/// @file calibration_routines.h
+///	@authot Don Gagne <don@thegagnes.com>
 
 /**
  * Least-squares fit of a sphere to a set of points.
@@ -75,30 +71,45 @@ enum detect_orientation_return {
 };
 static const unsigned detect_orientation_side_count = 6;
 
-/**
- * Wait for vehicle to become still and detect it's orientation.
- *
- * @param mavlink_fd the MAVLink file descriptor to print output to
- * @param accel_sub Subscription to onboard accel
- *
- * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements,
- * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
- */
-enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub);
+/// Wait for vehicle to become still and detect it's orientation
+///	@return Returns detect_orientation_return according to orientation when vehicle
+///		and ready for measurements
+enum detect_orientation_return detect_orientation(int	mavlink_fd,			///< Mavlink fd to write output to
+						  int	cancel_sub,			///< Cancel subscription from calibration_cancel_subscribe
+						  int	accel_sub,			///< Orb subcription to accel sensor
+						  bool	lenient_still_detection);	///< true: Use more lenient still position detection
 
-
-/**
- * Returns the human readable string representation of the orientation
- *
- * @param orientation Orientation to return string for, "error" if buffer is too small
- *
- * @return str Returned orientation string
- */
+/// Returns the human readable string representation of the orientation
+///	@param orientation Orientation to return string for, "error" if buffer is too small
 const char* detect_orientation_str(enum detect_orientation_return orientation);
 
-typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data);
+enum calibrate_return {
+	calibrate_return_ok,
+	calibrate_return_error,
+	calibrate_return_cancelled
+};
+
+typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return	orientation,	///< Orientation which was detected
+								  int				cancel_sub,	///< Cancel subscription from calibration_cancel_subscribe
+								  void*				worker_data);	///< Opaque worker data
+
+/// Perform calibration sequence which require a rest orientation detection prior to calibration.
+///	@return OK: Calibration succeeded, ERROR: Calibration failed
+calibrate_return calibrate_from_orientation(int		mavlink_fd,						///< Mavlink fd to write output to
+					    int		cancel_sub,						///< Cancel subscription from calibration_cancel_subscribe
+					    bool	side_data_collected[detect_orientation_side_count],	///< Sides for which data still needs calibration
+					    calibration_from_orientation_worker_t calibration_worker,		///< Worker routine which performs the actual calibration
+					    void*	worker_data,						///< Opaque data passed to worker routine
+					    bool	lenient_still_detection);				///< true: Use more lenient still position detection
+
+/// Called at the beginning of calibration in order to subscribe to the cancel command
+///	@return Handle to vehicle_command subscription
+int calibrate_cancel_subscribe(void);
+
+/// Called to cancel the subscription to the cancel command
+///	@param cancel_sub Cancel subcription from calibration_cancel_subscribe
+void calibrate_cancel_unsubscribe(int cancel_sub);
 
-int calibrate_from_orientation(int mavlink_fd,
-			       bool side_data_collected[detect_orientation_side_count],
-			       calibration_from_orientation_worker_t calibration_worker,
-			       void* worker_data);
+/// Used to periodically check for a cancel command
+bool calibrate_cancel_check(int mavlink_fd,	///< Mavlink fd for output
+			    int cancel_sub);	///< Cancel subcription fromcalibration_cancel_subscribe
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 57afcf19a..8cce01778 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2723,35 +2723,32 @@ void *commander_low_prio_loop(void *arg)
 							/* enable RC control input */
 							status.rc_input_blocked = false;
 							mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
+                            calib_ret = OK;
 						}
-
-						/* this always succeeds */
-						calib_ret = OK;
-
 					}
 
 					if (calib_ret == OK) {
 						tune_positive(true);
 
-					} else {
-						tune_negative(true);
-					}
+						// Update preflight check status
+						// we do not set the calibration return value based on it because the calibration
+						// might have worked just fine, but the preflight check fails for a different reason,
+						// so this would be prone to false negatives.
 
-					// Update preflight check status
-					// we do not set the calibration return value based on it because the calibration
-					// might have worked just fine, but the preflight check fails for a different reason,
-					// so this would be prone to false negatives.
+						bool checkAirspeed = false;
+						/* Perform airspeed check only if circuit breaker is not
+						 * engaged and it's not a rotary wing */
+						if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
+							checkAirspeed = true;
+						}
 
-					bool checkAirspeed = false;
-					/* Perform airspeed check only if circuit breaker is not
-					 * engaged and it's not a rotary wing */
-					if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
-						checkAirspeed = true;
-					}
+						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
 
-					status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
-					arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+					} else {
+						tune_negative(true);
+					}
 
 					break;
 				}
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 291ccfe80..e5df4175d 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -39,6 +39,7 @@
 
 #include "gyro_calibration.h"
 #include "calibration_messages.h"
+#include "calibration_routines.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
@@ -62,142 +63,180 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "gyro";
 
-int do_gyro_calibration(int mavlink_fd)
-{
-	const unsigned max_gyros = 3;
-
-	int32_t device_id[3];
-	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "HOLD STILL");
-
-	/* wait for the user to respond */
-	sleep(2);
-
-	struct gyro_scale gyro_scale_zero = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	struct gyro_scale gyro_scale[max_gyros] = {};
+static const unsigned max_gyros = 3;
 
-	int res = OK;
-
-	char str[30];
+/// Data passed to calibration worker routine
+typedef struct  {
+	int			mavlink_fd;
+	int32_t			device_id[max_gyros];
+	int			gyro_sensor_sub[max_gyros];
+	struct gyro_scale	gyro_scale[max_gyros];
+	struct gyro_report	gyro_report_0;
+} gyro_worker_data_t;
 
+static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
+{
+	gyro_worker_data_t*	worker_data = (gyro_worker_data_t*)(data);
+	unsigned		calibration_counter[max_gyros] = { 0 };
+	const unsigned		calibration_count = 5000;
+	struct gyro_report	gyro_report;
+	unsigned		poll_errcount = 0;
+	
+	struct pollfd fds[max_gyros];
 	for (unsigned s = 0; s < max_gyros; s++) {
-
-		/* ensure all scale fields are initialized tha same as the first struct */
-		(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
-
-		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-		/* reset all offsets to zero and all scales to one */
-		int fd = open(str, 0);
-
-		if (fd < 0) {
-			continue;
+		fds[s].fd = worker_data->gyro_sensor_sub[s];
+		fds[s].events = POLLIN;
+	}
+	
+	memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
+	
+	/* use first gyro to pace, but count correctly per-gyro for statistics */
+	while (calibration_counter[0] < calibration_count) {
+		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+			return calibrate_return_cancelled;
 		}
-
-		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
-
-		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
-		close(fd);
-
-		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
+		
+		int poll_ret = poll(&fds[0], max_gyros, 1000);
+		
+		if (poll_ret > 0) {
+			
+			for (unsigned s = 0; s < max_gyros; s++) {
+				bool changed;
+				orb_check(worker_data->gyro_sensor_sub[s], &changed);
+				
+				if (changed) {
+					orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
+					
+					if (s == 0) {
+						orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
+					}
+					
+					worker_data->gyro_scale[s].x_offset += gyro_report.x;
+					worker_data->gyro_scale[s].y_offset += gyro_report.y;
+					worker_data->gyro_scale[s].z_offset += gyro_report.z;
+					calibration_counter[s]++;
+				}
+				
+				if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
+					mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
+				}
+			}
+			
+		} else {
+			poll_errcount++;
+		}
+		
+		if (poll_errcount > 1000) {
+			mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
+			return calibrate_return_error;
 		}
 	}
-
-	unsigned calibration_counter[max_gyros] = { 0 };
-	const unsigned calibration_count = 5000;
-
-	struct gyro_report gyro_report_0 = {};
-
-	if (res == OK) {
-		/* determine gyro mean values */
-		unsigned poll_errcount = 0;
-
-		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro[max_gyros];
-		struct pollfd fds[max_gyros];
-
-		for (unsigned s = 0; s < max_gyros; s++) {
-			sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-			fds[s].fd = sub_sensor_gyro[s];
-			fds[s].events = POLLIN;
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
+			mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
+			return calibrate_return_error;
 		}
 
-		struct gyro_report gyro_report;
-
-		/* use first gyro to pace, but count correctly per-gyro for statistics */
-		while (calibration_counter[0] < calibration_count) {
-			/* wait blocking for new data */
-
-			int poll_ret = poll(&fds[0], max_gyros, 1000);
-
-			if (poll_ret > 0) {
-
-				for (unsigned s = 0; s < max_gyros; s++) {
-					bool changed;
-					orb_check(sub_sensor_gyro[s], &changed);
-
-					if (changed) {
-						orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
-
-						if (s == 0) {
-							orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
-						}
+		worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
+		worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
+		worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
+	}
 
-						gyro_scale[s].x_offset += gyro_report.x;
-						gyro_scale[s].y_offset += gyro_report.y;
-						gyro_scale[s].z_offset += gyro_report.z;
-						calibration_counter[s]++;
-					}
+	return calibrate_return_ok;
+}
 
-					if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
-						mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
-					}
-				}
+int do_gyro_calibration(int mavlink_fd)
+{
+	int			res = OK;
+	gyro_worker_data_t	worker_data;
 
-			} else {
-				poll_errcount++;
-			}
+	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
-			if (poll_errcount > 1000) {
-				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
-				res = ERROR;
-				break;
-			}
+	worker_data.mavlink_fd = mavlink_fd;
+	
+	struct gyro_scale gyro_scale_zero = {
+		0.0f,	// x offset
+		1.0f,	// x scale
+		0.0f,	// y offset
+		1.0f,	// y scale
+		0.0f,	// z offset
+		1.0f,	// z scale
+	};
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		char str[30];
+		
+		// Reset gyro ids to unavailable
+		worker_data.device_id[s] = 0;
+		(void)sprintf(str, "CAL_GYRO%u_ID", s);
+		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
+			return ERROR;
 		}
+		
+		// Reset all offsets to 0 and scales to 1
+		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
+		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		int fd = open(str, 0);
+		if (fd >= 0) {
+			worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+			res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
+			close(fd);
 
-		for (unsigned s = 0; s < max_gyros; s++) {
-			close(sub_sensor_gyro[s]);
-
-			gyro_scale[s].x_offset /= calibration_counter[s];
-			gyro_scale[s].y_offset /= calibration_counter[s];
-			gyro_scale[s].z_offset /= calibration_counter[s];
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
+				return ERROR;
+			}
 		}
+		
+	}
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+	}
+	
+	// Calibrate right-side up
+	
+	bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
+	
+	int cancel_sub  = calibrate_cancel_subscribe();	
+	calibrate_return cal_return = calibrate_from_orientation(mavlink_fd,                 // Mavlink fd to write output
+								cancel_sub,                 // Subscription to vehicle_command for cancel support
+								side_collected,		// Sides to calibrate
+								gyro_calibration_worker,     // Calibration worker
+								&worker_data,		// Opaque data for calibration worked
+								true);			// true: lenient still detection
+	calibrate_cancel_unsubscribe(cancel_sub);
+	
+	for (unsigned s = 0; s < max_gyros; s++) {
+		close(worker_data.gyro_sensor_sub[s]);
 	}
 
+	if (cal_return == calibrate_return_cancelled) {
+		// Cancel message already sent, we are done here
+		return ERROR;
+	} else if (cal_return == calibrate_return_error) {
+		res = ERROR;
+	}
+		
 	if (res == OK) {
 		/* check offsets */
-		float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
-		float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
-		float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
+		float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
+		float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
+		float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
 
 		/* maximum allowable calibration error in radians */
 		const float maxoff = 0.0055f;
 
-		if (!isfinite(gyro_scale[0].x_offset) ||
-		    !isfinite(gyro_scale[0].y_offset) ||
-		    !isfinite(gyro_scale[0].z_offset) ||
+		if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
+		    !isfinite(worker_data.gyro_scale[0].y_offset) ||
+		    !isfinite(worker_data.gyro_scale[0].z_offset) ||
 		    fabsf(xdiff) > maxoff ||
 		    fabsf(ydiff) > maxoff ||
 		    fabsf(zdiff) > maxoff) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
 			res = ERROR;
 		}
 	}
@@ -207,40 +246,38 @@ int do_gyro_calibration(int mavlink_fd)
 		bool failed = false;
 
 		for (unsigned s = 0; s < max_gyros; s++) {
+			if (worker_data.device_id[s] != 0) {
+				char str[30];
+				
+				(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
+				(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
+				(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
+				(void)sprintf(str, "CAL_GYRO%u_ID", s);
+				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
+
+				/* apply new scaling and offsets */
+				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+				int fd = open(str, 0);
+
+				if (fd < 0) {
+					failed = true;
+					continue;
+				}
 
-			/* if any reasonable amount of data is missing, skip */
-			if (calibration_counter[s] < calibration_count / 2) {
-				continue;
-			}
-
-			(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
-			failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset)));
-			(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
-			failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset)));
-			(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
-			failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset)));
-			(void)sprintf(str, "CAL_GYRO%u_ID", s);
-			failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s])));
-
-			/* apply new scaling and offsets */
-			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-			int fd = open(str, 0);
-
-			if (fd < 0) {
-				failed = true;
-				continue;
-			}
+				res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
+				close(fd);
 
-			res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
-			close(fd);
-
-			if (res != OK) {
-				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+				if (res != OK) {
+					mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);
+				}
 			}
 		}
 
 		if (failed) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params");
 			res = ERROR;
 		}
 	}
@@ -257,16 +294,14 @@ int do_gyro_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 	}
 
-	if (res == OK) {
-		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-
-	} else {
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-	}
+	mavlink_log_info(mavlink_fd, res == OK ? CAL_QGC_DONE_MSG : CAL_QGC_FAILED_MSG, sensor_name);
 
+	// This give a chance for the log messages to go out of the queue before someone else stomps on then
+	sleep(1);
+	
 	return res;
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 198acb027..3d10f98c1 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -65,8 +65,7 @@ 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);
+calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
 /// Data passed to calibration worker routine
 typedef struct  {
@@ -86,7 +85,7 @@ typedef struct  {
 
 int do_mag_calibration(int mavlink_fd)
 {
-	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	struct mag_scale mscale_null = {
 		0.0f,
@@ -113,7 +112,7 @@ int do_mag_calibration(int mavlink_fd)
 		(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);
+			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
 			break;
 		}
 
@@ -131,15 +130,15 @@ int do_mag_calibration(int mavlink_fd)
 		result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
 		if (result != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
 		}
 
+		/* calibrate range */
 		if (result == OK) {
-			/* calibrate range */
 			result = ioctl(fd, MAGIOCCALIBRATE, fd);
 
 			if (result != OK) {
-				mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
 				/* this is non-fatal - mark it accordingly */
 				result = OK;
 			}
@@ -148,39 +147,48 @@ int do_mag_calibration(int mavlink_fd)
 		close(fd);
 	}
 
+	// Calibrate all mags at the same time
 	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 */
-		result = param_save_default();
-		if (result != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+		switch (mag_calibrate_all(mavlink_fd, device_ids)) {
+			case calibrate_return_cancelled:
+				// Cancel message already displayed, we're done here
+				result = ERROR;
+				break;
+				
+			case calibrate_return_ok:
+				/* auto-save to EEPROM */
+				result = param_save_default();
+				if (result == OK) {
+					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+					break;
+				} else {
+					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+				}
+				// Fall through
+				
+			default:
+				mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+				break;
 		}
 	}
-	
-	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);
-	}
 
+	// This give a chance for the log messages to go out of the queue before someone else stomps on then
+	sleep(1);
+	
 	return result;
 }
 
-int mag_calibration_worker(detect_orientation_return orientation, void* data)
+static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 	
 	unsigned int calibration_counter_side;
 
 	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);
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] 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;
@@ -191,6 +199,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 	while (hrt_absolute_time() < calibration_deadline &&
 	       calibration_counter_side < worker_data->calibration_points_perside) {
 		
+		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+			result = calibrate_return_cancelled;
+			break;
+		}
+		
 		// Wait clocking for new data on all mags
 		struct pollfd fds[max_mags];
 		size_t fd_count = 0;
@@ -222,8 +235,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 			
 			// Progress indicator for side
 			mavlink_and_console_log_info(worker_data->mavlink_fd,
-						     "%s %s side calibration: progress <%u>",
-						     sensor_name,
+						     "[cal] %s side calibration: progress <%u>",
 						     detect_orientation_str(orientation),
 						     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
 		} else {
@@ -231,50 +243,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
 		}
 		
 		if (poll_errcount > worker_data->calibration_points_perside * 3) {
-			result = ERROR;
-			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			result = calibrate_return_error;
+			mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
 			break;
 		}
 	}
 	
-	// Mark the opposite side as collected as well. No need to collect opposite side since it
-	// would generate similar points.
-	detect_orientation_return alternateOrientation = orientation;
-	switch (orientation) {
-		case DETECT_ORIENTATION_TAIL_DOWN:
-			alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
-			break;
-		case DETECT_ORIENTATION_NOSE_DOWN:
-			alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
-			break;
-		case DETECT_ORIENTATION_LEFT:
-			alternateOrientation = DETECT_ORIENTATION_RIGHT;
-			break;
-		case DETECT_ORIENTATION_RIGHT:
-			alternateOrientation = DETECT_ORIENTATION_LEFT;
-			break;
-		case DETECT_ORIENTATION_UPSIDE_DOWN:
-			alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
-			break;
-		case DETECT_ORIENTATION_RIGHTSIDE_UP:
-			alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
-			break;
-		case DETECT_ORIENTATION_ERROR:
-			warnx("Invalid orientation in mag_calibration_worker");
-			break;
+	if (result == calibrate_return_ok) {
+		mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
+		
+		worker_data->done_count++;
+		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
 	}
-	worker_data->side_data_collected[alternateOrientation] = true;
-	mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
-	
-	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;
 }
 
-int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
+calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 {
-	int result = OK;
+	calibrate_return result = calibrate_return_ok;
 
 	mag_worker_data_t worker_data;
 	
@@ -285,10 +272,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 	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;
-	}
+	// Collect: Right-side up, Left Side, Nose down
+	worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
+	worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
+	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;
 	
 	for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
 		// Initialize to no subscription
@@ -310,21 +300,21 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		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;
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
+			result = calibrate_return_error;
 		}
 	}
 
 	
 	// Setup subscriptions to mag sensors
-	if (result == OK) {
+	if (result == calibrate_return_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;
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
+					result = calibrate_return_error;
 					break;
 				}
 			}
@@ -332,7 +322,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 	}
 	
 	// Limit update rate to get equally spaced measurements over time (in ms)
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				// Mag in this slot is available
@@ -344,8 +334,18 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		}
 		
 	}
-
-	result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
+    
+	if (result == calibrate_return_ok) {
+		int cancel_sub  = calibrate_cancel_subscribe();
+
+		result = calibrate_from_orientation(mavlink_fd,                         // Mavlink fd to write output
+						    cancel_sub,                         // Subscription to vehicle_command for cancel support
+						    worker_data.side_data_collected,    // Sides to calibrate
+						    mag_calibration_worker,             // Calibration worker
+						    &worker_data,			// Opaque data for calibration worked
+						    true);				// true: lenient still detection
+		calibrate_cancel_unsubscribe(cancel_sub);
+	}
 	
 	// Close subscriptions
 	for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
@@ -363,7 +363,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 	float sphere_radius[max_mags];
 	
 	// Sphere fit the data to get calibration values
-	if (result == OK) {
+	if (result == calibrate_return_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
@@ -375,8 +375,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 							 &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;
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
+					result = calibrate_return_error;
 				}
 			}
 		}
@@ -389,7 +389,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 		free(worker_data.z[cur_mag]);
 	}
 	
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;
@@ -400,27 +400,25 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 				(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;
+					mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
+					result = calibrate_return_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 (result == calibrate_return_ok) {
+					if (ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
+						mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
+						result = calibrate_return_error;
 					}
 				}
 
-				if (result == OK) {
+				if (result == calibrate_return_ok) {
 					mscale.x_offset = sphere_x[cur_mag];
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
-					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;
+					if (ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
+						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
+						result = calibrate_return_error;
 					}
 				}
 				
@@ -429,7 +427,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 					close(fd_mag);
 				}
 
-				if (result == OK) {
+				if (result == calibrate_return_ok) {
 					bool failed = false;
 					
 					/* set parameters */
@@ -449,13 +447,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
 					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;
+						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
+						result = calibrate_return_error;
 					} else {
-						mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
+						mavlink_and_console_log_info(mavlink_fd, "[cal] 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",
+						mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
 									     cur_mag,
 									     (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
 					}
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 07c3f2ab7..16e766646 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -145,7 +145,7 @@
 #endif
 static const int ERROR = -1;
 
-#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
+#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
 
 /**
  * Sensor app start / stop handling function
@@ -622,6 +622,16 @@ Sensors::Sensors() :
 	/* Barometer QNH */
 	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
 
+	// These are parameters for which QGroundControl always expects to be returned in a list request.
+	// We do a param_find here to force them into the list.
+	param_find("RC_CHAN_CNT");
+    param_find("CAL_MAG0_ID");
+    param_find("CAL_MAG1_ID");
+    param_find("CAL_MAG2_ID");
+    param_find("CAL_MAG0_ROT");
+    param_find("CAL_MAG1_ROT");
+    param_find("CAL_MAG2_ROT");
+	
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -1384,12 +1394,12 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
+						warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
+							warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
 						} else {
 							config_ok = true;
 						}
@@ -1450,12 +1460,12 @@ Sensors::parameter_update_poll(bool forced)
 					failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
 
 					if (failed) {
-						warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
+						warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
+							warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
 						} else {
 							config_ok = true;
 						}
@@ -1573,12 +1583,12 @@ Sensors::parameter_update_poll(bool forced)
 					}
 
 					if (failed) {
-						warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
+						warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 					} else {
 						/* apply new scaling and offsets */
 						res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
 						if (res) {
-							warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
+							warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
 						} else {
 							config_ok = true;
 						}