From a7f88d97b819a73ce5b0e804df21f9c9dc1ebadd Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 25 Apr 2015 12:37:43 -0700 Subject: Sensor cal rework - cancel support - versioned cal messages - better still detection - better messaging --- .../commander/accelerometer_calibration.cpp | 80 ++--- src/modules/commander/airspeed_calibration.cpp | 106 +++---- src/modules/commander/calibration_messages.h | 30 +- src/modules/commander/calibration_routines.cpp | 141 ++++++--- src/modules/commander/calibration_routines.h | 71 +++-- src/modules/commander/commander.cpp | 35 +-- src/modules/commander/gyro_calibration.cpp | 325 ++++++++++++--------- src/modules/commander/mag_calibration.cpp | 190 ++++++------ src/modules/sensors/sensors.cpp | 24 +- 9 files changed, 572 insertions(+), 430 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 409c2ea00..d0982b341 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -152,11 +152,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); -int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); +calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); +calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); -int accel_calibration_worker(detect_orientation_return orientation, void* worker_data); +calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); /// Data passed to calibration worker routine typedef struct { @@ -171,7 +170,7 @@ int do_accel_calibration(int mavlink_fd) int fd; int32_t device_id[max_accel_sens]; - mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { 0.0f, @@ -202,7 +201,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); } } @@ -210,13 +209,21 @@ int do_accel_calibration(int mavlink_fd) float accel_T[max_accel_sens][3][3]; unsigned active_sensors; + /* measure and calculate offsets & scales */ if (res == OK) { - /* measure and calculate offsets & scales */ - res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors); + calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors); + if (cal_return == calibrate_return_cancelled) { + // Cancel message already displayed, nothing left to do + return ERROR; + } else if (cal_return == calibrate_return_ok) { + res = OK; + } else { + res = ERROR; + } } if (res != OK || active_sensors == 0) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); return ERROR; } @@ -263,7 +270,7 @@ int do_accel_calibration(int mavlink_fd) failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); if (failed) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i); return ERROR; } @@ -271,7 +278,7 @@ int do_accel_calibration(int mavlink_fd) fd = open(str, 0); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] sensor does not exist"); res = ERROR; } else { res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); @@ -279,7 +286,7 @@ int do_accel_calibration(int mavlink_fd) } if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i); } } @@ -288,41 +295,44 @@ int do_accel_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } - mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + 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); return res; } -int accel_calibration_worker(detect_orientation_return orientation, void* data) +static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { const unsigned samples_num = 3000; accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation)); + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); - mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation), + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation), (double)worker_data->accel_ref[0][orientation][0], (double)worker_data->accel_ref[0][orientation][1], (double)worker_data->accel_ref[0][orientation][2]); worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); - return OK; + return calibrate_return_ok; } -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) +calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) { - int result = OK; + calibrate_return result = calibrate_return_ok; *active_sensors = 0; @@ -343,7 +353,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac for (unsigned i = 0; i < max_accel_sens; i++) { worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); if (worker_data.subs[i] < 0) { - result = ERROR; + result = calibrate_return_error; break; } @@ -353,8 +363,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac timestamps[i] = arp.timestamp; } - if (result == OK) { - result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data); + if (result == calibrate_return_ok) { + int cancel_sub = calibrate_cancel_subscribe(); + result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */); + calibrate_cancel_unsubscribe(cancel_sub); } /* close all subscriptions */ @@ -370,13 +382,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac } } - if (result == OK) { + if (result == calibrate_return_ok) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (result != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); + if (result != calibrate_return_ok) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error"); break; } } @@ -388,7 +400,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) +calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { struct pollfd fds[max_accel_sens]; @@ -432,7 +444,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a } if (errcount > samples_num / 10) { - return ERROR; + return calibrate_return_error; } } @@ -442,7 +454,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a } } - return OK; + return calibrate_return_ok; } int mat_invert3(float src[3][3], float dst[3][3]) @@ -468,7 +480,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) return OK; } -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) +calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) { /* calculate offsets */ for (unsigned i = 0; i < 3; i++) { @@ -490,7 +502,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s float mat_A_inv[3][3]; if (mat_invert3(mat_A, mat_A_inv) != OK) { - return ERROR; + return calibrate_return_error; } /* copy results to accel_T */ @@ -501,5 +513,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s } } - return OK; + return calibrate_return_ok; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 747d915ff..837a3f1e8 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -38,6 +38,7 @@ #include "airspeed_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -61,19 +62,20 @@ 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); + mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(int mavlink_fd) { + int result = OK; + unsigned calibration_counter = 0; + const unsigned maxcount = 3000; + /* give directions */ - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); const unsigned calibration_count = 2000; @@ -96,11 +98,13 @@ int do_airspeed_calibration(int mavlink_fd) paramreset_successful = true; } else { - mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); + mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); } close(fd); } + + int cancel_sub = calibrate_cancel_subscribe(); if (!paramreset_successful) { @@ -108,26 +112,26 @@ 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, "No airspeed sensor, see http://px4.io/help/aspd"); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); + goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } } - unsigned calibration_counter = 0; - - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; @@ -142,14 +146,13 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); + mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } } @@ -161,16 +164,15 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); } close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } /* auto-save to EEPROM */ @@ -178,30 +180,31 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + goto error_return; } } else { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "Create airflow now"); + mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); calibration_counter = 0; - const unsigned maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ struct pollfd fds[1]; fds[0].fd = diff_pres_sub; @@ -216,7 +219,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 air pressure! (got %d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -224,30 +227,26 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + mavlink_log_info(mavlink_fd, "[cal] 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); + mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); - return ERROR; + mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + goto error_return; } /* save */ - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); + mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); (void)param_save_default(); - close(diff_pres_sub); - feedback_calibration_failed(mavlink_fd); - return ERROR; + goto error_return; } else { - mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } @@ -255,21 +254,30 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } } if (calibration_counter == maxcount) { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); - return ERROR; + goto error_return; } - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); + +normal_return: + calibrate_cancel_unsubscribe(cancel_sub); close(diff_pres_sub); - return OK; + + // This give a chance for the log messages to go out of the queue before someone else stomps on then + sleep(1); + + return result; + +error_return: + result = ERROR; + goto normal_return; } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 29f44dc36..2f6d02a72 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -42,17 +42,25 @@ #ifndef CALIBRATION_MESSAGES_H_ #define CALIBRATION_MESSAGES_H_ -#define CAL_STARTED_MSG "%s calibration: started" -#define CAL_DONE_MSG "%s calibration: done" -#define CAL_FAILED_MSG "%s calibration: failed" -#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" +// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state +// machine to provide visual feedback for calibration. As such, the text for them or semantics of when +// they are displayed cannot be modified without causing QGC to break. If modifications are made, make +// sure to bump the calibration version number which will cause QGC to perform log based calibration +// instead of visual calibration until such a time as QGC is update to the new version. -#define CAL_FAILED_UNKNOWN_ERROR "ERROR: unknown error" -#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" -#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration, sensor %u" -#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration, sensor %u" -#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters, sensor %u" -#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" -#define CAL_FAILED_ORIENTATION_TIMEOUT "ERROR: timed out waiting for correct orientation" +// The number in the cal started message is used to indicate the version stamp for the current calibration code. +#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" +#define CAL_QGC_DONE_MSG "[cal] calibration done: %s" +#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" +#define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled" +#define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>" +#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" #endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 192b8c727..a320c5c5b 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -48,6 +48,8 @@ #include #include +#include + #include "calibration_routines.h" #include "calibration_messages.h" #include "commander_helper.h" @@ -230,23 +232,19 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } -enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) +enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position) { const unsigned ndim = 3; struct accel_report sensor; - /* exponential moving average of accel */ - float accel_ema[ndim] = { 0.0f }; - /* max-hold dispersion of accel */ - float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - /* EMA time constant in seconds*/ - float ema_len = 0.5f; - /* set "still" threshold to 0.25 m/s^2 */ - float still_thr2 = powf(0.25f, 2); - /* set accel error threshold to 5m/s^2 */ - float accel_err_thr = 5.0f; - /* still time required in us */ - hrt_abstime still_time = 2000000; + float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel + float ema_len = 0.5f; // EMA time constant in seconds + const float normal_still_thr = 0.25; // normal still threshold + float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); + float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 + hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us + struct pollfd fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -308,7 +306,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -325,7 +323,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); sleep(3); t_still = 0; } @@ -340,7 +338,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) } if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG); return DETECT_ORIENTATION_ERROR; } } @@ -381,7 +379,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] } - mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation"); return DETECT_ORIENTATION_ERROR; // Can't detect orientation } @@ -401,28 +399,35 @@ const char* detect_orientation_str(enum detect_orientation_return orientation) return rgOrientationStrs[orientation]; } -int calibrate_from_orientation(int mavlink_fd, - bool side_data_collected[detect_orientation_side_count], - calibration_from_orientation_worker_t calibration_worker, - void* worker_data) +calibrate_return calibrate_from_orientation(int mavlink_fd, + int cancel_sub, + bool side_data_collected[detect_orientation_side_count], + calibration_from_orientation_worker_t calibration_worker, + void* worker_data, + bool lenient_still_position) { - int result = OK; + calibrate_return result = calibrate_return_ok; // Setup subscriptions to onboard accel sensor int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0); if (sub_accel < 0) { - mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort"); - return ERROR; + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: No onboard accel found"); + return calibrate_return_error; } unsigned orientation_failures = 0; - // Rotate through all three main positions + // Rotate through all requested orientation while (true) { - if (orientation_failures > 10) { - result = ERROR; - mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT); + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + result = calibrate_return_cancelled; + break; + } + + if (orientation_failures > 4) { + result = calibrate_return_error; + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: timeout waiting for orientation"); break; } @@ -450,32 +455,35 @@ int calibrate_from_orientation(int mavlink_fd, strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation)); } } - mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr); + mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr); - mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides"); - enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel); + mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side"); + enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position); if (orient == DETECT_ORIENTATION_ERROR) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); continue; } /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient)); - mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side"); + mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient)); + mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side"); continue; } - mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient)); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient)); orientation_failures = 0; // Call worker routine - calibration_worker(orient, worker_data); + result = calibration_worker(orient, cancel_sub, worker_data); + if (result != calibrate_return_ok ) { + break; + } - mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient)); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient)); // Note that this side is complete side_data_collected[orient] = true; @@ -487,7 +495,62 @@ int calibrate_from_orientation(int mavlink_fd, close(sub_accel); } - // FIXME: Do we need an orientation complete routine? - return result; } + +int calibrate_cancel_subscribe(void) +{ + return orb_subscribe(ORB_ID(vehicle_command)); +} + +void calibrate_cancel_unsubscribe(int cmd_sub) +{ + orb_unsubscribe(cmd_sub); +} + +static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(true); + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command); + tune_negative(true); + break; + + default: + break; + } +} + +bool calibrate_cancel_check(int mavlink_fd, int cancel_sub) +{ + struct pollfd fds[1]; + fds[0].fd = cancel_sub; + fds[0].events = POLLIN; + + if (poll(&fds[0], 1, 0) > 0) { + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd); + + if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION && + (int)cmd.param1 == 0 && + (int)cmd.param2 == 0 && + (int)cmd.param3 == 0 && + (int)cmd.param4 == 0 && + (int)cmd.param5 == 0 && + (int)cmd.param6 == 0) { + calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_ACCEPTED); + mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG); + return true; + } else { + calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_DENIED); + } + } + + return false; +} \ No newline at end of file diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 8f34f0204..b8232730a 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,12 +31,8 @@ * ****************************************************************************/ -/** - * @file calibration_routines.h - * Calibration routines definitions. - * - * @author Lorenz Meier - */ +/// @file calibration_routines.h +/// @authot Don Gagne /** * Least-squares fit of a sphere to a set of points. @@ -75,30 +71,45 @@ enum detect_orientation_return { }; static const unsigned detect_orientation_side_count = 6; -/** - * Wait for vehicle to become still and detect it's orientation. - * - * @param mavlink_fd the MAVLink file descriptor to print output to - * @param accel_sub Subscription to onboard accel - * - * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements, - * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 - */ -enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub); +/// Wait for vehicle to become still and detect it's orientation +/// @return Returns detect_orientation_return according to orientation when vehicle +/// and ready for measurements +enum detect_orientation_return detect_orientation(int mavlink_fd, ///< Mavlink fd to write output to + int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe + int accel_sub, ///< Orb subcription to accel sensor + bool lenient_still_detection); ///< true: Use more lenient still position detection - -/** - * Returns the human readable string representation of the orientation - * - * @param orientation Orientation to return string for, "error" if buffer is too small - * - * @return str Returned orientation string - */ +/// Returns the human readable string representation of the orientation +/// @param orientation Orientation to return string for, "error" if buffer is too small const char* detect_orientation_str(enum detect_orientation_return orientation); -typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data); +enum calibrate_return { + calibrate_return_ok, + calibrate_return_error, + calibrate_return_cancelled +}; + +typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, ///< Orientation which was detected + int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe + void* worker_data); ///< Opaque worker data + +/// Perform calibration sequence which require a rest orientation detection prior to calibration. +/// @return OK: Calibration succeeded, ERROR: Calibration failed +calibrate_return calibrate_from_orientation(int mavlink_fd, ///< Mavlink fd to write output to + int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe + bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration + calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration + void* worker_data, ///< Opaque data passed to worker routine + bool lenient_still_detection); ///< true: Use more lenient still position detection + +/// Called at the beginning of calibration in order to subscribe to the cancel command +/// @return Handle to vehicle_command subscription +int calibrate_cancel_subscribe(void); + +/// Called to cancel the subscription to the cancel command +/// @param cancel_sub Cancel subcription from calibration_cancel_subscribe +void calibrate_cancel_unsubscribe(int cancel_sub); -int calibrate_from_orientation(int mavlink_fd, - bool side_data_collected[detect_orientation_side_count], - calibration_from_orientation_worker_t calibration_worker, - void* worker_data); +/// Used to periodically check for a cancel command +bool calibrate_cancel_check(int mavlink_fd, ///< Mavlink fd for output + int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 57afcf19a..8cce01778 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2723,35 +2723,32 @@ void *commander_low_prio_loop(void *arg) /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + calib_ret = OK; } - - /* this always succeeds */ - calib_ret = OK; - } if (calib_ret == OK) { tune_positive(true); - } else { - tune_negative(true); - } + // Update preflight check status + // we do not set the calibration return value based on it because the calibration + // might have worked just fine, but the preflight check fails for a different reason, + // so this would be prone to false negatives. - // Update preflight check status - // we do not set the calibration return value based on it because the calibration - // might have worked just fine, but the preflight check fails for a different reason, - // so this would be prone to false negatives. + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { - checkAirspeed = true; - } + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + } else { + tune_negative(true); + } break; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 291ccfe80..e5df4175d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -39,6 +39,7 @@ #include "gyro_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -62,142 +63,180 @@ static const int ERROR = -1; static const char *sensor_name = "gyro"; -int do_gyro_calibration(int mavlink_fd) -{ - const unsigned max_gyros = 3; - - int32_t device_id[3]; - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "HOLD STILL"); - - /* wait for the user to respond */ - sleep(2); - - struct gyro_scale gyro_scale_zero = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - struct gyro_scale gyro_scale[max_gyros] = {}; +static const unsigned max_gyros = 3; - int res = OK; - - char str[30]; +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + int32_t device_id[max_gyros]; + int gyro_sensor_sub[max_gyros]; + struct gyro_scale gyro_scale[max_gyros]; + struct gyro_report gyro_report_0; +} gyro_worker_data_t; +static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) +{ + gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); + unsigned calibration_counter[max_gyros] = { 0 }; + const unsigned calibration_count = 5000; + struct gyro_report gyro_report; + unsigned poll_errcount = 0; + + struct pollfd fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { - - /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); - - sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - /* reset all offsets to zero and all scales to one */ - int fd = open(str, 0); - - if (fd < 0) { - continue; + fds[s].fd = worker_data->gyro_sensor_sub[s]; + fds[s].events = POLLIN; + } + + memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); + + /* use first gyro to pace, but count correctly per-gyro for statistics */ + while (calibration_counter[0] < calibration_count) { + if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { + return calibrate_return_cancelled; } - - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + + int poll_ret = poll(&fds[0], max_gyros, 1000); + + if (poll_ret > 0) { + + for (unsigned s = 0; s < max_gyros; s++) { + bool changed; + orb_check(worker_data->gyro_sensor_sub[s], &changed); + + if (changed) { + orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); + } + + worker_data->gyro_scale[s].x_offset += gyro_report.x; + worker_data->gyro_scale[s].y_offset += gyro_report.y; + worker_data->gyro_scale[s].z_offset += gyro_report.z; + calibration_counter[s]++; + } + + if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { + mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); + } + } + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); + return calibrate_return_error; } } - - unsigned calibration_counter[max_gyros] = { 0 }; - const unsigned calibration_count = 5000; - - struct gyro_report gyro_report_0 = {}; - - if (res == OK) { - /* determine gyro mean values */ - unsigned poll_errcount = 0; - - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro[max_gyros]; - struct pollfd fds[max_gyros]; - - for (unsigned s = 0; s < max_gyros; s++) { - sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); - fds[s].fd = sub_sensor_gyro[s]; - fds[s].events = POLLIN; + + for (unsigned s = 0; s < max_gyros; s++) { + if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { + mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) + return calibrate_return_error; } - struct gyro_report gyro_report; - - /* use first gyro to pace, but count correctly per-gyro for statistics */ - while (calibration_counter[0] < calibration_count) { - /* wait blocking for new data */ - - int poll_ret = poll(&fds[0], max_gyros, 1000); - - if (poll_ret > 0) { - - for (unsigned s = 0; s < max_gyros; s++) { - bool changed; - orb_check(sub_sensor_gyro[s], &changed); - - if (changed) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); - - if (s == 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); - } + worker_data->gyro_scale[s].x_offset /= calibration_counter[s]; + worker_data->gyro_scale[s].y_offset /= calibration_counter[s]; + worker_data->gyro_scale[s].z_offset /= calibration_counter[s]; + } - gyro_scale[s].x_offset += gyro_report.x; - gyro_scale[s].y_offset += gyro_report.y; - gyro_scale[s].z_offset += gyro_report.z; - calibration_counter[s]++; - } + return calibrate_return_ok; +} - if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count); - } - } +int do_gyro_calibration(int mavlink_fd) +{ + int res = OK; + gyro_worker_data_t worker_data; - } else { - poll_errcount++; - } + mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + worker_data.mavlink_fd = mavlink_fd; + + struct gyro_scale gyro_scale_zero = { + 0.0f, // x offset + 1.0f, // x scale + 0.0f, // y offset + 1.0f, // y scale + 0.0f, // z offset + 1.0f, // z scale + }; + + for (unsigned s = 0; s < max_gyros; s++) { + char str[30]; + + // Reset gyro ids to unavailable + worker_data.device_id[s] = 0; + (void)sprintf(str, "CAL_GYRO%u_ID", s); + res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); + return ERROR; } + + // Reset all offsets to 0 and scales to 1 + (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale)); + sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + int fd = open(str, 0); + if (fd >= 0) { + worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + close(fd); - for (unsigned s = 0; s < max_gyros; s++) { - close(sub_sensor_gyro[s]); - - gyro_scale[s].x_offset /= calibration_counter[s]; - gyro_scale[s].y_offset /= calibration_counter[s]; - gyro_scale[s].z_offset /= calibration_counter[s]; + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); + return ERROR; + } } + + } + + for (unsigned s = 0; s < max_gyros; s++) { + worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + } + + // Calibrate right-side up + + bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false }; + + int cancel_sub = calibrate_cancel_subscribe(); + calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output + cancel_sub, // Subscription to vehicle_command for cancel support + side_collected, // Sides to calibrate + gyro_calibration_worker, // Calibration worker + &worker_data, // Opaque data for calibration worked + true); // true: lenient still detection + calibrate_cancel_unsubscribe(cancel_sub); + + for (unsigned s = 0; s < max_gyros; s++) { + close(worker_data.gyro_sensor_sub[s]); } + if (cal_return == calibrate_return_cancelled) { + // Cancel message already sent, we are done here + return ERROR; + } else if (cal_return == calibrate_return_error) { + res = ERROR; + } + if (res == OK) { /* check offsets */ - float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; - float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; - float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; + float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset; + float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ const float maxoff = 0.0055f; - if (!isfinite(gyro_scale[0].x_offset) || - !isfinite(gyro_scale[0].y_offset) || - !isfinite(gyro_scale[0].z_offset) || + if (!isfinite(worker_data.gyro_scale[0].x_offset) || + !isfinite(worker_data.gyro_scale[0].y_offset) || + !isfinite(worker_data.gyro_scale[0].z_offset) || fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration"); res = ERROR; } } @@ -207,40 +246,38 @@ int do_gyro_calibration(int mavlink_fd) bool failed = false; for (unsigned s = 0; s < max_gyros; s++) { + if (worker_data.device_id[s] != 0) { + char str[30]; + + (void)sprintf(str, "CAL_GYRO%u_XOFF", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset))); + (void)sprintf(str, "CAL_GYRO%u_YOFF", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset))); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset))); + (void)sprintf(str, "CAL_GYRO%u_ID", s); + failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); + + /* apply new scaling and offsets */ + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + int fd = open(str, 0); + + if (fd < 0) { + failed = true; + continue; + } - /* if any reasonable amount of data is missing, skip */ - if (calibration_counter[s] < calibration_count / 2) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset))); - (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset))); - (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset))); - (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s]))); - - /* apply new scaling and offsets */ - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); - - if (fd < 0) { - failed = true; - continue; - } + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]); + close(fd); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + } } } if (failed) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params"); res = ERROR; } } @@ -257,16 +294,14 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } } - if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - - } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - } + mavlink_log_info(mavlink_fd, res == OK ? CAL_QGC_DONE_MSG : 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); + return res; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 198acb027..3d10f98c1 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,8 +65,7 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; -int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); -int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); +calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); /// Data passed to calibration worker routine typedef struct { @@ -86,7 +85,7 @@ typedef struct { int do_mag_calibration(int mavlink_fd) { - mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); struct mag_scale mscale_null = { 0.0f, @@ -113,7 +112,7 @@ int do_mag_calibration(int mavlink_fd) (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); if (result != OK) { - mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag); + mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); break; } @@ -131,15 +130,15 @@ int do_mag_calibration(int mavlink_fd) result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (result != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag); } + /* calibrate range */ if (result == OK) { - /* calibrate range */ result = ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { - mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag); + mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag); /* this is non-fatal - mark it accordingly */ result = OK; } @@ -148,39 +147,48 @@ int do_mag_calibration(int mavlink_fd) close(fd); } + // Calibrate all mags at the same time if (result == OK) { - // Calibrate all mags at the same time - result = mag_calibrate_all(mavlink_fd, device_ids); - } - - if (result == OK) { - /* auto-save to EEPROM */ - result = param_save_default(); - if (result != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + switch (mag_calibrate_all(mavlink_fd, device_ids)) { + case calibrate_return_cancelled: + // Cancel message already displayed, we're done here + result = ERROR; + break; + + case calibrate_return_ok: + /* auto-save to EEPROM */ + result = param_save_default(); + 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); + break; + } else { + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + } + // Fall through + + default: + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + break; } } - - if (result == OK) { - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); - mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - } else { - mavlink_and_console_log_critical(mavlink_fd, CAL_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); + return result; } -int mag_calibration_worker(detect_orientation_return orientation, void* data) +static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { - int result = OK; + calibrate_return result = calibrate_return_ok; unsigned int calibration_counter_side; mag_worker_data_t* worker_data = (mag_worker_data_t*)(data); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation"); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation"); + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); sleep(2); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; @@ -191,6 +199,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { + if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { + result = calibrate_return_cancelled; + break; + } + // Wait clocking for new data on all mags struct pollfd fds[max_mags]; size_t fd_count = 0; @@ -222,8 +235,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) // Progress indicator for side mavlink_and_console_log_info(worker_data->mavlink_fd, - "%s %s side calibration: progress <%u>", - sensor_name, + "[cal] %s side calibration: progress <%u>", detect_orientation_str(orientation), (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); } else { @@ -231,50 +243,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) } if (poll_errcount > worker_data->calibration_points_perside * 3) { - result = ERROR; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + result = calibrate_return_error; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); break; } } - // Mark the opposite side as collected as well. No need to collect opposite side since it - // would generate similar points. - detect_orientation_return alternateOrientation = orientation; - switch (orientation) { - case DETECT_ORIENTATION_TAIL_DOWN: - alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; - break; - case DETECT_ORIENTATION_NOSE_DOWN: - alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; - break; - case DETECT_ORIENTATION_LEFT: - alternateOrientation = DETECT_ORIENTATION_RIGHT; - break; - case DETECT_ORIENTATION_RIGHT: - alternateOrientation = DETECT_ORIENTATION_LEFT; - break; - case DETECT_ORIENTATION_UPSIDE_DOWN: - alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; - break; - case DETECT_ORIENTATION_RIGHTSIDE_UP: - alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; - break; - case DETECT_ORIENTATION_ERROR: - warnx("Invalid orientation in mag_calibration_worker"); - break; + if (result == calibrate_return_ok) { + mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); } - worker_data->side_data_collected[alternateOrientation] = true; - mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); - - worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); return result; } -int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) { - int result = OK; + calibrate_return result = calibrate_return_ok; mag_worker_data_t worker_data; @@ -285,10 +272,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; - // Initialize to collect all sides - for (size_t cur_side=0; cur_side<6; cur_side++) { - worker_data.side_data_collected[cur_side] = false; - } + // Collect: Right-side up, Left Side, Nose down + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); - result = ERROR; + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory"); + result = calibrate_return_error; } } // Setup subscriptions to mag sensors - if (result == OK) { + if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag