From 26f9a1d42c6402a283a679e66236f247fd274cd2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 26 Apr 2013 15:25:17 +0400 Subject: abs/fabs bugfix, logging cleanup --- apps/commander/accelerometer_calibration.c | 80 +++++++++++------------------- 1 file changed, 29 insertions(+), 51 deletions(-) (limited to 'apps/commander/accelerometer_calibration.c') diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index 9e7a3b99e..f607b6da6 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -80,7 +80,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { /* announce change */ - mavlink_log_info(mavlink_fd, "accelerometer calibration started"); + mavlink_log_info(mavlink_fd, "accel calibration started"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -96,7 +96,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { - mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); + mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); @@ -122,7 +122,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); sleep(2); tune_confirm(); @@ -142,12 +141,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) { const int samples_num = 2500; - - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int16_t accel_raw_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); while (true) { bool done = true; char str[80]; @@ -159,25 +157,21 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], done = false; } } - if (done) { - mavlink_log_info(mavlink_fd, "all accel measurements complete"); + if (done) break; - } else { - mavlink_log_info(mavlink_fd, str); - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) { - sprintf(str, "orientation detection error: %i", orient); - mavlink_log_info(mavlink_fd, str); - return ERROR; - } - mavlink_log_info(mavlink_fd, "accel measurement started"); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); - //mavlink_log_info(mavlink_fd, "accel measurement complete"); - str_ptr = sprintf(str, "complete: %i [ %i %i %i ]", orient, accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); - data_collected[orient] = true; - tune_confirm(); - } + mavlink_log_info(mavlink_fd, str); + + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) + return ERROR; + + sprintf(str, "meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %i %i %i ]", orientation_strs[orient], accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_confirm(); } close(sensor_combined_sub); @@ -186,26 +180,16 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_T[3][3]; int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { - mavlink_log_info(mavlink_fd, "calibration values calculation error"); + mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); return ERROR; } - char str[80]; - sprintf(str, "accel offsets: [ %i %i %i ]", accel_offs[0], accel_offs[1], accel_offs[2]); - mavlink_log_info(mavlink_fd, str); - //mavlink_log_info(mavlink_fd, "accel transform matrix:"); - for (int i = 0; i < 3; i++) { - //sprintf(str, "\t[ %0.6f %0.6f %0.6f ]", accel_T[i][0], accel_T[i][1], accel_T[i][2]); - //mavlink_log_info(mavlink_fd, str); - } - /* convert raw accel offset to scaled and transform matrix to scales * rotation part of transform matrix is not used by now */ for (int i = 0; i < 3; i++) { accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i]; accel_scale[i] = accel_T[i][i]; } - return OK; } @@ -226,8 +210,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float ema_len = 0.2f; /* set "still" threshold to 0.005 m/s^2 */ float still_thr2 = pow(0.05f / CONSTANTS_ONE_G, 2); - /* set accel error threshold to 20% of accel vector length */ - float accel_err_thr = 0.2f; + /* set accel error threshold to 30% of accel vector length */ + float accel_err_thr = 0.3f; /* still time required in us */ int64_t still_time = 2000000; struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; @@ -264,7 +248,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "still"); + mavlink_log_info(mavlink_fd, "still..."); t_still = t; t_timeout = t + timeout; } else { @@ -279,7 +263,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { accel_disp[2] > still_thr_raw2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving"); + mavlink_log_info(mavlink_fd, "moving..."); t_still = 0; } } @@ -289,17 +273,11 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { return -3; } if (t > t_timeout) { - mavlink_log_info(mavlink_fd, "ERROR: timeout"); return -1; } } float accel_len = sqrt(accel_len2); float accel_err_thr_raw = accel_len * accel_err_thr; - char str[80]; - sprintf(str, "ema: [ %.1f %.1f %.1f ]", accel_ema[0], accel_ema[1], accel_ema[2]); - mavlink_log_info(mavlink_fd, str); - sprintf(str, "disp: [ %.1f %.1f %.1f ]", accel_disp[0], accel_disp[1], accel_disp[2]); - mavlink_log_info(mavlink_fd, str); if ( fabs(accel_ema[0] - accel_len) < accel_err_thr_raw && fabs(accel_ema[1]) < accel_err_thr_raw && fabs(accel_ema[2]) < accel_err_thr_raw ) @@ -316,13 +294,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { fabs(accel_ema[1] + accel_len) < accel_err_thr_raw && fabs(accel_ema[2]) < accel_err_thr_raw ) return 3; // [ 0, -g, 0 ] - if ( abs(accel_ema[0]) < accel_err_thr_raw && - abs(accel_ema[1]) < accel_err_thr_raw && - abs(accel_ema[2] - accel_len) < accel_err_thr_raw ) + if ( fabs(accel_ema[0]) < accel_err_thr_raw && + fabs(accel_ema[1]) < accel_err_thr_raw && + fabs(accel_ema[2] - accel_len) < accel_err_thr_raw ) return 4; // [ 0, 0, g ] - if ( abs(accel_ema[0]) < accel_err_thr_raw && - abs(accel_ema[1]) < accel_err_thr_raw && - abs(accel_ema[2] + accel_len) < accel_err_thr_raw ) + if ( fabs(accel_ema[0]) < accel_err_thr_raw && + fabs(accel_ema[1]) < accel_err_thr_raw && + fabs(accel_ema[2] + accel_len) < accel_err_thr_raw ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); return -2; // Can't detect orientation -- cgit v1.2.3