diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-09 13:56:39 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-09 22:56:25 +0100 |
commit | 22d80a80f479b589eb8ca188d4e789a907ffc719 (patch) | |
tree | 218bcff2b40b86125809ce2a6bb0d6411d278f6b /src | |
parent | 66fced90de5564735e6b71d8044abfe1263f81b1 (diff) | |
download | px4-firmware-22d80a80f479b589eb8ca188d4e789a907ffc719.tar.gz px4-firmware-22d80a80f479b589eb8ca188d4e789a907ffc719.tar.bz2 px4-firmware-22d80a80f479b589eb8ca188d4e789a907ffc719.zip |
Accel calibration: Better output
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 97 |
1 files changed, 59 insertions, 38 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 0e69f8a4e..8e4908d23 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -155,18 +155,18 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se 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(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g); int do_accel_calibration(int mavlink_fd) { int fd; int32_t device_id[max_sens]; - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides"); sleep(3); - mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen"); sleep(5); struct accel_scale accel_scale = { @@ -198,7 +198,7 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); } } @@ -212,7 +212,7 @@ int do_accel_calibration(int mavlink_fd) } if (res != OK || active_sensors == 0) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } @@ -228,10 +228,10 @@ int do_accel_calibration(int mavlink_fd) 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; + math::Vector<3> accel_offs_vec(accel_offs[i]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3, 3> accel_T_mat(accel_T[i]); + 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); @@ -259,7 +259,7 @@ int do_accel_calibration(int mavlink_fd) failed |= (OK != param_set(param_find(str), &(device_id[i]))); if (failed) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); return ERROR; } @@ -287,7 +287,7 @@ int do_accel_calibration(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); } else { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); @@ -335,7 +335,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se } if (old_done_count != done_count) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); + mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); } if (done) { @@ -343,7 +343,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se } /* inform user which axes are still needed */ - mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + mavlink_and_console_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", (!data_collected[5]) ? "down " : "", (!data_collected[0]) ? "back " : "", (!data_collected[1]) ? "front " : "", @@ -357,22 +357,22 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se int orient = detect_orientation(mavlink_fd, subs); if (orient < 0) { - mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "invalid motion, hold still..."); sleep(3); continue; } /* inform user about already handled side */ if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); - sleep(4); + mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(3); continue; } - mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + mavlink_and_console_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); sleep(1); read_accelerometer_avg(subs, accel_ref, orient, samples_num); - mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], (double)accel_ref[0][orient][2]); @@ -395,10 +395,17 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se if (res == OK) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { - res = calculate_calibration_values(accel_ref[i], accel_T[i], accel_offs[i], CONSTANTS_ONE_G); + res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + + /* verbose output on the console */ + printf("accel_T transformation matrix:\n"); + for (unsigned j = 0; j < 3; j++) { + printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); + } + printf("\n"); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; } } @@ -459,7 +466,21 @@ int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) float w = dt / ema_len; for (unsigned i = 0; i < ndim; i++) { - float d = ((float*)&sensor.x)[i] - accel_ema[i]; + + float di = 0.0f; + switch (i) { + case 0: + di = sensor.x; + break; + case 1: + di = sensor.y; + break; + case 2: + di = sensor.z; + break; + } + + float d = di - accel_ema[i]; accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); @@ -480,7 +501,7 @@ int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -497,7 +518,7 @@ int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); sleep(3); t_still = 0; } @@ -512,7 +533,7 @@ int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) } if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); return ERROR; } } @@ -553,7 +574,7 @@ int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) return 5; // [ 0, 0, -g ] } - mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); return ERROR; // Can't detect orientation } @@ -590,9 +611,9 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6 struct accel_report arp; orb_copy(ORB_ID(sensor_accel), subs[s], &arp); - for (int i = 0; i < 3; i++) { - accel_sum[s][i] += ((float*)&arp.x)[i]; - } + accel_sum[s][0] += arp.x; + accel_sum[s][1] += arp.y; + accel_sum[s][2] += arp.z; counts[s]++; } @@ -641,20 +662,20 @@ 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(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g) { /* calculate offsets */ - for (int i = 0; i < 3; i++) { - accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; + for (unsigned i = 0; i < 3; i++) { + accel_offs[sensor][i] = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][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_ref[i * 2][j] - accel_offs[j]; + for (unsigned i = 0; i < 3; i++) { + for (unsigned j = 0; j < 3; j++) { + float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j]; mat_A[i][j] = a; } } @@ -667,10 +688,10 @@ int calculate_calibration_values(float (&accel_ref)[6][3], float (&accel_T)[3][3 } /* copy results to accel_T */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { + for (unsigned i = 0; i < 3; i++) { + for (unsigned 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; + accel_T[sensor][j][i] = mat_A_inv[j][i] * g; } } |