From 3be1fc7d4271025e5681a2b1898760ca5ef0e930 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 May 2015 11:01:19 +0200 Subject: commander gyro calibration: Do not require a specific position, automatically start a retry after motion on the first try --- src/modules/commander/gyro_calibration.cpp | 93 +++++++++++++++++------------- 1 file changed, 53 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index bdef8771e..fc6398bd6 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -74,7 +74,7 @@ typedef struct { 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) +static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) { gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); unsigned calibration_counter[max_gyros] = { 0 }; @@ -89,6 +89,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient } memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); + memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale)); /* use first gyro to pace, but count correctly per-gyro for statistics */ while (calibration_counter[0] < calibration_count) { @@ -149,7 +150,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient int do_gyro_calibration(int mavlink_fd) { int res = OK; - gyro_worker_data_t worker_data; + gyro_worker_data_t worker_data = {}; mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -196,51 +197,63 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); } + + int cancel_sub = calibrate_cancel_subscribe(); + + unsigned try_count = 0; + unsigned max_tries = 20; + res = ERROR; - // 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 + do { + // Calibrate gyro and ensure user didn't move + calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data); + + if (cal_return == calibrate_return_cancelled) { + // Cancel message already sent, we are done here + res = ERROR; + break; + + } else if (cal_return == calibrate_return_error) { + res = ERROR; + + } else { + /* check offsets */ + 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(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, "[cal] motion, retrying.."); + res = ERROR; + + } else { + res = OK; + } + } + try_count++; + + } while (res == ERROR && try_count <= max_tries); + + if (try_count >= max_tries) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration"); + res = ERROR; + } + 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 = 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(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, "[cal] ERROR: Motion during calibration"); - res = ERROR; - } - } - if (res == OK) { /* set offset parameters to new values */ bool failed = false; -- cgit v1.2.3