zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

accelerometer calibration fix

sbg
Anton Babushkin 11 years ago
parent
ef6f1f6f80
commit
b75c8e672f
1 changed files with 17 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 45
      src/modules/commander/accelerometer_calibration.cpp

45
src/modules/commander/accelerometer_calibration.cpp
Unescape View File

@ -129,7 +129,7 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
int detect_orientation(int mavlink_fd, int sub_sensor_combined); 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 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 mat_invert3(float src[3][3], float dst[3][3]);
@ -142,8 +142,8 @@ int do_accel_calibration(int mavlink_fd) {
/* measure and calculate offsets & scales */ /* measure and calculate offsets & scales */
float accel_offs[3]; float accel_offs[3];
float accel_scale[3]; float accel_T[3][3];
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
if (res == OK) { if (res == OK) {
/* measurements complete successfully, rotate calibration values */ /* measurements complete successfully, rotate calibration values */
@ -153,34 +153,31 @@ int do_accel_calibration(int mavlink_fd) {
math::Matrix board_rotation(3, 3); math::Matrix board_rotation(3, 3);
get_rot_matrix(board_rotation_id, &board_rotation); get_rot_matrix(board_rotation_id, &board_rotation);
board_rotation = board_rotation.transpose(); board_rotation = board_rotation.transpose();
math::Vector3 vect(3); math::Vector3 accel_offs_vec;
vect(0) = accel_offs[0]; accel_offs_vec.set(&accel_offs[0]);
vect(1) = accel_offs[1]; math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec;
vect(2) = accel_offs[2]; math::Matrix accel_T_mat(3, 3);
math::Vector3 accel_offs_rotated = board_rotation * vect; accel_T_mat.set(&accel_T[0][0]);
vect(0) = accel_scale[0]; math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation;
vect(1) = accel_scale[1];
vect(2) = accel_scale[2];
math::Vector3 accel_scale_rotated = board_rotation * vect;
/* set parameters */ /* set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0)))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0)))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1)))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
} }
int fd = open(ACCEL_DEVICE_PATH, 0); int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = { struct accel_scale ascale = {
accel_offs_rotated(0), accel_offs_rotated(0),
accel_scale_rotated(0), accel_T_rotated(0, 0),
accel_offs_rotated(1), accel_offs_rotated(1),
accel_scale_rotated(1), accel_T_rotated(1, 1),
accel_offs_rotated(2), accel_offs_rotated(2),
accel_scale_rotated(2), accel_T_rotated(2, 2),
}; };
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
@ -206,7 +203,7 @@ int do_accel_calibration(int mavlink_fd) {
/* exit accel calibration mode */ /* exit accel calibration mode */
} }
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) {
const int samples_num = 2500; const int samples_num = 2500;
float accel_ref[6][3]; float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false }; bool data_collected[6] = { false, false, false, false, false, false };
@ -282,21 +279,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
} }
close(sensor_combined_sub); close(sensor_combined_sub);
/* calculate offsets and rotation+scale matrix */ /* calculate offsets and transform matrix */
float accel_T[3][3];
int res = calculate_calibration_values(accel_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) { if (res != 0) {
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
return ERROR; return ERROR;
} }
/* convert accel transform matrix to scales,
* rotation part of transform matrix is not used by now
*/
for (int i = 0; i < 3; i++) {
accel_scale[i] = accel_T[i][i];
}
return OK; return OK;
} }

Write Preview
Loading…
Cancel
Save