Commander: Implement calibration routines for multi-sensor setups

6 files changed, 363 insertions, 218 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index d9e7e21fc..10fa3eaa3 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -134,7 +134,6 @@
 #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>
@@ -150,16 +149,18 @@ static const int ERROR = -1;
 
 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);
+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]);
+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 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)
 {
 	int fd;
-	int32_t device_id;
+	int32_t device_id[max_sens];
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
@@ -179,20 +180,30 @@ int do_accel_calibration(int mavlink_fd)
 
 	int res = OK;
 
-	/* reset all offsets to zero and all scales to one */
-	fd = open(ACCEL_DEVICE_PATH, 0);
+	char str[30];
+
+	/* reset all sensors */
+	for (unsigned s = 0; s < max_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);
+
+		if (fd < 0) {
+			continue;
+		}
 
-	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
 
-	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
-	close(fd);
+		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+		close(fd);
 
-	if (res != OK) {
-		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+		}
 	}
 
-	float accel_offs[3];
-	float accel_T[3][3];
+	float accel_offs[max_sens][3];
+	float accel_T[max_sens][3][3];
 
 	if (res == OK) {
 		/* measure and calculate offsets & scales */
@@ -200,6 +211,7 @@ int do_accel_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
+
 		/* measurements completed successfully, rotate calibration values */
 		param_t board_rotation_h = param_find("SENS_BOARD_ROT");
 		int32_t board_rotation_int;
@@ -208,42 +220,58 @@ int do_accel_calibration(int mavlink_fd)
 		math::Matrix<3, 3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
 		math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
-		math::Vector<3> accel_offs_vec(&accel_offs[0]);
-		math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
-		math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
-		math::Matrix<3, 3> 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("CAL_ACC0_XOFF"), &(accel_scale.x_offset))
-		    || param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset))
-		    || param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset))
-		    || param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale))
-		    || param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale))
-		    || param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
-			res = ERROR;
-		}
 
-		if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
+		for (unsigned i = 0; i < max_sens; i++) {
+
+			/* handle individual sensors, one by one */
+			math::Vector<3> accel_offs_vec(&accel_offs[i][0]);
+			math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
+			math::Matrix<3, 3> accel_T_mat(&accel_T[i][0][0]);
+			math::Matrix<3, 3> 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);
+
+			bool failed = false;
+
+			/* set parameters */
+			(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+			failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset)));
+			(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+			failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset)));
+			(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+			failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset)));
+			(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
+			failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale)));
+			(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+			failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale)));
+			(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+			failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale)));
+			(void)sprintf(str, "CAL_ACC%u_ID", i);
+			failed |= (OK != param_set(param_find(str), &(device_id[i])));
+			
+			if (failed) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 				res = ERROR;
+			}
 		}
 	}
 
 	if (res == OK) {
 		/* apply new scaling and offsets */
-		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_APPLY_CAL_MSG);
+		for (unsigned s = 0; s < max_sens; s++) {
+			sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
+			fd = open(str, 0);
+			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+			close(fd);
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
 		}
 	}
 
@@ -266,14 +294,27 @@ int do_accel_calibration(int mavlink_fd)
 	return res;
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens][3], float accel_T[max_sens][3][3])
 {
-	const int samples_num = 2500;
-	float accel_ref[6][3];
+	const unsigned samples_num = 2500;
+
+	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" };
 
-	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int subs[max_sens];
+
+	uint64_t timestamps[max_sens];
+
+	unsigned active_sensors = 0;
+
+	for (unsigned i = 0; i < max_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 = {};
+		(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
+		timestamps[i] = arp.timestamp;
+	}
 
 	unsigned done_count = 0;
 	int res = OK;
@@ -312,7 +353,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		/* allow user enough time to read the message */
 		sleep(3);
 
-		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+		int orient = detect_orientation(mavlink_fd, &subs[0]);
 
 		if (orient < 0) {
 			mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
@@ -329,53 +370,70 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 
 		mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
 		sleep(1);
-		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+		read_accelerometer_avg(subs, accel_ref, orient, samples_num);
 		mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
-				 (double)accel_ref[orient][0],
-				 (double)accel_ref[orient][1],
-				 (double)accel_ref[orient][2]);
+				 (double)accel_ref[0][orient][0],
+				 (double)accel_ref[0][orient][1],
+				 (double)accel_ref[0][orient][2]);
 
 		data_collected[orient] = true;
 		tune_neutral(true);
 	}
 
-	close(sensor_combined_sub);
+	/* close all subscriptions */
+	for (unsigned i = 0; i < max_sens; i++) {
+		/* figure out which sensors were active */
+		struct accel_report arp = {};
+		(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
+		if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
+			active_sensors++;
+		}
+		close(subs[i]);
+	}
 
 	if (res == OK) {
 		/* calculate offsets and transform matrix */
-		res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+		for (unsigned i = 0; i < active_sensors; i++) {
+			res = calculate_calibration_values(accel_ref[i], accel_T[i], accel_offs[i], CONSTANTS_ONE_G);
 
-		if (res != OK) {
-			mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
+			if (res != OK) {
+				mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
+				break;
+			}
 		}
 	}
 
 	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 sub_sensor_combined)
+int detect_orientation(int mavlink_fd, int subs[max_sens])
 {
-	struct sensor_combined_s sensor;
+	const unsigned ndim = 3;
+
+	struct accel_report sensor;
 	/* exponential moving average of accel */
-	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
+	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 = pow(0.25f, 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 = sub_sensor_combined;
+	fds[0].fd = subs[0];
 	fds[0].events = POLLIN;
 
 	hrt_abstime t_start = hrt_absolute_time();
@@ -393,14 +451,14 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 		int poll_ret = poll(fds, 1, 1000);
 
 		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
+			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 (int i = 0; i < 3; i++) {
-				float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
+			for (unsigned i = 0; i < ndim; i++) {
+				float d = ((float*)&sensor.x)[i] - accel_ema[i];
 				accel_ema[i] += d * w;
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
@@ -502,28 +560,42 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 /*
  * 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 subs[max_sens], float accel_avg[max_sens][6][3], unsigned orient, unsigned samples_num)
 {
-	struct pollfd fds[1];
-	fds[0].fd = sensor_combined_sub;
-	fds[0].events = POLLIN;
-	int count = 0;
-	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+	struct pollfd fds[max_sens];
 
-	int errcount = 0;
+	for (unsigned i = 0; i < max_sens; i++) {
+		fds[i].fd = subs[i];
+		fds[i].events = POLLIN;
+	}
 
-	while (count < samples_num) {
-		int poll_ret = poll(fds, 1, 1000);
+	unsigned counts[max_sens] = { 0 };
+	float accel_sum[max_sens][3] = { 0.0f };
 
-		if (poll_ret == 1) {
-			struct sensor_combined_s sensor;
-			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+	unsigned errcount = 0;
 
-			for (int i = 0; i < 3; i++) {
-				accel_sum[i] += sensor.accelerometer_m_s2[i];
-			}
+	/* 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);
+
+		if (poll_ret > 0) {
+
+			for (unsigned s = 0; s < max_sens; s++) {
+				bool changed;
+				orb_check(subs[s], &changed);
 
-			count++;
+				if (changed) {
+
+					struct accel_report arp;
+					orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
+
+					for (int i = 0; i < 3; i++) {
+						accel_sum[s][i] += ((float*)&arp.x)[i];
+					}
+
+					counts[s]++;
+				}
+			}
 
 		} else {
 			errcount++;
@@ -535,8 +607,10 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 		}
 	}
 
-	for (int i = 0; i < 3; i++) {
-		accel_avg[i] = accel_sum[i] / count;
+	for (unsigned s = 0; s < max_sens; s++) {
+		for (unsigned i = 0; i < 3; i++) {
+			accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
+		}
 	}
 
 	return OK;
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index cae1d7684..747d915ff 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -89,7 +89,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	};
 
 	bool paramreset_successful = false;
-	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+	int  fd = open(AIRSPEED0_DEVICE_PATH, 0);
 
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
@@ -157,7 +157,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	if (isfinite(diff_pres_offset)) {
 
-		int  fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
+		int  fd_scale = open(AIRSPEED0_DEVICE_PATH, 0);
 		airscale.offset_pa = diff_pres_offset;
 		if (fd_scale > 0) {
 			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 8a4451100..60e8be23e 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -100,7 +100,7 @@ int buzzer_init()
 	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
 	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
 
-	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
+	buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
 		warnx("Buzzer: open fail\n");
@@ -201,7 +201,7 @@ int led_init()
 	blink_msg_end = 0;
 
 	/* first open normal LEDs */
-	leds = open(LED_DEVICE_PATH, 0);
+	leds = open(LED0_DEVICE_PATH, 0);
 
 	if (leds < 0) {
 		warnx("LED: open fail\n");
@@ -224,10 +224,10 @@ int led_init()
 	led_off(LED_AMBER);
 
 	/* then try RGB LEDs, this can fail on FMUv1*/
-	rgbleds = open(RGBLED_DEVICE_PATH, 0);
+	rgbleds = open(RGBLED0_DEVICE_PATH, 0);
 
 	if (rgbleds == -1) {
-		warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
+		warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
 	}
 
 	return 0;
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index e941e327c..dfd1657c5 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -45,6 +45,7 @@
 #include <fcntl.h>
 #include <poll.h>
 #include <math.h>
+#include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_gyro.h>
@@ -63,20 +64,23 @@ static const char *sensor_name = "gyro";
 
 int do_gyro_calibration(int mavlink_fd)
 {
-	int32_t device_id;
+	const unsigned max_gyros = 3;
+
+	int32_t device_id[3];
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 	mavlink_log_info(mavlink_fd, "HOLD STILL");
 
 	/* wait for the user to respond */
 	sleep(2);
 
-	struct gyro_scale gyro_scale = {
+	struct gyro_scale gyro_scale[max_gyros] = { {
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
+	}
 	};
 
 	int res = OK;
@@ -86,47 +90,75 @@ int do_gyro_calibration(int mavlink_fd)
 	mcu_unique_id(&mcu_id[0]);
 
 	/* store last 32bit number - not unique, but unique in a given set */
-	param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
+	(void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
 
-	/* reset all offsets to zero and all scales to one */
-	int fd = open(GYRO_DEVICE_PATH, 0);
+	char str[30];
 
-	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+	for (unsigned s = 0; s < max_gyros; s++) {
 
-	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
-	close(fd);
+		/* ensure all scale fields are initialized tha same as the first struct */
+		(void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0]));
+
+		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		/* reset all offsets to zero and all scales to one */
+		int fd = open(str, 0);
+
+		if (fd < 0) {
+			continue;
+		}
+
+		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+
+		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
+		close(fd);
 
-	if (res != OK) {
-		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+		}
 	}
 
+	unsigned calibration_counter[max_gyros] = { 0 };
+	const unsigned calibration_count = 5000;
+
 	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_multi(ORB_ID(sensor_gyro), 0);
+		int sub_sensor_gyro[max_gyros];
+		struct pollfd fds[max_gyros];
+
+		for (unsigned s = 0; s < max_gyros; s++) {
+			sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+			fds[s].fd = sub_sensor_gyro[s];
+			fds[s].events = POLLIN;
+		}
+
 		struct gyro_report gyro_report;
 
-		while (calibration_counter < calibration_count) {
+		/* use first gyro to pace, but count correctly per-gyro for statistics */
+		while (calibration_counter[0] < 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);
+
+				for (unsigned s = 0; s < max_gyros; s++) {
+					bool changed;
+					orb_check(sub_sensor_gyro[s], &changed);
+
+					if (changed) {
+						orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
+						gyro_scale[s].x_offset += gyro_report.x;
+						gyro_scale[s].y_offset += gyro_report.y;
+						gyro_scale[s].z_offset += gyro_report.z;
+						calibration_counter[s]++;
+					}
+
+					if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
+						mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
+					}
 				}
 
 			} else {
@@ -140,16 +172,18 @@ int do_gyro_calibration(int mavlink_fd)
 			}
 		}
 
-		close(sub_sensor_gyro);
+		for (unsigned s = 0; s < max_gyros; s++) {
+			close(sub_sensor_gyro[s]);
 
-		gyro_scale.x_offset /= calibration_count;
-		gyro_scale.y_offset /= calibration_count;
-		gyro_scale.z_offset /= calibration_count;
+			gyro_scale[s].x_offset /= calibration_counter[s];
+			gyro_scale[s].y_offset /= calibration_counter[s];
+			gyro_scale[s].z_offset /= calibration_counter[s];
+		}
 	}
 
 	if (res == OK) {
 		/* check offsets */
-		if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
+		if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) {
 			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
 			res = ERROR;
 		}
@@ -157,9 +191,41 @@ int do_gyro_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* set offset parameters to new values */
-		if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset))
-		    || param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset))
-		    || param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) {
+		bool failed = false;
+
+		for (unsigned s = 0; s < max_gyros; s++) {
+
+			/* if any reasonable amount of data is missing, skip */
+			if (calibration_counter[s] < calibration_count / 2) {
+				continue;
+			}
+
+			(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
+			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset)));
+			(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
+			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset)));
+			(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
+			failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset)));
+			(void)sprintf(str, "CAL_GYRO%u_ID", s);
+			failed |= (OK != param_set(param_find(str), &(device_id[s])));
+
+			/* apply new scaling and offsets */
+			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+			int fd = open(str, 0);
+
+			if (fd < 0) {
+				continue;
+			}
+
+			res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
+			close(fd);
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
+		}
+		
+		if (failed) {
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
 			res = ERROR;
 		}
@@ -279,8 +345,6 @@ int do_gyro_calibration(int mavlink_fd)
 	mavlink_log_info(mavlink_fd, "gyro scale calibration done");
 	tune_neutral();
 
-#endif
-
 	if (res == OK) {
 		/* set scale parameters to new values */
 		if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
@@ -289,21 +353,9 @@ int do_gyro_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
-		if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) {
-				res = ERROR;
-			}
 	}
 
-	if (res == OK) {
-		/* apply new scaling and offsets */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
-		close(fd);
-
-		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
-		}
-	}
+	#endif
 
 	if (res == OK) {
 		/* auto-save to EEPROM */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 7147fb283..1d2f38437 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -63,93 +63,121 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
 
+int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id);
+
 int do_mag_calibration(int mavlink_fd)
 {
-	int32_t device_id;
-	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;
+	const unsigned max_mags = 3;
 
-	/* maximum 500 values */
-	const unsigned int calibration_maxcount = 240;
-	unsigned int calibration_counter;
+	int32_t device_id[max_mags];
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	sleep(1);
 
-	struct mag_scale mscale_null = {
+	struct mag_scale mscale_null[max_mags] = {
+	{
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
 		0.0f,
 		1.0f,
-	};
+	}
+	} ;
 
-	int res = OK;
+	int res = ERROR;
 
-	/* erase old calibration */
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	char str[30];
 
-	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+	for (unsigned s = 0; s < max_mags; s++) {
 
-	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
+		mavlink_log_info(mavlink_fd, "Magnetometer #%u", s);
+		sleep(3);
 
-	if (res != OK) {
-		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
-	}
+		/* erase old calibration */
+		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+		int fd = open(str, O_RDONLY);
 
-	if (res == OK) {
-		/* calibrate range */
-		res = ioctl(fd, MAGIOCCALIBRATE, fd);
+		if (fd < 0) {
+			continue;
+		}
+
+		device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
+
+		/* ensure all scale fields are initialized tha same as the first struct */
+		(void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0]));
+
+		res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]);
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
-			/* this is non-fatal - mark it accordingly */
-			res = OK;
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+		}
+
+		if (res == OK) {
+			/* calibrate range */
+			res = ioctl(fd, MAGIOCCALIBRATE, fd);
+
+			if (res != OK) {
+				mavlink_log_info(mavlink_fd, "Skipped scale calibration");
+				/* this is non-fatal - mark it accordingly */
+				res = OK;
+			}
+		}
+
+		close(fd);
+
+		if (res == OK) {
+			res = calibrate_instance(mavlink_fd, s, device_id[s]);
 		}
 	}
 
-	close(fd);
+	return res;
+}
+
+int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
+{
+	/* 45 seconds */
+	uint64_t calibration_interval = 45 * 1000 * 1000;
+
+	/* maximum 500 values */
+	const unsigned int calibration_maxcount = 240;
+	unsigned int calibration_counter;
 
 	float *x = NULL;
 	float *y = NULL;
 	float *z = NULL;
 
-	if (res == OK) {
-		/* allocate memory */
-		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
+	char str[30];
+	int res = ERROR;
+	
+	/* allocate memory */
+	mavlink_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));
+	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));
 
-		if (x == NULL || y == NULL || z == NULL) {
-			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+	if (x == NULL || y == NULL || z == NULL) {
+		mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
 
-			/* clean up */
-			if (x != NULL) {
-				free(x);
-			}
-
-			if (y != NULL) {
-				free(y);
-			}
+		/* clean up */
+		if (x != NULL) {
+			free(x);
+		}
 
-			if (z != NULL) {
-				free(z);
-			}
+		if (y != NULL) {
+			free(y);
+		}
 
-			res = ERROR;
-			return res;
+		if (z != NULL) {
+			free(z);
 		}
 
-	} else {
-		/* exit */
-		return ERROR;
+		res = ERROR;
+		return res;
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
+		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s);
 
 		if (sub_mag < 0) {
 			mavlink_log_critical(mavlink_fd, "No mag found, abort");
@@ -239,8 +267,8 @@ int do_mag_calibration(int mavlink_fd)
 	if (res == OK) {
 		/* apply calibration and set parameters */
 		struct mag_scale mscale;
-
-		fd = open(MAG_DEVICE_PATH, 0);
+		(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) {
@@ -262,35 +290,26 @@ int do_mag_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res == OK) {
-			/* set parameters */
-			if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
-				res = ERROR;
-			}
-			if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
-				res = ERROR;
-			}
 
-			if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
-				res = ERROR;
-			}
-
-			if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
-				res = ERROR;
-			}
-
-			if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
-				res = ERROR;
-			}
-
-			if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
-				res = ERROR;
-			}
-
-			if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
+			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;
-			}
-
-			if (res != OK) {
 				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			}
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 40da9c77b..40aa4a2f0 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -652,7 +652,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 	int ret;
 	bool failed = false;
 
-	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+	int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
 		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");