From e7a522edbc7fb04547a92ec7349458aadfd2a0de Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 19 May 2015 16:22:52 +0200 Subject: [PATCH] reset board rotation offset params if level calibration failed --- .../commander/accelerometer_calibration.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f4fd88fa22..32c0ffd8cc 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -568,6 +568,12 @@ int do_level_calibration(int mavlink_fd) { param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + // save old values if calibration fails + float roll_offset_current; + float pitch_offset_current; + param_get(roll_offset_handler, &roll_offset_current); + param_get(pitch_offset_handler, &pitch_offset_current); + float zero = 0.0f; param_set(roll_offset_handler, &zero); param_set(pitch_offset_handler, &zero); @@ -600,28 +606,34 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + bool success = false; if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); - return 1; } if (fabsf(roll_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_fd, "excess roll angle"); - return 1; } else if (fabsf(pitch_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle"); - return 1; - } - else { + } else { roll_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG; param_set(roll_offset_handler, &roll_mean); param_set(pitch_offset_handler, &pitch_mean); + success = true; } - mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); - return 0; + if (success) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); + return 0; + } else { + // set old parameters + param_set(roll_offset_handler, &roll_offset_current); + param_set(pitch_offset_handler, &pitch_offset_current); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); + return 1; + } }