1 files changed, 52 insertions, 35 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 409c2ea00..f83640d28 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -152,11 +152,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
-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);
+calibrate_return 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);
+calibrate_return 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_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);
+calibrate_return 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);
 
 /// Data passed to calibration worker routine
 typedef struct  {
@@ -171,7 +170,7 @@ int do_accel_calibration(int mavlink_fd)
 	int fd;
 	int32_t device_id[max_accel_sens];
 
-	mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	struct accel_scale accel_scale = {
 		0.0f,
@@ -202,7 +201,7 @@ int do_accel_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 		}
 	}
 
@@ -210,13 +209,23 @@ int do_accel_calibration(int mavlink_fd)
 	float accel_T[max_accel_sens][3][3];
 	unsigned active_sensors;
 
+	/* measure and calculate offsets & scales */
 	if (res == OK) {
-		/* measure and calculate offsets & scales */
-		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
+		calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
+		if (cal_return == calibrate_return_cancelled) {
+			// Cancel message already displayed, nothing left to do
+			return ERROR;
+		} else if (cal_return == calibrate_return_ok) {
+			res = OK;
+		} else {
+			res = ERROR;
+		}
 	}
 
-	if (res != OK || active_sensors == 0) {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+	if (res != OK) {
+		if (active_sensors == 0) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
+		}
 		return ERROR;
 	}
 
@@ -263,7 +272,7 @@ int do_accel_calibration(int mavlink_fd)
 		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, i);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
 			return ERROR;
 		}
 
@@ -271,7 +280,7 @@ int do_accel_calibration(int mavlink_fd)
 		fd = open(str, 0);
 
 		if (fd < 0) {
-			mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
+			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
 			res = ERROR;
 		} else {
 			res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -279,7 +288,7 @@ int do_accel_calibration(int mavlink_fd)
 		}
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
 		}
 	}
 
@@ -288,41 +297,47 @@ int do_accel_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 
-		mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+		/* if there is a any preflight-check system response, let the barrage of messages through */
+		usleep(200000);
+
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 
 	} else {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
+	
+	/* give this message enough time to propagate */
+	usleep(600000);
 
 	return res;
 }
 
-int accel_calibration_worker(detect_orientation_return orientation, void* data)
+static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, 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));
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
 	
 	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),
+	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %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);
+	mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
 	
-	return OK;
+	return calibrate_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)
+calibrate_return 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;
+	calibrate_return result = calibrate_return_ok;
 	
 	*active_sensors = 0;
 	
@@ -343,7 +358,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 	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;
+			result = calibrate_return_error;
 			break;
 		}
 		
@@ -353,8 +368,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 		timestamps[i] = arp.timestamp;
 	}
 
-	if (result == OK) {
-		result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
+	if (result == calibrate_return_ok) {
+		int cancel_sub = calibrate_cancel_subscribe();
+		result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
+		calibrate_cancel_unsubscribe(cancel_sub);
 	}
 
 	/* close all subscriptions */
@@ -370,13 +387,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 		}
 	}
 
-	if (result == OK) {
+	if (result == calibrate_return_ok) {
 		/* calculate offsets and transform matrix */
 		for (unsigned i = 0; i < (*active_sensors); i++) {
 			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-			if (result != OK) {
-				mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
+			if (result != calibrate_return_ok) {
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
 				break;
 			}
 		}
@@ -388,7 +405,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-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)
+calibrate_return 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_accel_sens];
 
@@ -432,7 +449,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
 		}
 
 		if (errcount > samples_num / 10) {
-			return ERROR;
+			return calibrate_return_error;
 		}
 	}
 
@@ -442,7 +459,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
 		}
 	}
 
-	return OK;
+	return calibrate_return_ok;
 }
 
 int mat_invert3(float src[3][3], float dst[3][3])
@@ -468,7 +485,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
 	return OK;
 }
 
-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)
+calibrate_return 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++) {
@@ -490,7 +507,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
 	float mat_A_inv[3][3];
 
 	if (mat_invert3(mat_A, mat_A_inv) != OK) {
-		return ERROR;
+		return calibrate_return_error;
 	}
 
 	/* copy results to accel_T */
@@ -501,5 +518,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
 		}
 	}
 
-	return OK;
+	return calibrate_return_ok;
 }