fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 06:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

accelerometer calibration fix

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-20 23:16:23 +02:00
parent ef6f1f6f80
commit b75c8e672f
1 changed files with 17 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+17 -28
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;
} }