From 362672ece8210fe64537e434d17b3917a3a7e29a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Apr 2015 17:33:45 +0200 Subject: commander: Fix calibration feedback so that QGC picks up all error conditions --- src/modules/commander/accelerometer_calibration.cpp | 15 ++++++++++----- src/modules/commander/calibration_messages.h | 10 +++++----- src/modules/commander/calibration_routines.cpp | 8 ++++---- src/modules/commander/gyro_calibration.cpp | 13 ++++++++++--- src/modules/commander/mag_calibration.cpp | 8 ++++++-- 5 files changed, 35 insertions(+), 19 deletions(-) (limited to 'src') 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; } -- cgit v1.2.3