commitCal

5 files changed, 462 insertions, 337 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index d70e05000..e598f63bb 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -120,8 +120,11 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+// FIXME: Can some of these headers move out with detect_ move?
+
 #include "accelerometer_calibration.h"
 #include "calibration_messages.h"
+#include "calibration_routines.h"
 #include "commander_helper.h"
 
 #include <unistd.h>
@@ -149,18 +152,15 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
-static const unsigned max_sens = 3;
-
-int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors);
-int detect_orientation(int mavlink_fd, int (&subs)[max_sens]);
-int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num);
+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][6][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_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g);
+int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][6][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
 
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
-	int32_t device_id[max_sens];
+	int32_t device_id[max_accel_sens];
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
@@ -183,7 +183,7 @@ int do_accel_calibration(int mavlink_fd)
 	char str[30];
 
 	/* reset all sensors */
-	for (unsigned s = 0; s < max_sens; s++) {
+	for (unsigned s = 0; s < max_accel_sens; s++) {
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = open(str, 0);
@@ -202,8 +202,8 @@ int do_accel_calibration(int mavlink_fd)
 		}
 	}
 
-	float accel_offs[max_sens][3];
-	float accel_T[max_sens][3][3];
+	float accel_offs[max_accel_sens][3];
+	float accel_T[max_accel_sens][3][3];
 	unsigned active_sensors;
 
 	if (res == OK) {
@@ -296,20 +296,19 @@ int do_accel_calibration(int mavlink_fd)
 	return res;
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors)
+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)
 {
 	const unsigned samples_num = 3000;
 	*active_sensors = 0;
 
-	float accel_ref[max_sens][6][3];
-	bool data_collected[6] = { false, false, false, false, false, false };
-	const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
+	float accel_ref[max_accel_sens][detect_orientation_side_count][3];
+	bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
 
-	int subs[max_sens];
+	int subs[max_accel_sens];
 
-	uint64_t timestamps[max_sens];
+	uint64_t timestamps[max_accel_sens];
 
-	for (unsigned i = 0; i < max_sens; i++) {
+	for (unsigned i = 0; i < max_accel_sens; i++) {
 		subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
 		/* store initial timestamp - used to infer which sensors are active */
 		struct accel_report arp = {};
@@ -343,20 +342,23 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 		}
 
 		/* inform user which axes are still needed */
-		mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
-				 (!data_collected[5]) ? "down " : "",
-				 (!data_collected[0]) ? "back " : "",
-				 (!data_collected[1]) ? "front " : "",
-				 (!data_collected[2]) ? "left " : "",
-				 (!data_collected[3]) ? "right " : "",
-				 (!data_collected[4]) ? "up " : "");
+		char pendingStr[256];
+		pendingStr[0] = 0;
+		
+		for (unsigned cur_orientation = 0; cur_orientation < detect_orientation_side_count; cur_orientation++) {
+			if (data_collected[cur_orientation]) {
+				strcat(pendingStr, " ");
+				strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
+			}
+		}
+		mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
 
 		/* allow user enough time to read the message */
 		sleep(3);
 
-		int orient = detect_orientation(mavlink_fd, subs);
+		enum detect_orientation_return orient = detect_orientation(mavlink_fd, subs[0]);
 
-		if (orient < 0) {
+		if (orient == DETECT_ORIENTATION_ERROR) {
 			mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
 			sleep(2);
 			continue;
@@ -364,17 +366,17 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 
 		/* inform user about already handled side */
 		if (data_collected[orient]) {
-			mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
+			mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
 			sleep(3);
 			continue;
 		}
 
-		mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
+		mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orient));
 		sleep(1);
 		read_accelerometer_avg(subs, accel_ref, orient, samples_num);
-		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
+		mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
 		usleep(100000);
-		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", detect_orientation_str(orient),
 				 (double)accel_ref[0][orient][0],
 				 (double)accel_ref[0][orient][1],
 				 (double)accel_ref[0][orient][2]);
@@ -384,7 +386,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 	}
 
 	/* close all subscriptions */
-	for (unsigned i = 0; i < max_sens; i++) {
+	for (unsigned i = 0; i < max_accel_sens; i++) {
 		/* figure out which sensors were active */
 		struct accel_report arp = {};
 		(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
@@ -416,196 +418,31 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se
 	return res;
 }
 
-/**
- * Wait for vehicle become still and detect it's orientation.
- *
- * @param mavlink_fd the MAVLink file descriptor to print output to
- * @param subs the accelerometer subscriptions. Only the first one will be used.
- *
- * @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 (&subs)[max_sens])
-{
-	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;
-	struct pollfd fds[1];
-	fds[0].fd = subs[0];
-	fds[0].events = POLLIN;
-
-	hrt_abstime t_start = hrt_absolute_time();
-	/* set timeout to 30s */
-	hrt_abstime timeout = 30000000;
-	hrt_abstime t_timeout = t_start + timeout;
-	hrt_abstime t = t_start;
-	hrt_abstime t_prev = t_start;
-	hrt_abstime t_still = 0;
-
-	unsigned poll_errcount = 0;
-
-	while (true) {
-		/* wait blocking for new data */
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_accel), subs[0], &sensor);
-			t = hrt_absolute_time();
-			float dt = (t - t_prev) / 1000000.0f;
-			t_prev = t;
-			float w = dt / ema_len;
-
-			for (unsigned i = 0; i < ndim; i++) {
-
-				float di = 0.0f;
-				switch (i) {
-					case 0:
-						di = sensor.x;
-						break;
-					case 1:
-						di = sensor.y;
-						break;
-					case 2:
-						di = sensor.z;
-						break;
-				}
-
-				float d = di - 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) {
-				/* is still now */
-				if (t_still == 0) {
-					/* first time */
-					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
-					t_still = t;
-					t_timeout = t + timeout;
-
-				} else {
-					/* still since t_still */
-					if (t > t_still + still_time) {
-						/* vehicle is still, exit from the loop to detection of its orientation */
-						break;
-					}
-				}
-
-			} 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_and_console_log_info(mavlink_fd, "detected motion, hold still...");
-					sleep(3);
-					t_still = 0;
-				}
-			}
-
-		} else if (poll_ret == 0) {
-			poll_errcount++;
-		}
-
-		if (t > t_timeout) {
-			poll_errcount++;
-		}
-
-		if (poll_errcount > 1000) {
-			mavlink_and_console_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) {
-		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) {
-		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) {
-		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) {
-		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) {
-		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) {
-		return 5;        // [ 0, 0, -g ]
-	}
-
-	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
-
-	return ERROR;	// Can't detect orientation
-}
-
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num)
+int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][6][3], unsigned orient, unsigned samples_num)
 {
-	struct pollfd fds[max_sens];
+	struct pollfd fds[max_accel_sens];
 
-	for (unsigned i = 0; i < max_sens; i++) {
+	for (unsigned i = 0; i < max_accel_sens; i++) {
 		fds[i].fd = subs[i];
 		fds[i].events = POLLIN;
 	}
 
-	unsigned counts[max_sens] = { 0 };
-	float accel_sum[max_sens][3];
+	unsigned counts[max_accel_sens] = { 0 };
+	float accel_sum[max_accel_sens][3];
 	memset(accel_sum, 0, sizeof(accel_sum));
 
 	unsigned errcount = 0;
 
 	/* use the first sensor to pace the readout, but do per-sensor counts */
 	while (counts[0] < samples_num) {
-		int poll_ret = poll(&fds[0], max_sens, 1000);
+		int poll_ret = poll(&fds[0], max_accel_sens, 1000);
 
 		if (poll_ret > 0) {
 
-			for (unsigned s = 0; s < max_sens; s++) {
+			for (unsigned s = 0; s < max_accel_sens; s++) {
 				bool changed;
 				orb_check(subs[s], &changed);
 
@@ -632,7 +469,7 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6
 		}
 	}
 
-	for (unsigned s = 0; s < max_sens; s++) {
+	for (unsigned s = 0; s < max_accel_sens; s++) {
 		for (unsigned i = 0; i < 3; i++) {
 			accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
 			warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]);
@@ -665,7 +502,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
 	return OK;
 }
 
-int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g)
+int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][6][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++) {
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
index b1e209efc..f41ebb615 100644
--- a/src/modules/commander/calibration_messages.h
+++ b/src/modules/commander/calibration_messages.h
@@ -47,7 +47,8 @@
 #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_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"
 #define CAL_FAILED_APPLY_CAL_MSG	"ERROR: failed to apply calibration"
 #define CAL_FAILED_SET_PARAMS_MSG	"ERROR: failed to set parameters"
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 5796204bf..869027a70 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -38,11 +38,20 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <stdio.h>
 #include <math.h>
 #include <float.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <mavlink/mavlink_log.h>
+#include <geo/geo.h>
 
 #include "calibration_routines.h"
+#include "calibration_messages.h"
 
+// FIXME: Fix return codes
+static const int ERROR = -1;
 
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
@@ -218,3 +227,173 @@ 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)
+{
+	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;
+	struct pollfd fds[1];
+	fds[0].fd = accel_sub;
+	fds[0].events = POLLIN;
+	
+	hrt_abstime t_start = hrt_absolute_time();
+	/* set timeout to 30s */
+	hrt_abstime timeout = 30000000;
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+	
+	unsigned poll_errcount = 0;
+	
+	while (true) {
+		/* wait blocking for new data */
+		int poll_ret = poll(fds, 1, 1000);
+		
+		if (poll_ret) {
+			orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor);
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+			
+			for (unsigned i = 0; i < ndim; i++) {
+				
+				float di = 0.0f;
+				switch (i) {
+					case 0:
+						di = sensor.x;
+						break;
+					case 1:
+						di = sensor.y;
+						break;
+					case 2:
+						di = sensor.z;
+						break;
+				}
+				
+				float d = di - 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) {
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
+					t_still = t;
+					t_timeout = t + timeout;
+					
+				} else {
+					/* still since t_still */
+					if (t > t_still + still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+				
+			} 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_and_console_log_info(mavlink_fd, "detected motion, hold still...");
+					sleep(3);
+					t_still = 0;
+				}
+			}
+			
+		} else if (poll_ret == 0) {
+			poll_errcount++;
+		}
+		
+		if (t > t_timeout) {
+			poll_errcount++;
+		}
+		
+		if (poll_errcount > 1000) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+			return DETECT_ORIENTATION_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) {
+		return DETECT_ORIENTATION_NOSE_DOWN;        // [ 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) {
+		return DETECT_ORIENTATION_TAIL_DOWN;        // [ -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) {
+		return DETECT_ORIENTATION_LEFT;        // [ 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) {
+		return DETECT_ORIENTATION_RIGHT;        // [ 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) {
+		return DETECT_ORIENTATION_UPSIDE_DOWN;        // [ 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) {
+		return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
+	}
+	
+	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
+	
+	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
+}
+
+const char* detect_orientation_str(enum detect_orientation_return orientation)
+{
+	static const char* rgOrientationStrs[] = {
+		"up",
+		"down",
+		"front",
+		"back",
+		"left",
+		"right",
+		"error"
+	};
+	
+	return rgOrientationStrs[orientation];
+}
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index 3c8e49ee9..0f85a9598 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -57,4 +57,40 @@
  * @return 0 on success, 1 on failure
  */
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
-			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
\ No newline at end of file
+			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
+
+// FIXME: Change the name
+static const unsigned max_accel_sens = 3;
+
+// If the order of these are changed the detect_orientation_str routine must be updated as well
+enum detect_orientation_return {
+	DETECT_ORIENTATION_RIGHTSIDE_UP,
+	DETECT_ORIENTATION_UPSIDE_DOWN,
+	DETECT_ORIENTATION_NOSE_DOWN,
+	DETECT_ORIENTATION_TAIL_DOWN,
+	DETECT_ORIENTATION_LEFT,
+	DETECT_ORIENTATION_RIGHT,
+	DETECT_ORIENTATION_ERROR
+};
+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);
+
+
+/**
+ * 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
+ */
+const char* detect_orientation_str(enum detect_orientation_return orientation);
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index e0786db79..9b5ce238d 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -49,6 +49,7 @@
 #include <math.h>
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_mag.h>
 #include <mavlink/mavlink_log.h>
@@ -157,192 +158,263 @@ int do_mag_calibration(int mavlink_fd)
 
 int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
 {
-	/* 45 seconds */
-	uint64_t calibration_interval = 25 * 1000 * 1000;
-
-	/* maximum 500 values */
-	const unsigned int calibration_maxcount = 240;
-	unsigned int calibration_counter;
+	int result = OK;
+	
+	const unsigned int calibration_interval_perside_seconds = 5;
+	const uint64_t calibration_interval_perside_useconds = calibration_interval_perside_seconds * 1000 * 1000;
 
-	float *x = NULL;
-	float *y = NULL;
-	float *z = NULL;
+	const unsigned int calibration_sides = 3;
+	const unsigned int calibration_points_perside = 80;
+	const unsigned int calibration_points_maxcount = calibration_sides * calibration_points_perside;
+	unsigned int calibration_counter_total = 0;
+	unsigned int calibration_counter_side;
 
 	char str[30];
-	int res = OK;
 	
 	/* allocate memory */
 	mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
 
-	x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
-	y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
-	z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+	float* x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+	float* y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+	float* z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
 
 	if (x == NULL || y == NULL || z == NULL) {
 		mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
-
-		/* clean up */
-		if (x != NULL) {
-			free(x);
-		}
-
-		if (y != NULL) {
-			free(y);
-		}
-
-		if (z != NULL) {
-			free(z);
-		}
-
-		res = ERROR;
-		return res;
+		return ERROR;
 	}
-
-	if (res == OK) {
-		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
-
-		if (sub_mag < 0) {
-			mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort");
-			res = ERROR;
-		}  else {
+	
+	// Setup subscriptions to mag and onboard accel sensors
+	// FIXME: Is it ok to assume first accel is onboard accel
+	
+	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");
+		result = ERROR;
+	}
+	
+	int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);	// FIXME: How about a better variable name than s?
+	if (sub_mag < 0) {
+		mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort");
+		result = ERROR;
+	}
+	
+	// FIXME: Worker routine for accel orientation detection
+	
+	if (result == OK) {
+		// We only need to collect information from the three main sides. The reverse orientation of
+		// those sides is not needed since it would create the same points around the sphere.
+		bool side_data_collected[detect_orientation_side_count] = { false, true, false, true, false, true };
+		
+		// Rotate through all three main positions
+		while (true) {
+			unsigned int side_complete_count = 0;
+			
+			// Update the number of completed sides
+			for (unsigned i = 0; i < detect_orientation_side_count; i++) {
+				if (side_data_collected[i]) {
+					side_complete_count++;
+				}
+			}
+			
+			if (side_complete_count == detect_orientation_side_count) {
+				// We have completed all sides, move on
+				break;
+			}
+			
+			// FIXME: Worker routine for pending string creation
+			
+			/* inform user which axes are still needed */
+			char pendingStr[256];
+			pendingStr[0] = 0;
+			
+			for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
+				if (side_data_collected[cur_orientation]) {
+					strcat(pendingStr, " ");
+					strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
+				}
+			}
+			mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
+			
+			enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel);
+			
+			if (orient == DETECT_ORIENTATION_ERROR) {
+				mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
+				continue;
+			}
+			
+			/* inform user about already handled side */
+			if (side_data_collected[orient]) {
+				mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
+				continue;
+			}
+			
+			// Rotation for mag calibration goes here
 			struct mag_report mag;
-
+			
 			/* limit update rate to get equally spaced measurements over time (in ms) */
-			orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
+			orb_set_interval(sub_mag, (calibration_interval_perside_useconds / 1000) / calibration_points_perside);
+			
 			/* calibrate offsets */
-			uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+			uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval_perside_useconds;
 			unsigned poll_errcount = 0;
-
+			
 			mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
-
-			calibration_counter = 0U;
-
+			
+			calibration_counter_side = 0;
+			
 			while (hrt_absolute_time() < calibration_deadline &&
-			       calibration_counter < calibration_maxcount) {
-
+			       calibration_counter_side < calibration_points_perside) {
+				
 				/* wait blocking for new data */
 				struct pollfd fds[1];
 				fds[0].fd = sub_mag;
 				fds[0].events = POLLIN;
-
+				
 				int poll_ret = poll(fds, 1, 1000);
-
+				
 				if (poll_ret > 0) {
 					orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-					x[calibration_counter] = mag.x;
-					y[calibration_counter] = mag.y;
-					z[calibration_counter] = mag.z;
-
-					calibration_counter++;
-
-					if (calibration_counter % (calibration_maxcount / 20) == 0) {
+					
+					x[calibration_counter_total] = mag.x;
+					y[calibration_counter_total] = mag.y;
+					z[calibration_counter_total] = mag.z;
+					
+					calibration_counter_total++;
+					calibration_counter_side++;
+					
+	#if 0
+					// FIXME: Check total progress percentage
+					if (calibration_counter % (calibration_points_perside / 20) == 0) {
 						mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
 					}
-
+	#endif
+					
+					// Progress indicator for side
+					mavlink_and_console_log_info(mavlink_fd, "%s side calibration: progress <%u>", sensor_name, calibration_counter_side /calibration_points_perside)
 				} else {
 					poll_errcount++;
 				}
-
+				
+				// FIXME: How does this error count relate to poll interval? Seems to high.
+				// Seems like it should be some percentage of total points captured.
 				if (poll_errcount > 1000) {
-					mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
-					res = ERROR;
+					result = ERROR;
+					mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SENSOR_MSG);
 					break;
 				}
 			}
 
-			close(sub_mag);
+			// Note that this side is complete
+			side_data_collected[orient] = true;
+			tune_neutral(true);
 		}
 	}
+	
+	// Sensor subcriptions are no longer needed
+	if (sub_mag >= 0) {
+		close(sub_mag);
+	}
+	if (sub_accel >= 0) {
+		close(sub_accel);
+	}
 
+	// FIXME: Check as to how this happens?
+	if (result == OK && calibration_counter_total < (calibration_points_maxcount / 2)) {
+		mavlink_and_console_log_info(mavlink_fd, "ERROR: Not enough points collected");
+		result = ERROR;
+	}
+	
 	float sphere_x;
 	float sphere_y;
 	float sphere_z;
 	float sphere_radius;
-
-	if (res == OK && calibration_counter > (calibration_maxcount / 2)) {
-
+	
+	if (result == OK) {
 		/* sphere fit */
 		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
-		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+		sphere_fit_least_squares(x, y, z, calibration_counter_total, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
 		mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
 
 		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
-			res = ERROR;
+			mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit");
+			result = ERROR;
 		}
 	}
-
-	if (x != NULL) {
-		free(x);
-	}
-
-	if (y != NULL) {
-		free(y);
-	}
-
-	if (z != NULL) {
-		free(z);
-	}
-
-	if (res == OK) {
-		/* apply calibration and set parameters */
-		struct mag_scale mscale;
+	
+	// Data points are no longer needed
+	free(x);
+	free(y);
+	free(z);
+	
+	int fd_mag = -1;
+	struct mag_scale mscale;
+	
+	if (result == OK) {
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
-		int fd = open(str, 0);
-		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
-
-		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
+		
+		fd_mag = open(str, 0);
+		if (fd_mag < 0) {
+			mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device");
+			result = ERROR;
 		}
-
-		if (res == OK) {
-			mscale.x_offset = sphere_x;
-			mscale.y_offset = sphere_y;
-			mscale.z_offset = sphere_z;
-
-			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
-
-			if (res != OK) {
-				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
-			}
+	}
+	
+	if (result == OK) {
+		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");
+			result = ERROR;
 		}
+	}
 
-		close(fd);
+	if (result == OK) {
+		mscale.x_offset = sphere_x;
+		mscale.y_offset = sphere_y;
+		mscale.z_offset = sphere_z;
 
-		if (res == OK) {
-
-			bool failed = false;
-			/* set parameters */
-			(void)sprintf(str, "CAL_MAG%u_ID", s);
-			failed |= (OK != param_set(param_find(str), &(device_id)));
-			(void)sprintf(str, "CAL_MAG%u_XOFF", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
-			(void)sprintf(str, "CAL_MAG%u_YOFF", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
-			(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
-			(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
-			(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
-			(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
-			failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
-
-			if (failed) {
-				res = ERROR;
-				mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-			}
+		result = ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
+		if (result != OK) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			result = ERROR;
+		}
+	}
+	
+	// Mag device no longer needed
+	if (fd_mag >= 0) {
+		close(fd_mag);
+	}
 
+	if (result == OK) {
+		bool failed = false;
+		
+		/* set parameters */
+		(void)sprintf(str, "CAL_MAG%u_ID", s);
+		failed |= (OK != param_set(param_find(str), &(device_id)));
+		(void)sprintf(str, "CAL_MAG%u_XOFF", s);
+		failed |= (OK != param_set(param_find(str), &(mscale.x_offset)));
+		(void)sprintf(str, "CAL_MAG%u_YOFF", s);
+		failed |= (OK != param_set(param_find(str), &(mscale.y_offset)));
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", s);
+		failed |= (OK != param_set(param_find(str), &(mscale.z_offset)));
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", s);
+		failed |= (OK != param_set(param_find(str), &(mscale.x_scale)));
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", s);
+		failed |= (OK != param_set(param_find(str), &(mscale.y_scale)));
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", s);
+		failed |= (OK != param_set(param_find(str), &(mscale.z_scale)));
+
+		if (failed) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			result = ERROR;
+		} else {
 			mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
-		}
 
-		mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-				 (double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-				 (double)mscale.y_scale, (double)mscale.z_scale);
+			mavlink_and_console_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+					 (double)mscale.y_offset, (double)mscale.z_offset);
+			mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+					 (double)mscale.y_scale, (double)mscale.z_scale);
+		}
 	}
 
-	return res;
+	return result;
 }