Commander: Improve calibration routines to produce more conscise and better sequenced instructions

4 files changed, 51 insertions, 25 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 7a4e7a766..0cb41489f 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -83,7 +83,7 @@
  *     | accel_T[1][i] |
  *     [ accel_T[2][i] ]
  *
- * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per side is enough
  *     | accel_corr_ref[2][i] |
  *     [ accel_corr_ref[4][i] ]
  *
@@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
+	mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
+	sleep(3);
+	mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
+	sleep(5);
+
 	struct accel_scale accel_scale = {
 		0.0f,
 		1.0f,
@@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	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-" };
+	const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			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- " : "");
+		/* inform user which axes are still needed */
+		mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
+				 (!data_collected[0]) ? "front " : "",
+				 (!data_collected[1]) ? "back " : "",
+				 (!data_collected[2]) ? "left " : "",
+				 (!data_collected[3]) ? "right " : "",
+				 (!data_collected[4]) ? "up " : "",
+				 (!data_collected[5]) ? "down " : "");
+
+		/* allow user enough time to read the message */
+		sleep(3);
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
 
 		if (orient < 0) {
-			res = ERROR;
-			break;
+			mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
+			sleep(3);
+			continue;
 		}
 
+		/* inform user about already handled side */
 		if (data_collected[orient]) {
-			mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
+			mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
+			sleep(4);
 			continue;
 		}
 
-		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
+		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);
-		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+		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]);
@@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
+					mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
 					t_still = t;
 					t_timeout = t + timeout;
 
@@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				/* not still, reset still start time */
 				if (t_still != 0) {
 					mavlink_log_info(mavlink_fd, "detected motion, hold still...");
+					sleep(3);
 					t_still = 0;
 				}
 			}
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 339b11bbe..cae1d7684 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -61,6 +61,15 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "dpress";
 
+#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
+
+static void feedback_calibration_failed(int mavlink_fd)
+{
+	sleep(5);
+	mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+	mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
+}
+
 int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
@@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		float analog_scaling = 0.0f;
 		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
 		if (fabsf(analog_scaling) < 0.1f) {
-			mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
+			mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			feedback_calibration_failed(mavlink_fd);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
 
 	} else {
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		feedback_calibration_failed(mavlink_fd);
 		close(diff_pres_sub);
 		return ERROR;
 	}
@@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
 				if (calibration_counter % 500 == 0) {
-					mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
+					mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
@@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
 				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
 				close(diff_pres_sub);
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 				close(diff_pres_sub);
 
-				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+				feedback_calibration_failed(mavlink_fd);
 				return ERROR;
 			} else {
 				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
@@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			feedback_calibration_failed(mavlink_fd);
 			close(diff_pres_sub);
 			return ERROR;
 		}
 	}
 
 	if (calibration_counter == maxcount) {
-		mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		feedback_calibration_failed(mavlink_fd);
 		close(diff_pres_sub);
 		return ERROR;
 	}
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index d89c67c2b..8ab14dd52 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
 int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
+	mavlink_log_info(mavlink_fd, "HOLD STILL");
+
+	/* wait for the user to respond */
+	sleep(2);
 
 	struct gyro_scale gyro_scale = {
 		0.0f,
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 23900f386..7be8de9c6 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
 		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
 		unsigned poll_errcount = 0;
 
-		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
+		mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
 
 		calibration_counter = 0;