From e525ab3b73b449f2c4d7c477e1233c706c57b5a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 13:44:51 +0100 Subject: [PATCH] Commander: Fix accel cal duration --- .../commander/accelerometer_calibration.cpp | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d7a58e6254..dd6d5e3a9d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -326,7 +326,7 @@ int do_accel_calibration(int mavlink_fd) static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { - const unsigned samples_num = 3000; + const unsigned samples_num = 750; accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); @@ -574,7 +574,8 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; const unsigned cal_hz = 100; - const unsigned settle_time = 30; + unsigned settle_time = 30; + bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; @@ -582,18 +583,25 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level"); - param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); - param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF"); + param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF"); + param_t board_rot_handle = param_find("SENS_BOARD_ROT"); // 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); + int32_t board_rot_current = 0; + param_get(roll_offset_handle, &roll_offset_current); + param_get(pitch_offset_handle, &pitch_offset_current); + param_get(board_rot_handle, &board_rot_current); + + if (board_rot_current == 0) { + settle_time = 0; + } float zero = 0.0f; - param_set(roll_offset_handler, &zero); - param_set(pitch_offset_handler, &zero); + param_set(roll_offset_handle, &zero); + param_set(pitch_offset_handle, &zero); px4_pollfd_struct_t fds[1]; @@ -647,8 +655,8 @@ int do_level_calibration(int mavlink_fd) { } 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); + param_set(roll_offset_handle, &roll_mean); + param_set(pitch_offset_handle, &pitch_mean); success = true; } @@ -658,8 +666,8 @@ out: return 0; } else { // set old parameters - param_set(roll_offset_handler, &roll_offset_current); - param_set(pitch_offset_handler, &pitch_offset_current); + param_set(roll_offset_handle, &roll_offset_current); + param_set(pitch_offset_handle, &pitch_offset_current); mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); return 1; }