diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-10-20 19:36:42 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-10-20 19:36:42 +0200 |
commit | ef6f1f6f808e49ff3aca68aa08001e37093b89ec (patch) | |
tree | d4d0c9c432eddd98961fcf6f560e6798a3ab8c8c /src/modules | |
parent | 1a3845c66ab8d457a0227ddd4bd22f5053c7f6b6 (diff) | |
download | px4-firmware-ef6f1f6f808e49ff3aca68aa08001e37093b89ec.tar.gz px4-firmware-ef6f1f6f808e49ff3aca68aa08001e37093b89ec.tar.bz2 px4-firmware-ef6f1f6f808e49ff3aca68aa08001e37093b89ec.zip |
get_rot_matrix() moved to separate library, accel calibration of rotated board fixed
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 45 | ||||
-rw-r--r-- | src/modules/commander/module.mk | 1 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 93 |
3 files changed, 33 insertions, 106 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfa7d9e8a..c9bfedbda 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -112,11 +112,13 @@ #include <fcntl.h> #include <sys/prctl.h> #include <math.h> +#include <mathlib/mathlib.h> #include <string.h> #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> #include <geo/geo.h> +#include <conversion/rotation.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <mavlink/mavlink_log.h> @@ -144,24 +146,41 @@ int do_accel_calibration(int mavlink_fd) { int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + /* measurements complete successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + enum Rotation board_rotation_id; + param_get(board_rotation_h, &(board_rotation_id)); + math::Matrix board_rotation(3, 3); + get_rot_matrix(board_rotation_id, &board_rotation); + board_rotation = board_rotation.transpose(); + math::Vector3 vect(3); + vect(0) = accel_offs[0]; + vect(1) = accel_offs[1]; + vect(2) = accel_offs[2]; + math::Vector3 accel_offs_rotated = board_rotation * vect; + vect(0) = accel_scale[0]; + vect(1) = accel_scale[1]; + vect(2) = accel_scale[2]; + math::Vector3 accel_scale_rotated = board_rotation * vect; + + /* set parameters */ + 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_ZOFF"), &(accel_offs_rotated(2))) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0))) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1))) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) { mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], + accel_offs_rotated(0), + accel_scale_rotated(0), + accel_offs_rotated(1), + accel_scale_rotated(1), + accel_offs_rotated(2), + accel_scale_rotated(2), }; if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 91d75121e..554dfcb08 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,4 +47,3 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp - diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fcacaf8f..1b79de8fd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -67,6 +67,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> +#include <conversion/rotation.h> #include <systemlib/airspeed.h> @@ -136,75 +137,6 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** - * Enum for board and external compass rotations. - * This enum maps from board attitude to airframe attitude. - */ -enum Rotation { - ROTATION_NONE = 0, - ROTATION_YAW_45 = 1, - ROTATION_YAW_90 = 2, - ROTATION_YAW_135 = 3, - ROTATION_YAW_180 = 4, - ROTATION_YAW_225 = 5, - ROTATION_YAW_270 = 6, - ROTATION_YAW_315 = 7, - ROTATION_ROLL_180 = 8, - ROTATION_ROLL_180_YAW_45 = 9, - ROTATION_ROLL_180_YAW_90 = 10, - ROTATION_ROLL_180_YAW_135 = 11, - ROTATION_PITCH_180 = 12, - ROTATION_ROLL_180_YAW_225 = 13, - ROTATION_ROLL_180_YAW_270 = 14, - ROTATION_ROLL_180_YAW_315 = 15, - ROTATION_ROLL_90 = 16, - ROTATION_ROLL_90_YAW_45 = 17, - ROTATION_ROLL_90_YAW_90 = 18, - ROTATION_ROLL_90_YAW_135 = 19, - ROTATION_ROLL_270 = 20, - ROTATION_ROLL_270_YAW_45 = 21, - ROTATION_ROLL_270_YAW_90 = 22, - ROTATION_ROLL_270_YAW_135 = 23, - ROTATION_PITCH_90 = 24, - ROTATION_PITCH_270 = 25, - ROTATION_MAX -}; - -typedef struct { - uint16_t roll; - uint16_t pitch; - uint16_t yaw; -} rot_lookup_t; - -const rot_lookup_t rot_lookup[] = { - { 0, 0, 0 }, - { 0, 0, 45 }, - { 0, 0, 90 }, - { 0, 0, 135 }, - { 0, 0, 180 }, - { 0, 0, 225 }, - { 0, 0, 270 }, - { 0, 0, 315 }, - {180, 0, 0 }, - {180, 0, 45 }, - {180, 0, 90 }, - {180, 0, 135 }, - { 0, 180, 0 }, - {180, 0, 225 }, - {180, 0, 270 }, - {180, 0, 315 }, - { 90, 0, 0 }, - { 90, 0, 45 }, - { 90, 0, 90 }, - { 90, 0, 135 }, - {270, 0, 0 }, - {270, 0, 45 }, - {270, 0, 90 }, - {270, 0, 135 }, - { 0, 90, 0 }, - { 0, 270, 0 } -}; - -/** * Sensor app start / stop handling function * * @ingroup apps @@ -385,11 +317,6 @@ private: int parameters_update(); /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - - /** * Do accel-related initialisation. */ void accel_init(); @@ -804,24 +731,6 @@ Sensors::parameters_update() } void -Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) -{ - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; - float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; - float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - (*rot_matrix)(i, j) = R(i, j); -} - -void Sensors::accel_init() { int fd; |