From ef6f1f6f808e49ff3aca68aa08001e37093b89ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 20 Oct 2013 19:36:42 +0200 Subject: get_rot_matrix() moved to separate library, accel calibration of rotated board fixed --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/lib/conversion/module.mk | 38 +++++++++ src/lib/conversion/rotation.cpp | 30 +++++++ src/lib/conversion/rotation.h | 89 +++++++++++++++++++++ .../commander/accelerometer_calibration.cpp | 45 ++++++++--- src/modules/commander/module.mk | 1 - src/modules/sensors/sensors.cpp | 93 +--------------------- 8 files changed, 192 insertions(+), 106 deletions(-) create mode 100644 src/lib/conversion/module.mk create mode 100644 src/lib/conversion/rotation.cpp create mode 100644 src/lib/conversion/rotation.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c5..862abde92 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f..71986c3a3 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -111,6 +111,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 000000000..102711aaf --- /dev/null +++ b/src/lib/conversion/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Rotation library +# + +SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 000000000..b65226cf1 --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -0,0 +1,30 @@ +/* + * rotation.cpp + * + * Created on: 20.10.2013 + * Author: ton + */ + +#include "math.h" +#include "rotation.h" + +__EXPORT void +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); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h new file mode 100644 index 000000000..97c6a0907 --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -0,0 +1,89 @@ +/* + * rotation.h + * + * Created on: 20.10.2013 + * Author: ton + */ + +#ifndef ROTATION_H_ +#define ROTATION_H_ + +#include +#include + +/** + * 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 } +}; + +/** + * Get the rotation matrix + */ +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + +#endif /* ROTATION_H_ */ 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 #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -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 #include #include +#include #include @@ -135,75 +136,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 * @@ -384,11 +316,6 @@ private: */ int parameters_update(); - /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - /** * Do accel-related initialisation. */ @@ -803,24 +730,6 @@ Sensors::parameters_update() return OK; } -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() { -- cgit v1.2.3