From 0217e2ed56a9b11d739dd290eb75b23403a655ac Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 5 May 2015 15:44:12 +0200 Subject: [PATCH] fix accel calibration: rotate sensor values into board frame --- .../commander/accelerometer_calibration.cpp | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f83640d28f..efd88b3d40 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -407,6 +407,31 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel */ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { + /* get total sensor board rotation matrix */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + param_t board_offset_x = param_find("SENS_BOARD_X_OFF"); + param_t board_offset_y = param_find("SENS_BOARD_Y_OFF"); + param_t board_offset_z = param_find("SENS_BOARD_Z_OFF"); + + float board_offset[3]; + param_get(board_offset_x, &board_offset[0]); + param_get(board_offset_y, &board_offset[1]); + param_get(board_offset_z, &board_offset[2]); + + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0], + M_DEG_TO_RAD_F * board_offset[1], + M_DEG_TO_RAD_F * board_offset[2]); + + 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); + + /* combine board rotation with offset rotation */ + board_rotation = board_rotation_offset * board_rotation; + struct pollfd fds[max_accel_sens]; for (unsigned i = 0; i < max_accel_sens; i++) { @@ -453,6 +478,13 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc } } + // rotate sensor measurements from body frame into sensor frame using board rotation matrix + for (unsigned i = 0; i < max_accel_sens; i++) { + math::Vector<3> accel_sum_vec(&accel_sum[i][0]); + accel_sum_vec = board_rotation * accel_sum_vec; + memcpy(&accel_sum[i][0], &accel_sum_vec.data[0], sizeof(accel_sum[i])); + } + for (unsigned s = 0; s < max_accel_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];