Use new calibrate_from_orientation api

1 files changed, 96 insertions, 279 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 526b135d8..37db59c14 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,24 @@ 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][detect_orientation_side_count][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][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
+int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
+
+/// Data passed to calibration worker routine
+typedef struct  {
+	int		mavlink_fd;
+	unsigned	done_count;
+	int		subs[max_accel_sens];
+	float		accel_ref[max_accel_sens][detect_orientation_side_count][3];
+} accel_worker_data_t;
 
 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);
 
@@ -178,7 +187,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);
@@ -193,12 +202,12 @@ int do_accel_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
 		}
 	}
 
-	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) {
@@ -234,27 +243,27 @@ int do_accel_calibration(int mavlink_fd)
 		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)));
+		failed |= (OK != param_set_no_notification(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)));
+		failed |= (OK != param_set_no_notification(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)));
+		failed |= (OK != param_set_no_notification(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)));
+		failed |= (OK != param_set_no_notification(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)));
+		failed |= (OK != param_set_no_notification(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)));
+		failed |= (OK != param_set_no_notification(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])));
+		failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
 		
 		if (failed) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
 			return ERROR;
 		}
 
@@ -270,7 +279,7 @@ int do_accel_calibration(int mavlink_fd)
 		}
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
 		}
 	}
 
@@ -291,309 +300,117 @@ 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 accel_calibration_worker(detect_orientation_return orientation, void* data)
 {
 	const unsigned samples_num = 3000;
+	accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
+	
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
+	sleep(1);
+	
+	read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
+	
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
+				     (double)worker_data->accel_ref[0][orientation][0],
+				     (double)worker_data->accel_ref[0][orientation][1],
+				     (double)worker_data->accel_ref[0][orientation][2]);
+	
+	worker_data->done_count++;
+	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
+	
+	return OK;
+}
+
+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 result = OK;
+	
 	*active_sensors = 0;
+	
+	accel_worker_data_t worker_data;
+	
+	worker_data.mavlink_fd = mavlink_fd;
+	worker_data.done_count = 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" };
+	bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
 
-	int subs[max_sens];
+	// Initialize subs to error condition so we know which ones are open and which are not
+	for (size_t i=0; i<max_accel_sens; i++) {
+		worker_data.subs[i] = -1;
+	}
 
-	uint64_t timestamps[max_sens];
+	uint64_t timestamps[max_accel_sens];
 
-	for (unsigned i = 0; i < max_sens; i++) {
-		subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
+	for (unsigned i = 0; i < max_accel_sens; i++) {
+		worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
+		if (worker_data.subs[i] < 0) {
+			result = ERROR;
+			break;
+		}
+		
 		/* store initial timestamp - used to infer which sensors are active */
 		struct accel_report arp = {};
-		(void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp);
+		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
 		timestamps[i] = arp.timestamp;
 	}
 
-	unsigned done_count = 0;
-	int res = OK;
-
-	while (true) {
-		bool done = true;
-		unsigned old_done_count = done_count;
-		done_count = 0;
-
-		for (int i = 0; i < 6; i++) {
-			if (data_collected[i]) {
-				done_count++;
-
-			} else {
-				done = false;
-			}
-		}
-
-		if (old_done_count != done_count) {
-			mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
-		}
-
-		if (done) {
-			break;
-		}
-
-		/* 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 " : "");
-
-		/* allow user enough time to read the message */
-		sleep(1);
-
-		int orient = detect_orientation(mavlink_fd, subs);
-
-		if (orient < 0) {
-			mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still...");
-			sleep(2);
-			continue;
-		}
-
-		/* 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]);
-			sleep(1);
-			continue;
-		}
-
-		mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[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]);
-		usleep(100000);
-		mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %8.4f %8.4f %8.4f ]", orientation_strs[orient],
-				 (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);
+	if (result == OK) {
+		result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
 	}
 
 	/* 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)++;
+	for (unsigned i = 0; i < max_accel_sens; i++) {
+		if (worker_data.subs[i] >= 0) {
+			/* figure out which sensors were active */
+			struct accel_report arp = {};
+			(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
+			if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
+				(*active_sensors)++;
+			}
+			close(worker_data.subs[i]);
 		}
-		close(subs[i]);
 	}
 
-	if (res == OK) {
+	if (result == OK) {
 		/* calculate offsets and transform matrix */
 		for (unsigned i = 0; i < (*active_sensors); i++) {
-			res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-			if (res != OK) {
+			if (result != OK) {
 				mavlink_and_console_log_critical(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 (&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
+	return result;
 }
 
 /*
  * 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][detect_orientation_side_count][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);
 
@@ -620,7 +437,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];
 		}
@@ -652,7 +469,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][detect_orientation_side_count][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++) {