diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 8562827c34..3ffa556e48 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -307,7 +307,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
+					calibration_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
 					t_still = t;
 					t_timeout = t + timeout;
 
@@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 				   accel_disp[2] > still_thr2 * 4.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
+					calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
 					usleep(200000);
 					t_still = 0;
 				}
@@ -339,7 +339,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 		}
 
 		if (poll_errcount > 1000) {
-			mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
+			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
 			return DETECT_ORIENTATION_ERROR;
 		}
 	}
@@ -380,7 +380,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 		return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
 	}
 
-	mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
+	calibration_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
 
 	return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
 }
@@ -413,7 +413,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 
 	int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
 	if (sub_accel < 0) {
-		mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
+		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
 		return calibrate_return_error;
 	}
 
@@ -428,7 +428,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 
 		if (orientation_failures > 4) {
 			result = calibrate_return_error;
-			mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
+			calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
 			break;
 		}
 
@@ -456,28 +456,26 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 				strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
 			}
 		}
-		mavlink_and_console_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
+		calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
 
-		mavlink_and_console_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
+		calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
 		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position);
 
 		if (orient == DETECT_ORIENTATION_ERROR) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
+			calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
 			continue;
 		}
 
 		/* inform user about already handled side */
 		if (side_data_collected[orient]) {
 			orientation_failures++;
-			mavlink_and_console_log_critical(mavlink_log_pub, "%s side already completed", detect_orientation_str(orient));
-			mavlink_and_console_log_critical(mavlink_log_pub, "rotate to a pending side");
+			calibration_log_critical(mavlink_log_pub, "%s side already completed", detect_orientation_str(orient));
+			calibration_log_critical(mavlink_log_pub, "rotate to a pending side");
 			continue;
 		}
 
-		mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
-		// TODO FIXME: sleep here, so that QGC receives this with higher chance.
-		usleep(10000);
+		calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
 		orientation_failures = 0;
 
 		// Call worker routine
@@ -486,7 +484,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 			break;
 		}
 
-		mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
+		calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
 
 		// Note that this side is complete
 		side_data_collected[orient] = true;
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index d4f6a545d7..e54149d8ea 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -113,3 +113,27 @@ void calibrate_cancel_unsubscribe(int cancel_sub);
 /// Used to periodically check for a cancel command
 bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub,	///< uORB handle to write output to
 			    int cancel_sub);	///< Cancel subcription fromcalibration_cancel_subscribe
+
+
+// TODO FIXME: below are workarounds for QGC. The issue is that sometimes
+// a mavlink log message is overwritten by the following one. A workaround
+// is to wait for some time after publishing each message and hope that it
+// gets sent out in the meantime.
+
+#define calibration_log_info(_pub, _text, ...)			\
+	do { \
+		mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \
+		usleep(5000); \
+	} while(0);
+
+#define calibration_log_critical(_pub, _text, ...)			\
+	do { \
+		mavlink_and_console_log_critical(_pub, _text, ##__VA_ARGS__); \
+		usleep(5000); \
+	} while(0);
+
+#define calibration_log_emergency(_pub, _text, ...)			\
+	do { \
+		mavlink_and_console_log_emergency(_pub, _text, ##__VA_ARGS__); \
+		usleep(5000); \
+	} while(0);