Reset offsets/scales before calibration and use prescaled values in m/s^2 instead of raw values.

2 files changed, 76 insertions, 55 deletions

diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c
index f607b6da6..180736908 100644
--- a/apps/commander/accelerometer_calibration.c
+++ b/apps/commander/accelerometer_calibration.c
@@ -78,6 +78,13 @@
 #include <systemlib/conversions.h>
 #include <mavlink/mavlink_log.h>
 
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[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);
+
 void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
@@ -85,14 +92,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
 	status->flag_preflight_accel_calibration = true;
 	state_machine_publish(status_pub, status, mavlink_fd);
 
-	float accel_offs_scaled[3];
+	/* measure and calculate offsets & scales */
+	float accel_offs[3];
 	float accel_scale[3];
-	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs_scaled, accel_scale);
+	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
+
 	if (res == OK) {
 		/* measurements complete successfully, set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_scaled[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_scaled[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_scaled[2]))
+		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]))) {
@@ -101,11 +110,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
 
 		int fd = open(ACCEL_DEVICE_PATH, 0);
 		struct accel_scale ascale = {
-			accel_offs_scaled[0],
+			accel_offs[0],
 			accel_scale[0],
-			accel_offs_scaled[1],
+			accel_offs[1],
 			accel_scale[1],
-			accel_offs_scaled[2],
+			accel_offs[2],
 			accel_scale[2],
 		};
 
@@ -139,12 +148,30 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
 	state_machine_publish(status_pub, status, mavlink_fd);
 }
 
-int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) {
+int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
 	const int samples_num = 2500;
-	int16_t accel_raw_ref[6][3];
+	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));
 	while (true) {
 		bool done = true;
@@ -167,8 +194,8 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3],
 
 		sprintf(str, "meas started: %s", orientation_strs[orient]);
 		mavlink_log_info(mavlink_fd, str);
-		read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num);
-		str_ptr = sprintf(str, "meas result for %s: [ %i %i %i ]", orientation_strs[orient], accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]);
+		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
 		mavlink_log_info(mavlink_fd, str);
 		data_collected[orient] = true;
 		tune_confirm();
@@ -176,20 +203,20 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3],
 	close(sensor_combined_sub);
 
 	/* calculate offsets and rotation+scale matrix */
-	int16_t accel_offs[3];
 	float accel_T[3][3];
-	int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+	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 calc error");
 		return ERROR;
 	}
 
-	/* convert raw accel offset to scaled and transform matrix to scales
-	 * rotation part of transform matrix is not used by now */
+	/* convert accel transform matrix to scales,
+	 * rotation part of transform matrix is not used by now
+	 */
 	for (int i = 0; i < 3; i++) {
-		accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i];
 		accel_scale[i] = accel_T[i][i];
 	}
+
 	return OK;
 }
 
@@ -233,8 +260,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 			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_raw[i] * w;
-				float d = (float) sensor.accelerometer_raw[i] - accel_ema[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];
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
 				if (d > accel_disp[i])
@@ -273,9 +300,11 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 			return -3;
 		}
 		if (t > t_timeout) {
+			mavlink_log_info(mavlink_fd, "ERROR: timeout");
 			return -1;
 		}
 	}
+
 	float accel_len = sqrt(accel_len2);
 	float accel_err_thr_raw = accel_len * accel_err_thr;
 	if (  fabs(accel_ema[0] - accel_len) < accel_err_thr_raw &&
@@ -302,56 +331,47 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 		  fabs(accel_ema[1]) < accel_err_thr_raw &&
 		  fabs(accel_ema[2] + accel_len) < accel_err_thr_raw  )
 		return 5;	// [ 0, 0, -g ]
+
 	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+
 	return -2;	// Can't detect orientation
 }
 
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-int read_accelerometer_raw_avg(int sensor_combined_sub, int16_t 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] = { { .fd = sensor_combined_sub, .events = POLLIN } };
 	int count = 0;
-	int32_t accel_sum[3] = { 0, 0, 0 };
+	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+
 	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_raw[i];
-			}
+			for (int i = 0; i < 3; i++)
+				accel_sum[i] += sensor.accelerometer_m_s2[i];
 			count++;
 		} else {
 			return ERROR;
 		}
 	}
-	for (int i = 0; i < 3; i++) {
-		accel_avg[i] = (accel_sum[i] + count / 2) / count;
-	}
-	/* calculate dispersion */
-	return OK;
-}
 
-/*
- * Convert raw values from accelerometers to m/s^2.
- */
-void acceleration_raw_to_m_s2(float accel_corr[3], int16_t accel_raw[3],
-		float accel_T[3][3], int16_t accel_offs[3]) {
 	for (int i = 0; i < 3; i++) {
-		accel_corr[i] = 0.0f;
-		for (int j = 0; j < 3; j++) {
-			accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]);
-		}
+		accel_avg[i] = accel_sum[i] / count;
 	}
+
+	return OK;
 }
 
 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.0)
-		return -1;	// Singular matrix
+		return ERROR;	// Singular matrix
+
 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
 	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
 	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
@@ -361,34 +381,38 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
 	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
 	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
 	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
-	return 0;
+
+	return OK;
 }
 
-int calculate_calibration_values(int16_t accel_raw_ref[6][3],
-		float accel_T[3][3], int16_t accel_offs[3], float g) {
-	/* calculate raw offsets */
+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] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i])
-				+ (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2);
+		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
 	}
+
 	/* 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_raw_ref[i * 2][j] - accel_offs[j]);
+			float a = accel_ref[i * 2][j] - accel_offs[j];
 			mat_A[i][j] = a;
 		}
 	}
+
 	/* calculate inverse matrix for A */
 	float mat_A_inv[3][3];
-	mat_invert3(mat_A, mat_A_inv);
+	if (mat_invert3(mat_A, mat_A_inv) != OK)
+		return ERROR;
+
+	/* copy results to accel_T */
 	for (int i = 0; i < 3; i++) {
-		/* copy results to accel_T */
 		for (int j = 0; j < 3; j++) {
 			/* simplify matrices mult because b has only one non-zero element == g at index i */
 			accel_T[j][i] = mat_A_inv[j][i] * g;
 		}
 	}
-	return 0;
+
+	return OK;
 }
diff --git a/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h
index acf45b6b6..c0169c2a1 100644
--- a/apps/commander/accelerometer_calibration.h
+++ b/apps/commander/accelerometer_calibration.h
@@ -12,8 +12,5 @@
 #include <uORB/topics/vehicle_status.h>
 
 void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
-int detect_orientation(int mavlink_fd, int sub_sensor_combined);
-int mat_invert3(float src[3][3], float dst[3][3]);
-int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */