From 66fced90de5564735e6b71d8044abfe1263f81b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 9 Feb 2015 07:33:23 +0100 Subject: commander: Fix new-style accel calibration --- .../commander/accelerometer_calibration.cpp | 154 +++++++++++---------- 1 file changed, 78 insertions(+), 76 deletions(-) (limited to 'src') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 10fa3eaa3..0e69f8a4e 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -151,11 +151,11 @@ static const char *sensor_name = "accel"; static const unsigned max_sens = 3; -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens][3], float accel_T[max_sens][3][3]); -int detect_orientation(int mavlink_fd, int subs[max_sens]); -int read_accelerometer_avg(int subs[max_sens], float accel_avg[max_sens][6][3], unsigned orient, unsigned samples_num); +int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors); +int detect_orientation(int mavlink_fd, int (&subs)[max_sens]); +int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned 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); +int calculate_calibration_values(float (&accel_ref)[6][3], float (&accel_T)[3][3], float (&accel_offs)[3], float g); int do_accel_calibration(int mavlink_fd) { @@ -204,74 +204,78 @@ int do_accel_calibration(int mavlink_fd) float accel_offs[max_sens][3]; float accel_T[max_sens][3][3]; + unsigned active_sensors; if (res == OK) { /* measure and calculate offsets & scales */ - res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors); } - if (res == OK) { + if (res != OK || active_sensors == 0) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + return ERROR; + } - /* measurements completed successfully, rotate calibration values */ - param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - int32_t board_rotation_int; - param_get(board_rotation_h, &(board_rotation_int)); - enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix<3, 3> board_rotation; - get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); - - for (unsigned i = 0; i < max_sens; i++) { - - /* handle individual sensors, one by one */ - math::Vector<3> accel_offs_vec(&accel_offs[i][0]); - math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; - math::Matrix<3, 3> accel_T_mat(&accel_T[i][0][0]); - math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; - - accel_scale.x_offset = accel_offs_rotated(0); - accel_scale.x_scale = accel_T_rotated(0, 0); - accel_scale.y_offset = accel_offs_rotated(1); - accel_scale.y_scale = accel_T_rotated(1, 1); - accel_scale.z_offset = accel_offs_rotated(2); - accel_scale.z_scale = accel_T_rotated(2, 2); - - bool failed = false; - - /* set parameters */ - (void)sprintf(str, "CAL_ACC%u_XOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset))); - (void)sprintf(str, "CAL_ACC%u_YOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset))); - (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset))); - (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale))); - (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale))); - (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale))); - (void)sprintf(str, "CAL_ACC%u_ID", i); - failed |= (OK != param_set(param_find(str), &(device_id[i]))); - - if (failed) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - res = ERROR; - } + /* measurements completed successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; + math::Matrix<3, 3> board_rotation; + get_rot_matrix(board_rotation_id, &board_rotation); + math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); + + for (unsigned i = 0; i < active_sensors; i++) { + + /* handle individual sensors, one by one */ + math::Vector<3> accel_offs_vec(&accel_offs[i][0]); + math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; + math::Matrix<3, 3> accel_T_mat(&accel_T[i][0][0]); + math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); + + bool failed = false; + + /* set parameters */ + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset))); + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset))); + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset))); + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale))); + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale))); + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale))); + (void)sprintf(str, "CAL_ACC%u_ID", i); + failed |= (OK != param_set(param_find(str), &(device_id[i]))); + + if (failed) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + return ERROR; } - } - if (res == OK) { - /* apply new scaling and offsets */ - for (unsigned s = 0; s < max_sens; s++) { - sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - fd = open(str, 0); + sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); + fd = open(str, 0); + + if (fd < 0) { + mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist"); + res = ERROR; + } else { res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); + } - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); - } + if (res != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } @@ -280,23 +284,22 @@ int do_accel_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } - } - if (res == OK) { mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); } return res; } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens][3], float accel_T[max_sens][3][3]) +int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors) { - const unsigned samples_num = 2500; + const unsigned samples_num = 3000; + *active_sensors = 0; float accel_ref[max_sens][6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -306,8 +309,6 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens] uint64_t timestamps[max_sens]; - unsigned active_sensors = 0; - for (unsigned i = 0; i < max_sens; i++) { subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); /* store initial timestamp - used to infer which sensors are active */ @@ -353,7 +354,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens] /* allow user enough time to read the message */ sleep(3); - int orient = detect_orientation(mavlink_fd, &subs[0]); + int orient = detect_orientation(mavlink_fd, subs); if (orient < 0) { mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); @@ -386,18 +387,18 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens] struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { - active_sensors++; + (*active_sensors)++; } close(subs[i]); } if (res == OK) { /* calculate offsets and transform matrix */ - for (unsigned i = 0; i < active_sensors; i++) { + for (unsigned i = 0; i < (*active_sensors); i++) { res = calculate_calibration_values(accel_ref[i], accel_T[i], accel_offs[i], CONSTANTS_ONE_G); if (res != OK) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + mavlink_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; } } @@ -415,7 +416,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[max_sens] * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ -int detect_orientation(int mavlink_fd, int subs[max_sens]) +int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) { const unsigned ndim = 3; @@ -560,7 +561,7 @@ int detect_orientation(int mavlink_fd, int subs[max_sens]) /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int subs[max_sens], float accel_avg[max_sens][6][3], unsigned orient, unsigned samples_num) +int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num) { struct pollfd fds[max_sens]; @@ -610,6 +611,7 @@ int read_accelerometer_avg(int subs[max_sens], float accel_avg[max_sens][6][3], for (unsigned s = 0; s < max_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; + warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]); } } @@ -639,7 +641,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) return OK; } -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) +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++) { -- cgit v1.2.3