dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix accel calibration: rotate sensor values into board frame

This commit is contained in:
Roman Bapst 2015-05-05 15:44:12 +02:00
parent 577bdf3a0d
commit 0217e2ed56
1 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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];