1 files changed, 199 insertions, 123 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;