From 69986affbff2ea60aa8a27da193b7ebf5970c33a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 19 Oct 2020 22:16:55 -0400 Subject: [PATCH] commander: calibration restore sleep after CAL_QGC_DONE_MSG/CAL_QGC_FAILED_MSG --- src/modules/commander/accelerometer_calibration.cpp | 2 ++ src/modules/commander/gyro_calibration.cpp | 3 +++ src/modules/commander/level_calibration.cpp | 2 ++ 3 files changed, 7 insertions(+) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 9cd0d09c6d..8a64825586 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -442,11 +442,13 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) if (!failed) { calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + px4_usleep(600000); // give this message enough time to propagate return PX4_OK; } } calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + px4_usleep(600000); // give this message enough time to propagate return PX4_ERROR; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8362d5f5f6..0a2b1963b4 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -338,10 +338,13 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (!failed) { calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); + px4_usleep(600000); // give this message enough time to propagate return PX4_OK; } } calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); + px4_usleep(600000); // give this message enough time to propagate + return PX4_ERROR; } diff --git a/src/modules/commander/level_calibration.cpp b/src/modules/commander/level_calibration.cpp index c8e53bfacf..9e4092c887 100644 --- a/src/modules/commander/level_calibration.cpp +++ b/src/modules/commander/level_calibration.cpp @@ -175,10 +175,12 @@ out: if (success) { calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level"); + px4_usleep(600000); // give this message enough time to propagate return 0; } else { calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level"); + px4_usleep(600000); // give this message enough time to propagate return 1; } }