commander: Fix calibration feedback so that QGC picks up all error conditions

5 files changed, 35 insertions, 19 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index d0982b341..f83640d28 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -222,8 +222,10 @@ int do_accel_calibration(int mavlink_fd)
 		}
 	}
 
-	if (res != OK || active_sensors == 0) {
-		mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
+	if (res != OK) {
+		if (active_sensors == 0) {
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
+		}
 		return ERROR;
 	}
 
@@ -278,7 +280,7 @@ int do_accel_calibration(int mavlink_fd)
 		fd = open(str, 0);
 
 		if (fd < 0) {
-			mavlink_and_console_log_critical(mavlink_fd, "[cal] 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);
@@ -298,14 +300,17 @@ int do_accel_calibration(int mavlink_fd)
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 
+		/* 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_QGC_FAILED_MSG, sensor_name);
 	}
 	
-	// This give a chance for the log messages to go out of the queue before someone else stomps on then
-	sleep(1);
+	/* give this message enough time to propagate */
+	usleep(600000);
 
 	return res;
 }
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
index 2f6d02a72..7b4baae62 100644
--- a/src/modules/commander/calibration_messages.h
+++ b/src/modules/commander/calibration_messages.h
@@ -57,10 +57,10 @@
 #define CAL_QGC_ORIENTATION_DETECTED_MSG	"[cal] %s orientation detected"
 #define CAL_QGC_SIDE_DONE_MSG			"[cal] %s side done, rotate to a different side"
 
-#define CAL_ERROR_SENSOR_MSG		"[cal] ERROR: failed reading sensor"
-#define CAL_ERROR_RESET_CAL_MSG		"[cal] ERROR: failed to reset calibration, sensor %u"
-#define CAL_ERROR_APPLY_CAL_MSG		"[cal] ERROR: failed to apply calibration, sensor %u"
-#define CAL_ERROR_SET_PARAMS_MSG	"[cal] ERROR: failed to set parameters, sensor %u"
-#define CAL_ERROR_SAVE_PARAMS_MSG	"[cal] ERROR: failed to save parameters"
+#define CAL_ERROR_SENSOR_MSG		"[cal] calibration failed: reading sensor"
+#define CAL_ERROR_RESET_CAL_MSG		"[cal] calibration failed: to reset, sensor %u"
+#define CAL_ERROR_APPLY_CAL_MSG		"[cal] calibration failed: to apply calibration, sensor %u"
+#define CAL_ERROR_SET_PARAMS_MSG	"[cal] calibration failed: to set parameters, sensor %u"
+#define CAL_ERROR_SAVE_PARAMS_MSG	"[cal] calibration failed: failed to save parameters"
 
 #endif /* CALIBRATION_MESSAGES_H_ */
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index a320c5c5b..7e8c0fa52 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 				/* not still, reset still start time */
 				if (t_still != 0) {
 					mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
-					sleep(3);
+					usleep(500000);
 					t_still = 0;
 				}
 			}
@@ -412,7 +412,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 	
 	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
 	if (sub_accel < 0) {
-		mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: No onboard accel found");
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
 		return calibrate_return_error;
 	}
 	
@@ -427,7 +427,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 		
 		if (orientation_failures > 4) {
 			result = calibrate_return_error;
-			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: timeout waiting for orientation");
+			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
 			break;
 		}
 		
@@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 		// Note that this side is complete
 		side_data_collected[orient] = true;
 		tune_neutral(true);
-		sleep(1);
+		usleep(500000);
 	}
 	
 	if (sub_accel >= 0) {
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index e5df4175d..bdef8771e 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -298,10 +298,17 @@ int do_gyro_calibration(int mavlink_fd)
 		}
 	}
 
-	mavlink_log_info(mavlink_fd, res == OK ? CAL_QGC_DONE_MSG : CAL_QGC_FAILED_MSG, sensor_name);
+	/* if there is a any preflight-check system response, let the barrage of messages through */
+	usleep(200000);
 
-	// This give a chance for the log messages to go out of the queue before someone else stomps on then
-	sleep(1);
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+	} else {
+		mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+	}
+
+	/* give this message enough time to propagate */
+	usleep(600000);
 	
 	return res;
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 3d10f98c1..8a8d85818 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -158,6 +158,10 @@ int do_mag_calibration(int mavlink_fd)
 			case calibrate_return_ok:
 				/* auto-save to EEPROM */
 				result = param_save_default();
+
+				/* if there is a any preflight-check system response, let the barrage of messages through */
+				usleep(200000);
+
 				if (result == OK) {
 					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 					mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
@@ -173,8 +177,8 @@ int do_mag_calibration(int mavlink_fd)
 		}
 	}
 
-	// This give a chance for the log messages to go out of the queue before someone else stomps on then
-	sleep(1);
+	/* give this message enough time to propagate */
+	usleep(600000);
 	
 	return result;
 }