Merge branch 'master' into sdlog2_ver

21 files changed, 903 insertions, 614 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index cfa7d9e8a..5eeca5a1a 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -100,10 +100,29 @@
  * accel_T = A^-1 * g
  * g = 9.80665
  *
+ * ===== Rotation =====
+ *
+ * Calibrating using model:
+ * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
+ *
+ * Actual correction:
+ * accel_corr = rot * accel_T * (accel_raw - accel_offs)
+ *
+ * Known: accel_T_r, accel_offs_r, rot
+ * Unknown: accel_T, accel_offs
+ *
+ * Solution:
+ * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
+ * => accel_T = rot^-1 * accel_T_r * rot
+ * => accel_offs = rot^-1 * accel_offs_r
+ *
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include "accelerometer_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <unistd.h>
@@ -112,11 +131,13 @@
 #include <fcntl.h>
 #include <sys/prctl.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
 #include <geo/geo.h>
+#include <conversion/rotation.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
@@ -127,93 +148,122 @@
 #endif
 static const int ERROR = -1;
 
-int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+static const char *sensor_name = "accel";
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
 int detect_orientation(int mavlink_fd, int sub_sensor_combined);
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
 int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
 
-int do_accel_calibration(int mavlink_fd) {
-	/* announce change */
-	mavlink_log_info(mavlink_fd, "accel calibration started");
-	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
+int do_accel_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+
+	struct accel_scale accel_scale = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	int res = OK;
+
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+	close(fd);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
 
-	/* measure and calculate offsets & scales */
 	float accel_offs[3];
-	float accel_scale[3];
-	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
+	float accel_T[3][3];
 
 	if (res == OK) {
-		/* measurements complete successfully, set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
+		/* measure and calculate offsets & scales */
+		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
+	}
+
+	if (res == OK) {
+		/* measurements completed successfully, rotate calibration values */
+		param_t board_rotation_h = param_find("SENS_BOARD_ROT");
+		int32_t board_rotation_int;
+		param_get(board_rotation_h, &(board_rotation_int));
+		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
+		math::Matrix board_rotation(3, 3);
+		get_rot_matrix(board_rotation_id, &board_rotation);
+		math::Matrix board_rotation_t = board_rotation.transpose();
+		math::Vector3 accel_offs_vec;
+		accel_offs_vec.set(&accel_offs[0]);
+		math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
+		math::Matrix accel_T_mat(3, 3);
+		accel_T_mat.set(&accel_T[0][0]);
+		math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+
+		accel_scale.x_offset = accel_offs_rotated(0);
+		accel_scale.x_scale = accel_T_rotated(0, 0);
+		accel_scale.y_offset = accel_offs_rotated(1);
+		accel_scale.y_scale = accel_T_rotated(1, 1);
+		accel_scale.z_offset = accel_offs_rotated(2);
+		accel_scale.z_scale = accel_T_rotated(2, 2);
+
+		/* set parameters */
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
+		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
+		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
+		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
+		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
+		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			res = ERROR;
 		}
+	}
 
+	if (res == OK) {
+		/* apply new scaling and offsets */
 		int fd = open(ACCEL_DEVICE_PATH, 0);
-		struct accel_scale ascale = {
-			accel_offs[0],
-			accel_scale[0],
-			accel_offs[1],
-			accel_scale[1],
-			accel_offs[2],
-			accel_scale[2],
-		};
-
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-			warn("WARNING: failed to set scale / offsets for accel");
-
+		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 		close(fd);
 
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+		}
+	}
+
+	if (res == OK) {
 		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
+		res = param_save_default();
 
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 		}
+	}
+
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-		return OK;
 	} else {
-		/* measurements error */
-		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		return ERROR;
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 	}
 
-	/* exit accel calibration mode */
+	return res;
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
+{
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
 	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
 
-	/* reset existing calibration */
-	int fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-	int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
-	close(fd);
-
-	if (OK != ioctl_res) {
-		warn("ERROR: failed to set scale / offsets for accel");
-		return ERROR;
-	}
-
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
 	unsigned done_count = 0;
+	int res = OK;
 
 	while (true) {
 		bool done = true;
@@ -221,64 +271,63 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		done_count = 0;
 
 		for (int i = 0; i < 6; i++) {
-			if (!data_collected[i]) {
+			if (data_collected[i]) {
+				done_count++;
+
+			} else {
 				done = false;
 			}
 		}
 
-		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
-			(!data_collected[0]) ? "x+ " : "",
-			(!data_collected[1]) ? "x- " : "",
-			(!data_collected[2]) ? "y+ " : "",
-			(!data_collected[3]) ? "y- " : "",
-			(!data_collected[4]) ? "z+ " : "",
-			(!data_collected[5]) ? "z- " : "");
-
 		if (old_done_count != done_count)
-			mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
 
 		if (done)
 			break;
 
+		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
+				 (!data_collected[0]) ? "x+ " : "",
+				 (!data_collected[1]) ? "x- " : "",
+				 (!data_collected[2]) ? "y+ " : "",
+				 (!data_collected[3]) ? "y- " : "",
+				 (!data_collected[4]) ? "z+ " : "",
+				 (!data_collected[5]) ? "z- " : "");
+
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+
 		if (orient < 0) {
-			close(sensor_combined_sub);
-			return ERROR;
+			res = ERROR;
+			break;
 		}
 
 		if (data_collected[orient]) {
-			mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
+			mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
 			continue;
 		}
 
 		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
 		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
-			(double)accel_ref[orient][0],
-			(double)accel_ref[orient][1],
-			(double)accel_ref[orient][2]);
+				 (double)accel_ref[orient][0],
+				 (double)accel_ref[orient][1],
+				 (double)accel_ref[orient][2]);
 
 		data_collected[orient] = true;
 		tune_neutral();
 	}
+
 	close(sensor_combined_sub);
 
-	/* calculate offsets and rotation+scale matrix */
-	float accel_T[3][3];
-	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
-	if (res != 0) {
-		mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
-		return ERROR;
-	}
+	if (res == OK) {
+		/* calculate offsets and transform matrix */
+		res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-	/* convert accel transform matrix to scales,
-	 * rotation part of transform matrix is not used by now
-	 */
-	for (int i = 0; i < 3; i++) {
-		accel_scale[i] = accel_T[i][i];
+		if (res != OK) {
+			mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
+		}
 	}
 
-	return OK;
+	return res;
 }
 
 /*
@@ -287,14 +336,15 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
  * @return 0..5 according to orientation when vehicle is still and ready for measurements,
  * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
  */
-int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
+int detect_orientation(int mavlink_fd, int sub_sensor_combined)
+{
 	struct sensor_combined_s sensor;
 	/* exponential moving average of accel */
 	float accel_ema[3] = { 0.0f, 0.0f, 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.2f;
+	float ema_len = 0.5f;
 	/* set "still" threshold to 0.25 m/s^2 */
 	float still_thr2 = pow(0.25f, 2);
 	/* set accel error threshold to 5m/s^2 */
@@ -318,30 +368,38 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	while (true) {
 		/* wait blocking for new data */
 		int poll_ret = poll(fds, 1, 1000);
+
 		if (poll_ret) {
 			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
 			t = hrt_absolute_time();
 			float dt = (t - t_prev) / 1000000.0f;
 			t_prev = t;
 			float w = dt / ema_len;
+
 			for (int i = 0; i < 3; i++) {
-				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
-				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
+				float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
+				accel_ema[i] += d * w;
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
+
+				if (d > still_thr2 * 8.0f)
+					d = still_thr2 * 8.0f;
+
 				if (d > accel_disp[i])
 					accel_disp[i] = d;
 			}
+
 			/* still detector with hysteresis */
-			if (  accel_disp[0] < still_thr2 &&
-				  accel_disp[1] < still_thr2 &&
-				  accel_disp[2] < still_thr2 ) {
+			if (accel_disp[0] < still_thr2 &&
+			    accel_disp[1] < still_thr2 &&
+			    accel_disp[2] < still_thr2) {
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
 					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
 					t_still = t;
 					t_timeout = t + timeout;
+
 				} else {
 					/* still since t_still */
 					if (t > t_still + still_time) {
@@ -349,62 +407,71 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 						break;
 					}
 				}
-			} else if ( accel_disp[0] > still_thr2 * 2.0f ||
-					    accel_disp[1] > still_thr2 * 2.0f ||
-					    accel_disp[2] > still_thr2 * 2.0f) {
+
+			} else if (accel_disp[0] > still_thr2 * 4.0f ||
+				   accel_disp[1] > still_thr2 * 4.0f ||
+				   accel_disp[2] > still_thr2 * 4.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
+					mavlink_log_info(mavlink_fd, "detected motion, hold still...");
 					t_still = 0;
 				}
 			}
+
 		} else if (poll_ret == 0) {
 			poll_errcount++;
 		}
+
 		if (t > t_timeout) {
 			poll_errcount++;
 		}
 
 		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
-			return -1;
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			return ERROR;
 		}
 	}
 
-	if (  fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 0;	// [ g, 0, 0 ]
-	if (  fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 1;	// [ -g, 0, 0 ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 2;	// [ 0, g, 0 ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 3;	// [ 0, -g, 0 ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
 		return 4;	// [ 0, 0, g ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
 		return 5;	// [ 0, 0, -g ]
 
-	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+	mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
 
-	return -2;	// Can't detect orientation
+	return ERROR;	// Can't detect orientation
 }
 
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num)
+{
 	struct pollfd fds[1];
 	fds[0].fd = sensor_combined_sub;
 	fds[0].events = POLLIN;
@@ -415,12 +482,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 
 	while (count < samples_num) {
 		int poll_ret = poll(fds, 1, 1000);
+
 		if (poll_ret == 1) {
 			struct sensor_combined_s sensor;
 			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
 			for (int i = 0; i < 3; i++)
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
+
 			count++;
+
 		} else {
 			errcount++;
 			continue;
@@ -437,10 +508,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 	return OK;
 }
 
-int mat_invert3(float src[3][3], float dst[3][3]) {
+int mat_invert3(float src[3][3], float dst[3][3])
+{
 	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
-			    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
-			    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+
 	if (det == 0.0f)
 		return ERROR;	// Singular matrix
 
@@ -457,7 +530,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
 	return OK;
 }
 
-int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g)
+{
 	/* calculate offsets */
 	for (int i = 0; i < 3; i++) {
 		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
@@ -466,6 +540,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 	/* fill matrix A for linear equations system*/
 	float mat_A[3][3];
 	memset(mat_A, 0, sizeof(mat_A));
+
 	for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
 			float a = accel_ref[i * 2][j] - accel_offs[j];
@@ -475,6 +550,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 
 	/* calculate inverse matrix for A */
 	float mat_A_inv[3][3];
+
 	if (mat_invert3(mat_A, mat_A_inv) != OK)
 		return ERROR;
 
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
new file mode 100644
index 000000000..fd8b8564d
--- /dev/null
+++ b/src/modules/commander/calibration_messages.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file calibration_messages.h
+ *
+ * Common calibration messages.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#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>"
+
+#define CAL_FAILED_SENSOR_MSG	"ERROR: failed reading sensor"
+#define CAL_FAILED_RESET_CAL_MSG	"ERROR: failed to reset calibration"
+#define CAL_FAILED_APPLY_CAL_MSG	"ERROR: failed to apply calibration"
+#define CAL_FAILED_SET_PARAMS_MSG	"ERROR: failed to set parameters"
+#define CAL_FAILED_SAVE_PARAMS_MSG	"ERROR: failed to save parameters"
+
+#endif /* CALIBRATION_MESSAGES_H_ */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2ef509980..9814a7bcc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
 				/* reset the arming mode to disarmed */
 				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+
 				if (arming_res != TRANSITION_DENIED) {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+
 				} else {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
 				}
@@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			break;
 		}
 
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
-	{
-		transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-		if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-			if (safety->safety_switch_available && !safety->safety_off) {
-				print_reject_arm("NOT ARMING: Press safety switch first.");
-				arming_res = TRANSITION_DENIED;
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
+			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
-			} else {
-				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
-			}
+			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+				if (safety->safety_switch_available && !safety->safety_off) {
+					print_reject_arm("NOT ARMING: Press safety switch first.");
+					arming_res = TRANSITION_DENIED;
 
-			if (arming_res == TRANSITION_CHANGED) {
-				mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-			} else {
-				mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				} else {
+					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+				}
+
+				if (arming_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
 			}
 		}
-	}
 		break;
 
 	default:
@@ -687,7 +690,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
-	bool rc_calibration_ok = (OK == rc_calibration_check());
+	bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -802,7 +805,7 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				/* re-check RC calibration */
-				rc_calibration_ok = (OK == rc_calibration_check());
+				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
 
 				/* navigation parameters */
 				param_get(_param_takeoff_alt, &takeoff_alt);
@@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[])
 			last_idle_time = system_load.tasks[0].total_runtime;
 
 			/* check if board is connected via USB */
-			struct stat statbuf;
+			//struct stat statbuf;
 			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
@@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (armed.armed) {
 					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+
 				} else {
 					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
@@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[])
 		counter++;
 
 		int blink_state = blink_msg_state();
+
 		if (blink_state > 0) {
 			/* blinking LED message, don't touch LEDs */
 			if (blink_state == 2) {
 				/* blinking LED message completed, restore normal state */
 				control_status_leds(&status, &armed, true);
 			}
+
 		} else {
 			/* normal state */
 			control_status_leds(&status, &armed, status_changed);
@@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
 	ret = pthread_join(commander_low_prio_thread, NULL);
 
 	if (ret) {
-		warn("join failed", ret);
+		warn("join failed: %d", ret);
 	}
 
 	rgbled_set_mode(RGBLED_MODE_OFF);
@@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
 	/* driving rgbled */
 	if (changed) {
 		bool set_normal_color = false;
+
 		/* set mode */
 		if (status->arming_state == ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
@@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
 				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
 					rgbled_set_color(RGBLED_COLOR_AMBER);
 				}
+
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
@@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg)
 	fds[0].events = POLLIN;
 
 	while (!thread_should_exit) {
-
-		/* wait for up to 100ms for data */
+		/* wait for up to 200ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
+		/* timed out - periodic check for thread_should_exit, etc. */
 		if (pret == 0)
 			continue;
 
@@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg)
 
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
-					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_rc_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
@@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg)
 			/* send acknowledge command */
 			// XXX TODO
 		}
-
 	}
 
 	close(cmd_sub);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 369e6da62..30cd0d48d 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -33,10 +33,12 @@
 
 /**
  * @file gyro_calibration.cpp
+ *
  * Gyroscope calibration routine
  */
 
 #include "gyro_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
@@ -56,21 +58,14 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "gyro";
+
 int do_gyro_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
-
-	const unsigned calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
-	unsigned calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
+	struct gyro_scale gyro_scale = {
 		0.0f,
 		1.0f,
 		0.0f,
@@ -79,97 +74,100 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,
 	};
 
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
+	int res = OK;
 
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
 	close(fd);
 
-	unsigned poll_errcount = 0;
-
-	while (calibration_counter < calibration_count) {
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
 
-		/* wait blocking for new data */
-		struct pollfd fds[1];
-		fds[0].fd = sub_sensor_combined;
-		fds[0].events = POLLIN;
+	if (res == OK) {
+		/* determine gyro mean values */
+		const unsigned calibration_count = 5000;
+		unsigned calibration_counter = 0;
+		unsigned poll_errcount = 0;
+
+		/* subscribe to gyro sensor topic */
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		struct gyro_report gyro_report;
+
+		while (calibration_counter < calibration_count) {
+			/* wait blocking for new data */
+			struct pollfd fds[1];
+			fds[0].fd = sub_sensor_gyro;
+			fds[0].events = POLLIN;
+
+			int poll_ret = poll(fds, 1, 1000);
+
+			if (poll_ret > 0) {
+				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				gyro_scale.x_offset += gyro_report.x;
+				gyro_scale.y_offset += gyro_report.y;
+				gyro_scale.z_offset += gyro_report.z;
+				calibration_counter++;
+
+				if (calibration_counter % (calibration_count / 20) == 0)
+					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+
+			} else {
+				poll_errcount++;
+			}
+
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
+		}
 
-		int poll_ret = poll(fds, 1, 1000);
+		close(sub_sensor_gyro);
 
-		if (poll_ret > 0) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			gyro_offset[0] += raw.gyro_rad_s[0];
-			gyro_offset[1] += raw.gyro_rad_s[1];
-			gyro_offset[2] += raw.gyro_rad_s[2];
-			calibration_counter++;
-			if (calibration_counter % (calibration_count / 20) == 0)
-				mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
-
-		} else {
-			poll_errcount++;
-		}
+		gyro_scale.x_offset /= calibration_count;
+		gyro_scale.y_offset /= calibration_count;
+		gyro_scale.z_offset /= calibration_count;
+	}
 
-		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
-			close(sub_sensor_combined);
-			return ERROR;
+	if (res == OK) {
+		/* check offsets */
+		if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
+			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
+			res = ERROR;
 		}
 	}
 
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
-
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
+	if (res == OK) {
+		/* set offset parameters to new values */
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
+		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
+		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
+			res = ERROR;
 		}
+	}
 
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
+#if 0
+	/* beep on offset calibration end */
+	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
+	tune_neutral();
 
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
+	/* scale calibration */
+	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
 
-		if (save_ret != 0) {
-			warnx("WARNING: auto-save of params to storage failed");
-			mavlink_log_critical(mavlink_fd, "gyro store failed");
-			close(sub_sensor_combined);
-			return ERROR;
-		}
+	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
+	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
 
-		tune_neutral();
-		/* third beep by cal end routine */
+	/* apply new offsets */
+	fd = open(GYRO_DEVICE_PATH, 0);
 
-	} else {
-		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
-		close(sub_sensor_combined);
-		return ERROR;
-	}
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+		warn("WARNING: failed to apply new offsets for gyro");
 
-	mavlink_log_info(mavlink_fd, "offset calibration done.");
+	close(fd);
 
-#if 0
-	/*** --- SCALING --- ***/
-
-	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
-	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
 
 	unsigned rotations_count = 30;
 	float gyro_integral = 0.0f;
@@ -178,9 +176,11 @@ int do_gyro_calibration(int mavlink_fd)
 	// XXX change to mag topic
 	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
 
-	float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
-	if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
-	if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
+	float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
+
+	if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
+
+	if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
 
 
 	uint64_t last_time = hrt_absolute_time();
@@ -190,7 +190,7 @@ int do_gyro_calibration(int mavlink_fd)
 
 		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
 		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
-			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
+		    && (hrt_absolute_time() - start_time > 5 * 1e6)) {
 			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
 			close(sub_sensor_combined);
 			return OK;
@@ -218,14 +218,17 @@ int do_gyro_calibration(int mavlink_fd)
 			// calculate error between estimate and measurement
 			// apply declination correction for true heading as well.
 			//float mag = -atan2f(magNav(1),magNav(0));
-			float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
-			if (mag > M_PI_F) mag -= 2*M_PI_F;
-			if (mag < -M_PI_F) mag += 2*M_PI_F;
+			float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
+
+			if (mag > M_PI_F) mag -= 2 * M_PI_F;
+
+			if (mag < -M_PI_F) mag += 2 * M_PI_F;
 
 			float diff = mag - mag_last;
 
-			if (diff > M_PI_F) diff -= 2*M_PI_F;
-			if (diff < -M_PI_F) diff += 2*M_PI_F;
+			if (diff > M_PI_F) diff -= 2 * M_PI_F;
+
+			if (diff < -M_PI_F) diff += 2 * M_PI_F;
 
 			baseline_integral += diff;
 			mag_last = mag;
@@ -235,63 +238,68 @@ int do_gyro_calibration(int mavlink_fd)
 
 //			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
 
-		// } else if (poll_ret == 0) {
-		// 	/* any poll failure for 1s is a reason to abort */
-		// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-		// 	return;
+			// } else if (poll_ret == 0) {
+			// 	/* any poll failure for 1s is a reason to abort */
+			// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
+			// 	return;
 		}
 	}
 
 	float gyro_scale = baseline_integral / gyro_integral;
-	
+
 	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
-#else
-	float gyro_scales[] = { 1.0f, 1.0f, 1.0f };
-#endif
 
-	
 
+	if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
+		mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
+		close(sub_sensor_gyro);
+		mavlink_log_critical(mavlink_fd, "gyro calibration failed");
+		return ERROR;
+	}
 
-	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
+	/* beep on calibration end */
+	mavlink_log_info(mavlink_fd, "gyro scale calibration done");
+	tune_neutral();
 
-		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
-			|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
-			|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
+#endif
+
+	if (res == OK) {
+		/* set scale parameters to new values */
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
+		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
+		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
+			res = ERROR;
 		}
+	}
 
-		/* set offsets to actual value */
+	if (res == OK) {
+		/* apply new scaling and offsets */
 		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			gyro_scales[0],
-			gyro_offset[1],
-			gyro_scales[1],
-			gyro_offset[2],
-			gyro_scales[2],
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
+		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
 		close(fd);
 
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+		}
+	}
+
+	if (res == OK) {
 		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
+		res = param_save_default();
 
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 		}
+	}
 
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
-		/* third beep by cal end routine */
-		close(sub_sensor_combined);
-		return OK;
 	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-		close(sub_sensor_combined);
-		return ERROR;
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 	}
+
+	return res;
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index b0d4266be..09f4104f8 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -33,12 +33,14 @@
 
 /**
  * @file mag_calibration.cpp
+ *
  * Magnetometer calibration routine
  */
 
 #include "mag_calibration.h"
 #include "commander_helper.h"
 #include "calibration_routines.h"
+#include "calibration_messages.h"
 
 #include <stdio.h>
 #include <stdlib.h>
@@ -59,26 +61,20 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "mag";
+
 int do_mag_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
 	/* 45 seconds */
 	uint64_t calibration_interval = 45 * 1000 * 1000;
 
-	/* maximum 2000 values */
+	/* maximum 500 values */
 	const unsigned int calibration_maxcount = 500;
 	unsigned int calibration_counter = 0;
 
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
 	struct mag_scale mscale_null = {
 		0.0f,
 		1.0f,
@@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd)
 		1.0f,
 	};
 
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
-	}
+	int res = OK;
 
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
+	/* erase old calibration */
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
-	close(fd);
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
 
-	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+	if (res == OK) {
+		/* calibrate range */
+		res = ioctl(fd, MAGIOCCALIBRATE, fd);
 
-	/* calibrate offsets */
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
+		}
+	}
 
-	// uint64_t calibration_start = hrt_absolute_time();
+	close(fd);
 
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+	float *x;
+	float *y;
+	float *z;
 
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
+	if (res == OK) {
+		/* allocate memory */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
 
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
+		x = (float *)malloc(sizeof(float) * calibration_maxcount);
+		y = (float *)malloc(sizeof(float) * calibration_maxcount);
+		z = (float *)malloc(sizeof(float) * calibration_maxcount);
 
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return ERROR;
+		if (x == NULL || y == NULL || z == NULL) {
+			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+			res = ERROR;
+		}
 	}
 
-	mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
+	if (res == OK) {
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		struct mag_report mag;
 
-	unsigned poll_errcount = 0;
+		/* limit update rate to get equally spaced measurements over time (in ms) */
+		orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
 
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
+		/* calibrate offsets */
+		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+		unsigned poll_errcount = 0;
 
-		/* wait blocking for new data */
-		struct pollfd fds[1];
-		fds[0].fd = sub_mag;
-		fds[0].events = POLLIN;
+		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
 
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
+		while (hrt_absolute_time() < calibration_deadline &&
+		       calibration_counter < calibration_maxcount) {
 
-			axis_index++;
+			/* wait blocking for new data */
+			struct pollfd fds[1];
+			fds[0].fd = sub_mag;
+			fds[0].events = POLLIN;
 
-			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
-			tune_neutral();
+			int poll_ret = poll(fds, 1, 1000);
 
-			axis_deadline += calibration_interval / 3;
-		}
+			if (poll_ret > 0) {
+				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
 
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret > 0) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+				x[calibration_counter] = mag.x;
+				y[calibration_counter] = mag.y;
+				z[calibration_counter] = mag.z;
 
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
+				calibration_counter++;
 
-			calibration_counter++;
-			if (calibration_counter % (calibration_maxcount / 20) == 0)
-				mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
+				if (calibration_counter % (calibration_maxcount / 20) == 0)
+					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
 
+			} else {
+				poll_errcount++;
+			}
 
-		} else {
-			poll_errcount++;
-		}
-
-		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
-			close(sub_mag);
-			free(x);
-			free(y);
-			free(z);
-			return ERROR;
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
 		}
 
-
+		close(sub_mag);
 	}
 
 	float sphere_x;
@@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd)
 	float sphere_z;
 	float sphere_radius;
 
-	mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-	mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
+	if (res == OK) {
+		/* sphere fit */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
+		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
 
-	free(x);
-	free(y);
-	free(z);
+		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
+			mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
+			res = ERROR;
+		}
+	}
 
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
+	if (x != NULL)
+		free(x);
 
-		fd = open(MAG_DEVICE_PATH, 0);
+	if (y != NULL)
+		free(y);
 
-		struct mag_scale mscale;
+	if (z != NULL)
+		free(z);
 
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
+	if (res == OK) {
+		/* apply calibration and set parameters */
+		struct mag_scale mscale;
 
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
+		fd = open(MAG_DEVICE_PATH, 0);
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
 
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
+		}
 
-		close(fd);
+		if (res == OK) {
+			mscale.x_offset = sphere_x;
+			mscale.y_offset = sphere_y;
+			mscale.z_offset = sphere_z;
 
-		/* announce and set new offset */
+			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
 
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
 		}
 
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
+		close(fd);
 
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
+		if (res == OK) {
+			/* set parameters */
+			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
+				res = ERROR;
 
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+				res = ERROR;
 
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
+			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
+				res = ERROR;
 
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
+			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
+				res = ERROR;
 
-		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
+			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
+				res = ERROR;
 
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			}
 
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
-			close(sub_mag);
-			return ERROR;
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
 		}
 
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
+		if (res == OK) {
+			/* auto-save to EEPROM */
+			res = param_save_default();
 
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			}
+		}
 
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+				 (double)mscale.y_offset, (double)mscale.z_offset);
+		mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+				 (double)mscale.y_scale, (double)mscale.z_scale);
 
-		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
-		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
+		if (res == OK) {
+			mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
-		close(sub_mag);
-		return OK;
-		/* third beep by cal end routine */
+		} else {
+			mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		}
 
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-		close(sub_mag);
-		return ERROR;
+		return res;
 	}
 }
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 91d75121e..554dfcb08 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -47,4 +47,3 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp
-
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index edb21c7ab..61a54170a 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -610,6 +610,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(120);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1278,7 +1287,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
index b1dc39445..96a443c6e 100644
--- a/src/modules/segway/BlockSegwayController.cpp
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -26,7 +26,7 @@ void BlockSegwayController::update() {
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.main_state == MAIN_STATE_AUTO) {
 		// update guidance
 	}
 
@@ -34,17 +34,18 @@ void BlockSegwayController::update() {
 	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.main_state == MAIN_STATE_AUTO ||
+	    _status.main_state == MAIN_STATE_SEATBELT ||
+	    _status.main_state == MAIN_STATE_EASY) {
 		_actuators.control[0] = spdCmd;
 		_actuators.control[1] = spdCmd;
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+	} else if (_status.main_state == MAIN_STATE_MANUAL) {
+		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
 			_actuators.control[CH_LEFT] = _manual.throttle;
 			_actuators.control[CH_RIGHT] = _manual.pitch;
 
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
 			_actuators.control[0] = spdCmd;
 			_actuators.control[1] = spdCmd;
 		}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 4d719e0e1..8875f65fc 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -44,8 +44,34 @@
 
 #include <systemlib/param/param.h>
 
+/**
+ * Gyro X offset FIXME
+ *
+ * This is an X-axis offset for the gyro.
+ * Adjust it according to the calibration data.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+
+/**
+ * Gyro Y offset FIXME with dot.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+
+/**
+ * Gyro Z offset FIXME
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 3fcacaf8f..1b79de8fd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -67,6 +67,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
+#include <conversion/rotation.h>
 
 #include <systemlib/airspeed.h>
 
@@ -136,75 +137,6 @@
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
- * Enum for board and external compass rotations.
- * This enum maps from board attitude to airframe attitude.
- */
-enum Rotation {
-	ROTATION_NONE                = 0,
-	ROTATION_YAW_45              = 1,
-	ROTATION_YAW_90              = 2,
-	ROTATION_YAW_135             = 3,
-	ROTATION_YAW_180             = 4,
-	ROTATION_YAW_225             = 5,
-	ROTATION_YAW_270             = 6,
-	ROTATION_YAW_315             = 7,
-	ROTATION_ROLL_180            = 8,
-	ROTATION_ROLL_180_YAW_45     = 9,
-	ROTATION_ROLL_180_YAW_90     = 10,
-	ROTATION_ROLL_180_YAW_135    = 11,
-	ROTATION_PITCH_180           = 12,
-	ROTATION_ROLL_180_YAW_225    = 13,
-	ROTATION_ROLL_180_YAW_270    = 14,
-	ROTATION_ROLL_180_YAW_315    = 15,
-	ROTATION_ROLL_90             = 16,
-	ROTATION_ROLL_90_YAW_45      = 17,
-	ROTATION_ROLL_90_YAW_90      = 18,
-	ROTATION_ROLL_90_YAW_135     = 19,
-	ROTATION_ROLL_270            = 20,
-	ROTATION_ROLL_270_YAW_45     = 21,
-	ROTATION_ROLL_270_YAW_90     = 22,
-	ROTATION_ROLL_270_YAW_135    = 23,
-	ROTATION_PITCH_90            = 24,
-	ROTATION_PITCH_270           = 25,
-	ROTATION_MAX
-};
-
-typedef struct {
-	uint16_t roll;
-	uint16_t pitch;
-	uint16_t yaw;
-} rot_lookup_t;
-
-const rot_lookup_t rot_lookup[] = {
-	{  0,   0,   0 },
-	{  0,   0,  45 },
-	{  0,   0,  90 },
-	{  0,   0, 135 },
-	{  0,   0, 180 },
-	{  0,   0, 225 },
-	{  0,   0, 270 },
-	{  0,   0, 315 },
-	{180,   0,   0 },
-	{180,   0,  45 },
-	{180,   0,  90 },
-	{180,   0, 135 },
-	{  0, 180,   0 },
-	{180,   0, 225 },
-	{180,   0, 270 },
-	{180,   0, 315 },
-	{ 90,   0,   0 },
-	{ 90,   0,  45 },
-	{ 90,   0,  90 },
-	{ 90,   0, 135 },
-	{270,   0,   0 },
-	{270,   0,  45 },
-	{270,   0,  90 },
-	{270,   0, 135 },
-	{  0,  90,   0 },
-	{  0, 270,   0 }
-};
-
-/**
  * Sensor app start / stop handling function
  *
  * @ingroup apps
@@ -385,11 +317,6 @@ private:
 	int		parameters_update();
 
 	/**
-	 * Get the rotation matrices
-	 */
-	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
-
-	/**
 	 * Do accel-related initialisation.
 	 */
 	void		accel_init();
@@ -804,24 +731,6 @@ Sensors::parameters_update()
 }
 
 void
-Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
-{
-	/* first set to zero */
-	rot_matrix->Matrix::zero(3, 3);
-
-	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
-	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
-	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
-
-	math::EulerAngles euler(roll, pitch, yaw);
-
-	math::Dcm R(euler);
-
-	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-			(*rot_matrix)(i, j) = R(i, j);
-}
-
-void
 Sensors::accel_init()
 {
 	int	fd;
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index b1bb1a66d..cce46bf5f 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -50,6 +50,8 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer.h"
 
@@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
 	return 0;
 }
 
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+		/* skip up to it AND one beyond - could be on the NUL symbol now */
+		buflen -= (p - buf) + 1;
+		return p + 1;
+	}
+
+	return nullptr;
+}
+
 /****************************************************************************/
 
 NullMixer::NullMixer() :
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index bbfa130a9..723bf9f3b 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -128,7 +128,9 @@
 #ifndef _SYSTEMLIB_MIXER_MIXER_H
 #define _SYSTEMLIB_MIXER_MIXER_H value
 
+#include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+#include "mixer_load.h"
 
 /**
  * Abstract class defining a mixer mixing zero or more inputs to
@@ -210,6 +212,24 @@ protected:
 	 */
 	static int			scale_check(struct mixer_scaler_s &scaler);
 
+	/**
+	 * Find a tag
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @param tag			character to search for.
+	 */
+	static const char *		findtag(const char *buf, unsigned &buflen, char tag);
+
+	/**
+	 * Skip a line
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @return			0 / OK if a line could be skipped, 1 else
+	 */
+	static const char *		skipline(const char *buf, unsigned &buflen);
+
 private:
 };
 
@@ -239,6 +259,11 @@ public:
 	void				reset();
 
 	/**
+	 * Count the mixers in the group.
+	 */
+	unsigned			count();
+
+	/**
 	 * Adds mixers to the group based on a text description in a buffer.
 	 *
 	 * Mixer definitions begin with a single capital letter and a colon.
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 1dbc512cd..3ed99fba0 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/src/modules/systemlib/mixer/mixer_group.cpp
@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
 	return index;
 }
 
+unsigned
+MixerGroup::count()
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr)) {
+		mixer = mixer->_next;
+		index++;
+	}
+
+	return index;
+}
+
 void
 MixerGroup::groups_required(uint32_t &groups)
 {
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 
 			/* only adjust buflen if parsing was successful */
 			buflen = resid;
+			debug("SUCCESS - buflen: %d", buflen);
 		} else {
 
 			/*
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
new file mode 100644
index 000000000..a55ddf8a3
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_load.c
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <nuttx/config.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
+{
+	FILE		*fp;
+	char		line[120];
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL) {
+		return 1;
+	}
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+			continue;
+
+		/* compact whitespace in the buffer */
+		char *t, *f;
+		for (f = line; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+				while (*t == ' ')
+					t++;
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+			return 1;
+		}
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	return 0;
+}
+
diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h
new file mode 100644
index 000000000..4b7091d5b
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_load.h
+ *
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_LOAD_H
+#define _SYSTEMLIB_MIXER_LOAD_H value
+
+#include <nuttx/config.h>
+
+__BEGIN_DECLS
+
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 2446cc3fb..b89f341b6 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_v[0],
@@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 };
-const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
 	4, /* quad_v */
@@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	if (used > (int)buflen) {
-		debug("multirotor spec used %d of %u", used, buflen);
+		debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
 		return nullptr;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
 		geometry = MultirotorMixer::QUAD_PLUS;
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index c8434f991..c3985b5de 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer()
 		free(_info);
 }
 
-static const char *
-findtag(const char *buf, unsigned &buflen, char tag)
-{
-	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
-			return buf;
-		buf++;
-		buflen--;
-	}
-	return nullptr;
-}
-
-static void
-skipline(const char *buf, unsigned &buflen)
-{
-	const char *p;
-
-	/* if we can find a CR or NL in the buffer, skip up to it */
-	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
-		buflen -= (p - buf);
-}
-
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
 {
@@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	scaler.negative_scale	= s[0] / 10000.0f;
 	scaler.positive_scale	= s[1] / 10000.0f;
@@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 
 	buf = findtag(buf, buflen, 'S');
 	if ((buf == nullptr) || (buflen < 16)) {
-		debug("contorl parser failed finding tag, ret: '%s'", buf);
+		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
 	}
 
@@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	control_group		= u[0];
 	control_index		= u[1];
@@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
-	/* enforce that the mixer ends with space or a new line */
-	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
-			continue;
-
-		/* require a space or newline at the end of the buffer, fail on printable chars */
-		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
-			/* found a line ending or space, so no split symbols / numbers. good. */
-			break;
-		} else {
-			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
-			goto out;
-		}
-
-	}
-
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
 		goto out;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		goto out;
+	}
 
 	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
 
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
index 4d45e1c50..fc7485e20 100644
--- a/src/modules/systemlib/mixer/module.mk
+++ b/src/modules/systemlib/mixer/module.mk
@@ -39,4 +39,5 @@ LIBNAME	= mixerlib
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
-		  mixer_simple.cpp
+		  mixer_simple.cpp \
+		  mixer_load.c
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index ccdb2ea38..398657dd7 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -508,64 +508,63 @@ param_get_default_file(void)
 int
 param_save_default(void)
 {
-	int result;
-	unsigned retries = 0;
-
-	/* delete the file in case it exists */
-	struct stat buffer;
-	if (stat(param_get_default_file(), &buffer) == 0) {
-
-		do {
-			result = unlink(param_get_default_file());
-			if (result != 0) {
-				retries++;
-				usleep(1000 * retries);
-			}
-		} while (result != OK && retries < 10);
+	int res;
+	int fd;
 
-		if (result != OK)
-			warnx("unlinking file %s failed.", param_get_default_file());
-	}
+	const char *filename = param_get_default_file();
+	const char *filename_tmp = malloc(strlen(filename) + 5);
+	sprintf(filename_tmp, "%s.tmp", filename);
 
-	/* create the file */
-	int fd;
+	/* delete temp file if exist */
+	res = unlink(filename_tmp);
+
+	if (res != OK && errno == ENOENT)
+		res = OK;
+
+	if (res != OK)
+		warn("failed to delete temp file: %s", filename_tmp);
+
+	if (res == OK) {
+		/* write parameters to temp file */
+		fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
 
-	do {
-		/* do another attempt in case the unlink call is not synced yet */
-		fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
 		if (fd < 0) {
-			retries++;
-			usleep(1000 * retries);
+			warn("failed to open temp file: %s", filename_tmp);
+			res = ERROR;
 		}
 
-	} while (fd < 0 && retries < 10);
+		if (res == OK) {
+			res = param_export(fd, false);
 
-	if (fd < 0) {
-		
-		warn("opening '%s' for writing failed", param_get_default_file());
-		return fd;
-	}
+			if (res != OK)
+				warnx("failed to write parameters to file: %s", filename_tmp);
+		}
 
-	do {
-		result = param_export(fd, false);
+		close(fd);
+	}
 
-		if (result != OK) {
-			retries++;
-			usleep(1000 * retries);
-		}
+	if (res == OK) {
+		/* delete parameters file */
+		res = unlink(filename);
 
-	} while (result != 0 && retries < 10);
+		if (res != OK && errno == ENOENT)
+			res = OK;
 
+		if (res != OK)
+			warn("failed to delete parameters file: %s", filename);
+	}
 
-	close(fd);
+	if (res == OK) {
+		/* rename temp file to parameters */
+		res = rename(filename_tmp, filename);
 
-	if (result != OK) {
-		warn("error exporting parameters to '%s'", param_get_default_file());
-		(void)unlink(param_get_default_file());
-		return result;
+		if (res != OK)
+			warn("failed to rename %s to %s", filename_tmp, filename);
 	}
 
-	return 0;
+	free(filename_tmp);
+
+	return res;
 }
 
 /**
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index 60d6473b8..b4350cc24 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -47,14 +47,12 @@
 #include <mavlink/mavlink_log.h>
 #include <uORB/topics/rc_channels.h>
 
-int rc_calibration_check(void) {
+int rc_calibration_check(int mavlink_fd) {
 
 	char nbuf[20];
 	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
 	_parameter_handles_rev, _parameter_handles_dz;
 
-	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
 	float param_min, param_max, param_trim, param_rev, param_dz;
 
 	/* first check channel mappings */
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
index e2238d151..e70b83cce 100644
--- a/src/modules/systemlib/rc_check.h
+++ b/src/modules/systemlib/rc_check.h
@@ -47,6 +47,6 @@
  * @return			0 / OK if RC calibration is ok, index + 1 of the first
  *				channel that failed else (so 1 == first channel failed)
  */
-__EXPORT int	rc_calibration_check(void);
+__EXPORT int	rc_calibration_check(int mavlink_fd);
 
 __END_DECLS