diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index f607b6da69..1807369080 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -78,6 +78,13 @@ #include #include +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); + void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); @@ -85,14 +92,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - float accel_offs_scaled[3]; + /* measure and calculate offsets & scales */ + float accel_offs[3]; float accel_scale[3]; - int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs_scaled, accel_scale); + int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + if (res == OK) { /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_scaled[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_scaled[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_scaled[2])) + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) || 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]))) { @@ -101,11 +110,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { - accel_offs_scaled[0], + accel_offs[0], accel_scale[0], - accel_offs_scaled[1], + accel_offs[1], accel_scale[1], - accel_offs_scaled[2], + accel_offs[2], accel_scale[2], }; @@ -139,12 +148,30 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m state_machine_publish(status_pub, status, mavlink_fd); } -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) { +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; - int16_t accel_raw_ref[6][3]; + float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + /* reset existing calibration */ + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); + close(fd); + + if (OK != ioctl_res) { + warn("ERROR: failed to set scale / offsets for accel"); + return ERROR; + } + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); while (true) { bool done = true; @@ -167,8 +194,8 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], 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]); + read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; tune_confirm(); @@ -176,20 +203,20 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], close(sensor_combined_sub); /* calculate offsets and rotation+scale matrix */ - int16_t accel_offs[3]; float accel_T[3][3]; - int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); return ERROR; } - /* convert raw accel offset to scaled and transform matrix to scales - * rotation part of transform matrix is not used by now */ + /* convert accel 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; } @@ -233,8 +260,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { t_prev = t; float w = dt / ema_len; for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_raw[i] * w; - float d = (float) sensor.accelerometer_raw[i] - accel_ema[i]; + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; + float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); if (d > accel_disp[i]) @@ -273,9 +300,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; if ( fabs(accel_ema[0] - accel_len) < accel_err_thr_raw && @@ -302,56 +331,47 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { 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 } /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], int samples_num) { +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; int count = 0; - int32_t accel_sum[3] = { 0, 0, 0 }; + float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); if (poll_ret == 1) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) { - accel_sum[i] += sensor.accelerometer_raw[i]; - } + for (int i = 0; i < 3; i++) + accel_sum[i] += sensor.accelerometer_m_s2[i]; count++; } else { return ERROR; } } - for (int i = 0; i < 3; i++) { - accel_avg[i] = (accel_sum[i] + count / 2) / count; - } - /* calculate dispersion */ - return OK; -} -/* - * Convert raw values from accelerometers to m/s^2. - */ -void acceleration_raw_to_m_s2(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]) { for (int i = 0; i < 3; i++) { - accel_corr[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); - } + accel_avg[i] = accel_sum[i] / count; } + + return OK; } int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); if (det == 0.0) - return -1; // Singular matrix + return ERROR; // Singular matrix + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; @@ -361,34 +381,38 @@ int mat_invert3(float src[3][3], float dst[3][3]) { dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - return 0; + + return OK; } -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g) { - /* calculate raw offsets */ +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { + /* calculate offsets */ for (int i = 0; i < 3; i++) { - accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) - + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); + accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; } + /* fill matrix A for linear equations system*/ float mat_A[3][3]; memset(mat_A, 0, sizeof(mat_A)); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); + float a = accel_ref[i * 2][j] - accel_offs[j]; mat_A[i][j] = a; } } + /* calculate inverse matrix for A */ float mat_A_inv[3][3]; - mat_invert3(mat_A, mat_A_inv); + if (mat_invert3(mat_A, mat_A_inv) != OK) + return ERROR; + + /* copy results to accel_T */ for (int i = 0; i < 3; i++) { - /* copy results to accel_T */ for (int j = 0; j < 3; j++) { /* simplify matrices mult because b has only one non-zero element == g at index i */ accel_T[j][i] = mat_A_inv[j][i] * g; } } - return 0; + + return OK; } diff --git a/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h index acf45b6b6e..c0169c2a13 100644 --- a/apps/commander/accelerometer_calibration.h +++ b/apps/commander/accelerometer_calibration.h @@ -12,8 +12,5 @@ #include void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); -int detect_orientation(int mavlink_fd, int sub_sensor_combined); -int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g); #endif /* ACCELEROMETER_CALIBRATION_H_ */