Merge branch 'master' into betabeta

44 files changed, 877 insertions, 663 deletions

diff --git a/NuttX b/NuttX
-Subproject 94818ce16f63c4728a1d9316628a3651287f16d
+Subproject 678eea3d2c157c686439597abb0367b0f1e643f
diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6
index a20ca04df..191c58da4 100644
--- a/ROMFS/px4fmu_common/init.d/13002_firefly6
+++ b/ROMFS/px4fmu_common/init.d/13002_firefly6
@@ -17,5 +17,7 @@ set PWM_AUX_DISARMED 1000
 set PWM_AUX_MIN 1000
 set PWM_AUX_MAX 2000
 
+set MAV_TYPE 21
+
 param set VT_MOT_COUNT 6
 param set VT_IDLE_PWM_MC 1080
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index e957626ce..86042220e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -16,5 +16,8 @@ then
 	set PX4IO_LIMIT 200
 fi
 
-echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
-px4io limit $PX4IO_LIMIT
+if px4io limit $PX4IO_LIMIT
+then
+else
+	echo "[i] Set PX4IO update rate to $PX4IO_LIMIT Hz failed!"
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index fddcf5b3e..e456eff7f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,7 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 20000 -d /dev/ttyACM0 -x
+mavlink start -r 30000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index e2b247a76..1237d9bb1 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -24,7 +24,6 @@ set LOG_FILE /fs/microsd/bootlog.txt
 # Try to mount the microSD card.
 #
 # REBOOTWORK this needs to start after the flight control loop
-echo "[i] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	echo "[i] microSD mounted: /fs/microsd"
@@ -53,11 +52,9 @@ fi
 #
 if [ -f $FRC ]
 then
-	echo "[i] Executing init script: $FRC"
+	echo "[i] Executing script: $FRC"
 	sh $FRC
 	set MODE custom
-else
-	echo "[i] Init script not found: $FRC"
 fi
 unset FRC
 
@@ -126,6 +123,7 @@ then
 	set PWM_AUX_DISARMED none
 	set PWM_AUX_MIN none
 	set PWM_AUX_MAX none
+	set FAILSAFE_AUX none
 	set MK_MODE none
 	set FMU_MODE pwm
 	set MAVLINK_F default
@@ -174,10 +172,8 @@ then
 	#
 	if [ -f $FCONFIG ]
 	then
-		echo "[i] Config: $FCONFIG"
+		echo "[i] Custom config: $FCONFIG"
 		sh $FCONFIG
-	else
-		echo "[i] Config not found: $FCONFIG"
 	fi
 	unset FCONFIG
 
@@ -330,10 +326,8 @@ then
 		then
 			if px4io start
 			then
-				echo "[i] PX4IO started"
 				sh /etc/init.d/rc.io
 			else
-				echo "[i] ERROR: PX4IO start failed"
 				tone_alarm $TUNE_ERR
 			fi
 		fi
@@ -681,8 +675,6 @@ then
 	then
 		echo "[i] Addons script: $FEXTRAS"
 		sh $FEXTRAS
-	else
-		echo "[i] No addons script: $FEXTRAS"
 	fi
 	unset FEXTRAS
 
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index 2e001cac7..2a7e493a3 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -72,8 +72,10 @@ uint8 VEHICLE_TYPE_FLAPPING_WING = 16	    # Flapping wing
 uint8 VEHICLE_TYPE_KITE = 17		    # Kite
 uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18    # Onboard companion controller
 uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19	    # Vtol with two engines
-uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20	    # Vtol with four engines*/
-uint8 VEHICLE_TYPE_ENUM_END = 21
+uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20	    # Vtol with four engines
+uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 		# Vtol with six engines
+uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22		# Vtol with eight engines
+uint8 VEHICLE_TYPE_ENUM_END = 23
 
 uint8 VEHICLE_BATTERY_WARNING_NONE = 0	    # no battery low voltage warning active
 uint8 VEHICLE_BATTERY_WARNING_LOW = 1	    # warning of low voltage
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
index 08ff4a988..e314bf1d8 100644
--- a/nuttx-configs/aerocore/nsh/defconfig
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -403,7 +403,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_DESCRIPTORS=42
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 389cb5ab5..2c9a21f21 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -326,7 +326,7 @@ CONFIG_DRAM_START=0x20000000
 CONFIG_DRAM_SIZE=196608
 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
 # The actual usage is 420 bytes
-CONFIG_ARCH_INTERRUPTSTACK=1500
+CONFIG_ARCH_INTERRUPTSTACK=750
 
 #
 # Boot options
@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=44
+CONFIG_NFILE_DESCRIPTORS=42
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index d331aa26a..4ccc5dacb 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=44
+CONFIG_NFILE_DESCRIPTORS=42
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index d749a0d7b..cfa59eb3a 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -316,7 +316,6 @@ BlinkM::init()
 	ret = I2C::init();
 
 	if (ret != OK) {
-		warnx("I2C init failed");
 		return ret;
 	}
 
@@ -970,7 +969,7 @@ blinkm_main(int argc, char *argv[])
 		if (OK != g_blinkm->init()) {
 			delete g_blinkm;
 			g_blinkm = nullptr;
-			errx(1, "init failed");
+			errx(1, "no BlinkM found");
 		}
 
 		exit(0);
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 293021f8b..c7d4f1ce4 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -216,8 +216,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);
 
-	message("[boot] Successfully initialized SPI port 1\r\n");
-
 	/*
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
@@ -243,7 +241,6 @@ __EXPORT int nsh_archinitialize(void)
 
 	/* Get the SPI port for the microSD slot */
 
-	message("[boot] Initializing SPI port 3\n");
 	spi3 = up_spiinitialize(3);
 
 	if (!spi3) {
@@ -252,8 +249,6 @@ __EXPORT int nsh_archinitialize(void)
 		return -ENODEV;
 	}
 
-	message("[boot] Successfully initialized SPI port 3\n");
-
 	/* Now bind the SPI interface to the MMCSD driver */
 	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
 
@@ -263,7 +258,5 @@ __EXPORT int nsh_archinitialize(void)
 		return -ENODEV;
 	}
 
-	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-
 	return OK;
 }
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 9b25c574a..266642cbb 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -296,8 +296,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
 	up_udelay(20);
 
-	message("[boot] Initialized SPI port 1 (SENSORS)\n");
-
 	/* Get the SPI port for the FRAM */
 
 	spi2 = up_spiinitialize(2);
@@ -317,8 +315,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SETMODE(spi2, SPIDEV_MODE3);
 	SPI_SELECT(spi2, SPIDEV_FLASH, false);
 
-	message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
-
 	spi4 = up_spiinitialize(4);
 
 	/* Default SPI4 to 1MHz and de-assert the known chip selects. */
@@ -328,8 +324,6 @@ __EXPORT int nsh_archinitialize(void)
 	SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
 	SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
 
-	message("[boot] Initialized SPI port 4\n");
-
 	#ifdef CONFIG_MMCSD
 	/* First, get an instance of the SDIO interface */
 
@@ -350,7 +344,6 @@ __EXPORT int nsh_archinitialize(void)
 	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
 	sdio_mediachange(sdio, true);
 
-	message("[boot] Initialized SDIO\n");
 	#endif
 
 	return OK;
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 3a3848446..5bf88da3d 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1490,8 +1490,9 @@ start(enum HMC5883_BUS busid, enum Rotation rotation)
 		started |= start_bus(bus_options[i], rotation);
 	}
 
-	if (!started)
-		errx(1, "driver start failed");
+	if (!started) {
+		exit(1);
+	}
 }
 
 /**
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 8ea7a6170..b343a3e30 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -674,7 +674,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no PX4 FLOW connected");
 }
 
 /**
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 1e38a766e..512d6318d 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -625,7 +625,7 @@ rgbled_main(int argc, char *argv[])
 			if (g_rgbled == nullptr) {
 				// fall back to default bus
 				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
-					errx(1, "init failed");
+					errx(1, "no RGB led on bus #%d", i2cdevice);
 				}
 				i2cdevice = PX4_I2C_BUS_LED;
 			}
@@ -640,7 +640,7 @@ rgbled_main(int argc, char *argv[])
 			if (OK != g_rgbled->init()) {
 				delete g_rgbled;
 				g_rgbled = nullptr;
-				errx(1, "init failed");
+				errx(1, "no RGB led on bus #%d", i2cdevice);
 			}
 		}
 
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index b0a587762..4d9bd8ae0 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -254,13 +254,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
 
 	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
 	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
-		mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
 		success = false;
 		goto out;
 	}
 
 	if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
-		mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
 		// XXX do not make this fatal yet
 	}
 
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 409c2ea00..f83640d28 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,23 @@ 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);
+	if (res != OK) {
+		if (active_sensors == 0) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
+		}
 		return ERROR;
 	}
 
@@ -263,7 +272,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 +280,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_QGC_FAILED_MSG, "sensor does not exist");
 			res = ERROR;
 		} else {
 			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -279,7 +288,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 +297,47 @@ 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);
+		/* if there is a any preflight-check system response, let the barrage of messages through */
+		usleep(200000);
+
+		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);
 	}
+	
+	/* give this message enough time to propagate */
+	usleep(600000);
 
 	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 +358,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 +368,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 +387,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 +405,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 +449,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 +459,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 +485,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 +507,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 +518,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..7b4baae62 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] calibration failed: reading sensor"
+#define CAL_ERROR_RESET_CAL_MSG		"[cal] calibration failed: to reset, sensor %u"
+#define CAL_ERROR_APPLY_CAL_MSG		"[cal] calibration failed: to apply calibration, sensor %u"
+#define CAL_ERROR_SET_PARAMS_MSG	"[cal] calibration failed: to set parameters, sensor %u"
+#define CAL_ERROR_SAVE_PARAMS_MSG	"[cal] calibration failed: 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..7e8c0fa52 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,8 +323,8 @@ 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...");
-					sleep(3);
+					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
+					usleep(500000);
 					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_QGC_FAILED_MSG, "No onboard accel");
+		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_QGC_FAILED_MSG, "timeout: no motion");
 			break;
 		}
 		
@@ -450,44 +455,102 @@ 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;
 		tune_neutral(true);
-		sleep(1);
+		usleep(500000);
 	}
 	
 	if (sub_accel >= 0) {
 		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 5e0346155..7aaf5e5cd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -182,21 +182,6 @@ static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
 static struct offboard_control_mode_s offboard_control_mode;
 
-/* tasks waiting for low prio thread */
-typedef enum {
-	LOW_PRIO_TASK_NONE = 0,
-	LOW_PRIO_TASK_PARAM_SAVE,
-	LOW_PRIO_TASK_PARAM_LOAD,
-	LOW_PRIO_TASK_GYRO_CALIBRATION,
-	LOW_PRIO_TASK_MAG_CALIBRATION,
-	LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
-	LOW_PRIO_TASK_RC_CALIBRATION,
-	LOW_PRIO_TASK_ACCEL_CALIBRATION,
-	LOW_PRIO_TASK_AIRSPEED_CALIBRATION
-} low_prio_task_t;
-
-static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
-
 /**
  * The daemon app only briefly exists to start
  * the background job. The stack size assigned in the
@@ -228,6 +213,8 @@ int commander_thread_main(int argc, char *argv[]);
 
 void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
 
+void get_circuit_breaker_params();
+
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
@@ -552,8 +539,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				}
 				else {
 
-					//Refuse to arm if preflight checks have failed
-					if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
+					// Refuse to arm if preflight checks have failed
+					if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
 						mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
 						cmd_result = VEHICLE_CMD_RESULT_DENIED;			
 						break;
@@ -960,10 +947,10 @@ int commander_thread_main(int argc, char *argv[])
 
 	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
 		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
-			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
-			      mission.current_seq);
-			mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
-					 mission.dataman_id, mission.count, mission.current_seq);
+			if (mission.count > 0) {
+				mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
+						 mission.dataman_id, mission.count, mission.current_seq);
+			}
 
 		} else {
 			const char *missionfail = "reading mission state failed";
@@ -1032,7 +1019,7 @@ int commander_thread_main(int argc, char *argv[])
 	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
 
 	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+		telemetry_subs[i] = -1;
 		telemetry_last_heartbeat[i] = 0;
 		telemetry_last_dl_loss[i] = 0;
 		telemetry_lost[i] = true;
@@ -1129,6 +1116,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_get(_param_sys_type, &(status.system_type)); // get system type
 	status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
 
+	get_circuit_breaker_params();
+
 	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
@@ -1136,9 +1125,9 @@ int commander_thread_main(int argc, char *argv[])
 		checkAirspeed = true;
 	}
 
-	//Run preflight check
+	// Run preflight check
 	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
-	if(!status.condition_system_sensors_initialized) {
+	if (!status.condition_system_sensors_initialized) {
 		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
 	}
 	else {
@@ -1170,7 +1159,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* initialize low priority thread */
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2100);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1222,14 +1211,7 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 
-				status.circuit_breaker_engaged_power_check =
-					circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
-				status.circuit_breaker_engaged_airspd_check =
-					circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
-				status.circuit_breaker_engaged_enginefailure_check =
-					circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
-				status.circuit_breaker_engaged_gpsfailure_check =
-					circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
+				get_circuit_breaker_params();
 
 				status_changed = true;
 
@@ -1290,6 +1272,11 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+
+			if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) {
+				telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+			}
+
 			orb_check(telemetry_subs[i], &updated);
 
 			if (updated) {
@@ -1611,8 +1598,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
-			/* TODO: check for sensors */
+		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
 							     mavlink_fd);
 
@@ -1620,8 +1606,6 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_changed = true;
 			}
 
-		} else {
-			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -2121,6 +2105,19 @@ int commander_thread_main(int argc, char *argv[])
 }
 
 void
+get_circuit_breaker_params()
+{
+	status.circuit_breaker_engaged_power_check =
+		circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+	status.circuit_breaker_engaged_airspd_check =
+		circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+	status.circuit_breaker_engaged_enginefailure_check =
+		circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
+	status.circuit_breaker_engaged_gpsfailure_check =
+		circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
+}
+
+void
 check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
 {
 	hrt_abstime t = hrt_absolute_time();
@@ -2669,7 +2666,7 @@ void *commander_low_prio_loop(void *arg)
 
 					/* try to go to INIT/PREFLIGHT arming state */
 					if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
-							true /* fRunPreArmChecks */, mavlink_fd)) {
+							false /* fRunPreArmChecks */, mavlink_fd)) {
 						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 						break;
 					}
@@ -2718,51 +2715,38 @@ 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;
 				}
 
 			case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-				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;
-				}
-
-				// Update preflight check status
-				status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
-
 					if (((int)(cmd.param1)) == 0) {
 						int ret = param_load_default();
 
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 5f735b7b7..99b926be4 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -85,8 +85,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
 }
 
 bool is_vtol(const struct vehicle_status_s * current_status) {
-	return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
-		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR;
+	return (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
+		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR ||
+		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_HEXAROTOR ||
+		current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR);
 }
 
 static int buzzer = -1;
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 291ccfe80..bdef8771e 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)&gyro_scale[s]);
-			close(fd);
+				res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.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,21 @@ 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);
+	/* if there is a any preflight-check system response, let the barrage of messages through */
+	usleep(200000);
 
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	} else {
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
 
+	/* give this message enough time to propagate */
+	usleep(600000);
+	
 	return res;
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 198acb027..8a8d85818 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,52 @@ 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 there is a any preflight-check system response, let the barrage of messages through */
+				usleep(200000);
+
+				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);
-	}
 
+	/* give this message enough time to propagate */
+	usleep(600000);
+	
 	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 +203,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 +239,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 +247,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 +276,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 +304,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 +326,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 +338,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 +367,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 +379,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 +393,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 +404,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 +431,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 +451,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/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9aba61e14..73fdb0940 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -140,6 +140,11 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			prearm_ret = OK;
 			status->condition_system_sensors_initialized = true;
 
+			/* recover from a prearm fail */
+			if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+				status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
+			}
+
 		} else {
 			armed->lockdown = false;
 		}
@@ -211,23 +216,35 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			valid_transition = true;
 		}
 
-		/* Sensors need to be initialized for STANDBY state */
-		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
-			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
+		// Sensors need to be initialized for STANDBY state, except for HIL
+		if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
+			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+			(!status->condition_system_sensors_initialized)) {
+			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
 			feedback_provided = true;
 			valid_transition = false;
 			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 		}
 
-		/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
-		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
-			new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-			if (status->condition_system_sensors_initialized) {
-				mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+		// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
+		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+				if (status->condition_system_sensors_initialized) {
+					mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+				} else {
+					mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
+				}
+				feedback_provided = true;
+
+			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+					status->condition_system_sensors_initialized) {
+				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
+				feedback_provided = true;
 			} else {
-				mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
+
 			}
-			feedback_provided = true;
 		}
 
 		// Finish up the state transition
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 68bf12024..b442b7430 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -685,20 +685,21 @@ task_main(int argc, char *argv[])
 
 	fsync(g_task_fd);
 
+	printf("dataman: ");
 	/* see if we need to erase any items based on restart type */
 	int sys_restart_val;
 	if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
-			warnx("Power on restart");
+			printf("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
 		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
-			warnx("In flight restart");
+			printf("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
 		} else {
-			warnx("Unknown restart");
+			printf("Unknown restart");
 		}
 	} else {
-		warnx("Unknown restart");
+		printf("Unknown restart");
 	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
@@ -706,7 +707,7 @@ task_main(int argc, char *argv[])
 	/* worker thread is shutting down but still processing requests */
 	g_fd = g_task_fd;
 
-	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
+	printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset);
 
 	/* Tell startup that the worker thread has completed its initialization */
 	sem_post(&g_init_sema);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index ba049bac4..22ff3edf6 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -90,7 +90,7 @@
 static const int ERROR = -1;
 
 #define DEFAULT_DEVICE_NAME			"/dev/ttyS1"
-#define MAX_DATA_RATE				20000	///< max data rate in bytes/s
+#define MAX_DATA_RATE				60000	///< max data rate in bytes/s
 #define MAIN_LOOP_DELAY 			10000	///< 100 Hz @ 1000 bytes/s data rate
 #define FLOW_CONTROL_DISABLE_THRESHOLD		40	///< picked so that some messages still would fit it.
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 276035aa2..7ddc52fd1 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2231,7 +2231,7 @@ protected:
 };
 
 
-StreamListItem *streams_list[] = {
+const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
 	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index 7e4416609..5b6b9d633 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -56,6 +56,6 @@ public:
 	~StreamListItem() {};
 };
 
-extern StreamListItem *streams_list[];
+extern const StreamListItem *streams_list[];
 
 #endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 20d7cfdbb..9c8794017 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -130,7 +130,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				} else {
 					/* when index is >= 0, send this parameter again */
-					send_param(param_for_index(req_read.param_index));
+					send_param(param_for_used_index(req_read.param_index));
 				}
 			}
 			break;
@@ -192,6 +192,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 		/* look for the first parameter which is used */
 		param_t p;
 		do {
+			/* walk through all parameters, including unused ones */
 			p = param_for_index(_send_all_index);
 			_send_all_index++;
 		} while (p != PARAM_INVALID && !param_used(p));
@@ -200,7 +201,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
 			send_param(p);
 		}
 
-		if (_send_all_index >= (int) param_count()) {
+		if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
 			_send_all_index = -1;
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index faede15cb..c4e332bf1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -822,7 +822,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
 				if (!(_offboard_control_mode.ignore_attitude)) {
-					static struct vehicle_attitude_setpoint_s att_sp = {};
+					struct vehicle_attitude_setpoint_s att_sp = {};
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 3a0dfe4c3..ec512ab56 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -908,7 +908,7 @@ MulticopterAttitudeControl::start()
 	_control_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       1600,
+				       1500,
 				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 5191a4de3..6ffb37d97 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1426,7 +1426,7 @@ MulticopterPositionControl::start()
 	_control_task = task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       1600,
+				       1500,
 				       (main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 904f44238..949231b15 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -134,7 +134,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 {
 	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
 	for (size_t i = 0; i < nMissionItems; i++) {
-		static struct mission_item_s missionitem;
+		struct mission_item_s missionitem;
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 9691e26a8..042f926d3 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -518,7 +518,7 @@ Navigator::start()
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT + 20,
-					 1800,
+					 1700,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 7800a6f03..3d8c78cf1 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -48,18 +48,6 @@
  */
 
 /**
- * Loiter radius after RTL (FW only)
- *
- * Default value of loiter radius after RTL (fixedwing only).
- *
- * @unit meters
- * @min 20
- * @max 200
- * @group Return To Land
- */
-PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
-
-/**
  * RTL altitude
  *
  * Altitude to fly back in RTL in meters
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e2274342e..ae4913559 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -51,7 +51,6 @@
 #include <errno.h>
 #include <unistd.h>
 #include <stdio.h>
-#include <poll.h>
 #include <stdlib.h>
 #include <string.h>
 #include <ctype.h>
@@ -151,11 +150,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
 		log_msgs_skipped++; \
 	}
 
-#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
-	fds[fdsc_count].fd = subs.##_var##_sub; \
-	fds[fdsc_count].events = POLLIN; \
-	fdsc_count++;
-
 #define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
 
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
@@ -218,7 +212,7 @@ static void *logwriter_thread(void *arg);
  */
 __EXPORT int sdlog2_main(int argc, char *argv[]);
 
-static bool copy_if_updated(orb_id_t topic, int handle, void *buffer);
+static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer);
 
 /**
  * Mainloop of sd log deamon.
@@ -811,14 +805,25 @@ int write_parameters(int fd)
 	return written;
 }
 
-bool copy_if_updated(orb_id_t topic, int handle, void *buffer)
+bool copy_if_updated(orb_id_t topic, int *handle, void *buffer)
 {
-	bool updated;
-
-	orb_check(handle, &updated);
+	bool updated = false;
+
+	if (*handle < 0) {
+		if (OK == orb_exists(topic, 0)) {
+			*handle = orb_subscribe(topic);
+			/* copy first data */
+			if (*handle >= 0) {
+				orb_copy(topic, *handle, buffer);
+				updated = true;
+			}
+		}
+	} else {
+		orb_check(*handle, &updated);
 
-	if (updated) {
-		orb_copy(topic, handle, buffer);
+		if (updated) {
+			orb_copy(topic, *handle, buffer);
+		}
 	}
 
 	return updated;
@@ -1121,54 +1126,47 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int mc_att_ctrl_status_sub;
 	} subs;
 
-	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
-	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
-	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
-	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
-	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
-	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
-	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
-	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
-	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
-	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
-	subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
-	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
-	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
-	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
-	subs.tsync_sub = orb_subscribe(ORB_ID(time_offset));
-	subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status));
-
-	/* we need to rate-limit wind, as we do not need the full update rate */
-	orb_set_interval(subs.wind_sub, 90);
-	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
+	subs.cmd_sub = -1;
+	subs.status_sub = -1;
+	subs.vtol_status_sub = -1;
+	subs.gps_pos_sub = -1;
+	subs.sensor_sub = -1;
+	subs.att_sub = -1;
+	subs.att_sp_sub = -1;
+	subs.rates_sp_sub = -1;
+	subs.act_outputs_sub = -1;
+	subs.act_controls_sub = -1;
+	subs.act_controls_1_sub = -1;
+	subs.local_pos_sub = -1;
+	subs.local_pos_sp_sub = -1;
+	subs.global_pos_sub = -1;
+	subs.triplet_sub = -1;
+	subs.vicon_pos_sub = -1;
+	subs.vision_pos_sub = -1;
+	subs.flow_sub = -1;
+	subs.rc_sub = -1;
+	subs.airspeed_sub = -1;
+	subs.esc_sub = -1;
+	subs.global_vel_sp_sub = -1;
+	subs.battery_sub = -1;
+	subs.range_finder_sub = -1;
+	subs.estimator_status_sub = -1;
+	subs.tecs_status_sub = -1;
+	subs.system_power_sub = -1;
+	subs.servorail_status_sub = -1;
+	subs.wind_sub = -1;
+	subs.tsync_sub = -1;
+	subs.mc_att_ctrl_status_sub = -1;
+	subs.encoders_sub = -1;
 
 	/* add new topics HERE */
 
 
-	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-		subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
-	}
-
-	if (_extended_logging) {
-		subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
-	} else {
-		subs.sat_info_sub = 0;
+	for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		subs.telemetry_subs[i] = -1;
 	}
+	
+	subs.sat_info_sub = -1;
 
 	/* close non-needed fd's */
 
@@ -1216,12 +1214,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		usleep(sleep_delay);
 
 		/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
-		if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
+		if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf.cmd)) {
 			handle_command(&buf.cmd);
 		}
 
 		/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
-		bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+		bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status);
 
 		if (status_updated) {
 			if (log_when_armed) {
@@ -1230,7 +1228,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GPS POSITION - LOG MANAGEMENT --- */
-		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
+		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
 			gps_time = buf_gps_pos.time_utc_usec;
@@ -1261,7 +1259,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VTOL VEHICLE STATUS --- */
-		if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) {
+		if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) {
 			log_msg.msg_type = LOG_VTOL_MSG;
 			log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot;
 			LOGBUFFER_WRITE_AND_COUNT(VTOL);
@@ -1292,7 +1290,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- SATELLITE INFO - UNIT #1 --- */
 		if (_extended_logging) {
 
-			if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
+			if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) {
 
 				/* log the SNR of each satellite for a detailed view of signal quality */
 				unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
@@ -1338,7 +1336,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- SENSOR COMBINED --- */
-		if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
+		if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
 			bool write_IMU = false;
 			bool write_IMU1 = false;
 			bool write_IMU2 = false;
@@ -1480,7 +1478,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ATTITUDE --- */
-		if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
+		if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) {
 			log_msg.msg_type = LOG_ATT_MSG;
 			log_msg.body.log_ATT.q_w = buf.att.q[0];
 			log_msg.body.log_ATT.q_x = buf.att.q[1];
@@ -1499,7 +1497,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ATTITUDE SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) {
 			log_msg.msg_type = LOG_ATSP_MSG;
 			log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
 			log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
@@ -1513,7 +1511,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- RATES SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), &subs.rates_sp_sub, &buf.rates_sp)) {
 			log_msg.msg_type = LOG_ARSP_MSG;
 			log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
 			log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
@@ -1522,14 +1520,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR OUTPUTS --- */
-		if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) {
+		if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) {
 			log_msg.msg_type = LOG_OUT0_MSG;
 			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
 			LOGBUFFER_WRITE_AND_COUNT(OUT0);
 		}
 
 		/* --- ACTUATOR CONTROL --- */
-		if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
+		if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &subs.act_controls_sub, &buf.act_controls)) {
 			log_msg.msg_type = LOG_ATTC_MSG;
 			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
 			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@@ -1539,7 +1537,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR CONTROL FW VTOL --- */
-		if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
+		if(copy_if_updated(ORB_ID(actuator_controls_1), &subs.act_controls_1_sub,&buf.act_controls)) {
 			log_msg.msg_type = LOG_ATC1_MSG;
 			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
 			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@@ -1549,7 +1547,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- LOCAL POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
+		if (copy_if_updated(ORB_ID(vehicle_local_position), &subs.local_pos_sub, &buf.local_pos)) {
 			log_msg.msg_type = LOG_LPOS_MSG;
 			log_msg.body.log_LPOS.x = buf.local_pos.x;
 			log_msg.body.log_LPOS.y = buf.local_pos.y;
@@ -1575,7 +1573,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- LOCAL POSITION SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), &subs.local_pos_sp_sub, &buf.local_pos_sp)) {
 			log_msg.msg_type = LOG_LPSP_MSG;
 			log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
 			log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
@@ -1591,7 +1589,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
+		if (copy_if_updated(ORB_ID(vehicle_global_position), &subs.global_pos_sub, &buf.global_pos)) {
 			log_msg.msg_type = LOG_GPOS_MSG;
 			log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
 			log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
@@ -1610,7 +1608,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL POSITION SETPOINT --- */
-		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
+		if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) {
 
 			if (buf.triplet.current.valid) {
 				log_msg.msg_type = LOG_GPSP_MSG;
@@ -1628,7 +1626,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VICON POSITION --- */
-		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
+		if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) {
 			log_msg.msg_type = LOG_VICN_MSG;
 			log_msg.body.log_VICN.x = buf.vicon_pos.x;
 			log_msg.body.log_VICN.y = buf.vicon_pos.y;
@@ -1640,7 +1638,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VISION POSITION --- */
-		if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
+		if (copy_if_updated(ORB_ID(vision_position_estimate), &subs.vision_pos_sub, &buf.vision_pos)) {
 			log_msg.msg_type = LOG_VISN_MSG;
 			log_msg.body.log_VISN.x = buf.vision_pos.x;
 			log_msg.body.log_VISN.y = buf.vision_pos.y;
@@ -1656,7 +1654,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- FLOW --- */
-		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
+		if (copy_if_updated(ORB_ID(optical_flow), &subs.flow_sub, &buf.flow)) {
 			log_msg.msg_type = LOG_FLOW_MSG;
 			log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
 			log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
@@ -1672,7 +1670,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- RC CHANNELS --- */
-		if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
+		if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) {
 			log_msg.msg_type = LOG_RC_MSG;
 			/* Copy only the first 8 channels of 14 */
 			memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
@@ -1682,7 +1680,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- AIRSPEED --- */
-		if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
+		if (copy_if_updated(ORB_ID(airspeed), &subs.airspeed_sub, &buf.airspeed)) {
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
@@ -1691,7 +1689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ESCs --- */
-		if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
+		if (copy_if_updated(ORB_ID(esc_status), &subs.esc_sub, &buf.esc)) {
 			for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
 				log_msg.msg_type = LOG_ESC_MSG;
 				log_msg.body.log_ESC.counter = buf.esc.counter;
@@ -1711,7 +1709,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL VELOCITY SETPOINT --- */
-		if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
+		if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), &subs.global_vel_sp_sub, &buf.global_vel_sp)) {
 			log_msg.msg_type = LOG_GVSP_MSG;
 			log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
 			log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
@@ -1720,7 +1718,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- BATTERY --- */
-		if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
+		if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) {
 			log_msg.msg_type = LOG_BATT_MSG;
 			log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 			log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
@@ -1730,7 +1728,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- SYSTEM POWER RAILS --- */
-		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+		if (copy_if_updated(ORB_ID(system_power), &subs.system_power_sub, &buf.system_power)) {
 			log_msg.msg_type = LOG_PWR_MSG;
 			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
 			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
@@ -1748,8 +1746,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TELEMETRY --- */
-		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-			if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
+		for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (copy_if_updated(telemetry_status_orb_id[i], &subs.telemetry_subs[i], &buf.telemetry)) {
 				log_msg.msg_type = LOG_TEL0_MSG + i;
 				log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
 				log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
@@ -1764,7 +1762,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- BOTTOM DISTANCE --- */
-		if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
+		if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) {
 			log_msg.msg_type = LOG_DIST_MSG;
 			log_msg.body.log_DIST.bottom = buf.range_finder.distance;
 			log_msg.body.log_DIST.bottom_rate = 0.0f;
@@ -1773,7 +1771,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ESTIMATOR STATUS --- */
-		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
+		if (copy_if_updated(ORB_ID(estimator_status), &subs.estimator_status_sub, &buf.estimator_status)) {
 			log_msg.msg_type = LOG_EST0_MSG;
 			unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
 			memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
@@ -1792,7 +1790,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TECS STATUS --- */
-		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
+		if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
 			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
@@ -1812,7 +1810,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- WIND ESTIMATE --- */
-		if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
+		if (copy_if_updated(ORB_ID(wind_estimate), &subs.wind_sub, &buf.wind_estimate)) {
 			log_msg.msg_type = LOG_WIND_MSG;
 			log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
 			log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
@@ -1822,7 +1820,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ENCODERS --- */
-		if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) {
+		if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) {
 			log_msg.msg_type = LOG_ENCD_MSG;
 			log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
 			log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
@@ -1832,14 +1830,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TIMESYNC OFFSET --- */
-		if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) {
+		if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) {
 			log_msg.msg_type = LOG_TSYN_MSG;
 			log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns;
 			LOGBUFFER_WRITE_AND_COUNT(TSYN);
 		}
 
 		/* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */
-		if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) {
+		if (copy_if_updated(ORB_ID(mc_att_ctrl_status), &subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) {
 			log_msg.msg_type = LOG_MACS_MSG;
 			log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ;
 			log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 3fc8627c1..4fbc0416e 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
@@ -246,6 +246,7 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 	struct rc_parameter_map_s _rc_parameter_map;
+	float _param_rc_values[RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
 
 	math::Matrix<3, 3>	_board_rotation;	/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_mag_rotation[3];		/**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -523,6 +524,7 @@ Sensors::Sensors() :
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
+	_param_rc_values{},
 	_board_rotation{},
 	_mag_rotation{},
 
@@ -620,6 +622,18 @@ 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.
+	(void)param_find("RC_CHAN_CNT");
+	(void)param_find("RC_TH_USER");
+	(void)param_find("CAL_MAG0_ID");
+	(void)param_find("CAL_MAG1_ID");
+	(void)param_find("CAL_MAG2_ID");
+	(void)param_find("CAL_MAG0_ROT");
+	(void)param_find("CAL_MAG1_ROT");
+	(void)param_find("CAL_MAG2_ROT");
+	(void)param_find("SYS_PARAM_VER");
+	
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -1382,12 +1396,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;
 						}
@@ -1448,12 +1462,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;
 						}
@@ -1571,12 +1585,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;
 						}
@@ -1609,7 +1623,8 @@ Sensors::parameter_update_poll(bool forced)
 			close(fd);
 		}
 
-		warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
+		/* do not output this for now, as its covered in preflight checks */
+		// warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
 	}
 }
 
@@ -1633,7 +1648,7 @@ Sensors::rc_parameter_map_poll(bool forced)
 
 			/* Set the handle by index if the index is set, otherwise use the id */
 			if (_rc_parameter_map.param_index[i] >= 0) {
-				_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
+				_parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
 
 			} else {
 				_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
@@ -1838,8 +1853,6 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 void
 Sensors::set_params_from_rc()
 {
-	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
-
 	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
 		if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
@@ -1851,8 +1864,8 @@ Sensors::set_params_from_rc()
 		float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
 		/* Check if the value has changed,
 		 * maybe we need to introduce a more aggressive limit here */
-		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
-			param_rc_values[i] = rc_val;
+		if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) {
+			_param_rc_values[i] = rc_val;
 			float param_val = math::constrain(
 						  _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
 						  _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 8dea6e6cc..4ec885ab3 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -92,7 +92,7 @@ struct param_wbuf_s {
 };
 
 // XXX this should be param_info_count, but need to work out linking
-uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {};
+uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {};
 
 /** flexible array holding modified parameter values */
 UT_array	*param_values;
@@ -265,8 +265,37 @@ param_count_used(void)
 param_t
 param_for_index(unsigned index)
 {
-	if (index < param_info_count)
+	if (index < param_info_count) {
 		return (param_t)index;
+	}
+
+	return PARAM_INVALID;
+}
+
+param_t
+param_for_used_index(unsigned index)
+{
+	if (index < param_info_count) {
+
+		/* walk all params and count */
+		int count = 0;
+
+		for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) {
+			for (unsigned j = 0; j < 8; j++) {
+				if (param_changed_storage[i] & (1 << j)) {
+
+					/* we found the right used count,
+					 * return the param value
+					 */
+					if (index == count) {
+						return (param_t)i;
+					}
+
+					count++;
+				}
+			}
+		}
+	}
 
 	return PARAM_INVALID;
 }
@@ -274,8 +303,9 @@ param_for_index(unsigned index)
 int
 param_get_index(param_t param)
 {
-	if (handle_in_range(param))
+	if (handle_in_range(param)) {
 		return (unsigned)param;
+	}
 
 	return -1;
 }
@@ -283,7 +313,9 @@ param_get_index(param_t param)
 int
 param_get_used_index(param_t param)
 {
-	if (!handle_in_range(param)) {
+	int param_storage_index = param_get_index(param);
+
+	if (param_storage_index < 0) {
 		return -1;
 	}
 
@@ -293,12 +325,17 @@ param_get_used_index(param_t param)
 	for (unsigned i = 0; i < (unsigned)param + 1; i++) {
 		for (unsigned j = 0; j < 8; j++) {
 			if (param_changed_storage[i] & (1 << j)) {
+
+				if (param_storage_index == i) {
+					return count;
+				}
+
 				count++;
 			}
 		}
 	}
 
-	return count;
+	return -1;
 }
 
 const char *
diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index b29a7e51d..9cbe3570b 100644
--- a/src/modules/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
@@ -130,6 +130,14 @@ __EXPORT bool		param_used(param_t param);
 __EXPORT param_t	param_for_index(unsigned index);
 
 /**
+ * Look up an used parameter by index.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index of the parameter in use, or -1 if the parameter does not exist.
+ */
+__EXPORT param_t	param_for_used_index(unsigned index);
+
+/**
  * Look up the index of a parameter.
  *
  * @param param		The parameter to obtain the index for.
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index b4f81d429..d531c6b0b 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -41,6 +41,7 @@
 #include <drivers/device/device.h>
 
 #include <sys/types.h>
+#include <sys/stat.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <string.h>
@@ -1196,6 +1197,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 
 } // namespace
 
+int
+orb_exists(const struct orb_metadata *meta, int instance)
+{
+	/*
+	 * Generate the path to the node and try to open it.
+	 */
+	char path[orb_maxpath];
+	int inst = instance;
+	int ret = node_mkpath(path, PUBSUB, meta, &inst);
+
+	if (ret != OK) {
+		errno = -ret;
+		return ERROR;
+	}
+
+	struct stat buffer;
+	return stat(path, &buffer);
+}
+
 orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
 {
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 44dc6614f..a8b19d91f 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -319,6 +319,15 @@ extern int	orb_check(int handle, bool *updated) __EXPORT;
 extern int	orb_stat(int handle, uint64_t *time) __EXPORT;
 
 /**
+ * Check if a topic has already been created.
+ *
+ * @param meta		ORB topic metadata.
+ * @param instance	ORB instance
+ * @return		OK if the topic exists, ERROR otherwise with errno set accordingly.
+ */
+extern int	orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
+
+/**
  * Return the priority of the topic
  *
  * @param handle	A handle returned from orb_subscribe.
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 6b855cf58..98443e716 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -284,9 +284,9 @@ do_show_print(void *arg, param_t param)
 		}
 	}
 
-	printf("%c %c %s: ", (param_used(param) ? 'x' : ' '),
+	printf("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '),
 	       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
-	       param_name(param));
+	       param_name(param), param_get_used_index(param), param_get_index(param));
 
 	/*
 	 * This case can be expanded to handle printing common structure types.