diff --git a/src/lib/conversion/CMakeLists.txt b/src/lib/conversion/CMakeLists.txt index 8fdc3b11551..d2b87413f11 100644 --- a/src/lib/conversion/CMakeLists.txt +++ b/src/lib/conversion/CMakeLists.txt @@ -32,3 +32,5 @@ ############################################################################ px4_add_library(conversion rotation.cpp) + +px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion) diff --git a/src/lib/conversion/RotationTest.cpp b/src/lib/conversion/RotationTest.cpp new file mode 100644 index 00000000000..ecff2a40b46 --- /dev/null +++ b/src/lib/conversion/RotationTest.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +/** + * @file RotationTest.cpp + * Tests for rotations. + */ + +#include + +#include "rotation.h" + + +TEST(Rotations, matrix_vs_3f) +{ + //iterate through all defined rotations + for (size_t i = 0; i < (size_t)Rotation::ROTATION_MAX; i++) { + + // GIVEN: an initial vector and a rotation + const matrix::Vector3f original = {1.f, 2.f, 3.f}; + const enum Rotation rotation = static_cast(i); + + // WHEN: we transform the vector using the rotate_3f function and the rotation matrix + matrix::Vector3f transformed_3f = original; + rotate_3f(rotation, transformed_3f(0), transformed_3f(1), transformed_3f(2)); + + matrix::Dcmf matrix = get_rot_matrix(rotation); + matrix::Vector3f transformed_mat = matrix * original; + + // THEN: the results should be the same + EXPECT_NEAR(transformed_mat(0), transformed_3f(0), 10e-6); + EXPECT_NEAR(transformed_mat(1), transformed_3f(1), 10e-6); + EXPECT_NEAR(transformed_mat(2), transformed_3f(2), 10e-6); + } +} diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index ae3bd011a4f..5e6081cccb5 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -238,14 +238,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) } case ROTATION_PITCH_90_YAW_180: { - tmp = x; x = z; z = tmp; + tmp = x; + x = -z; y = -y; - return; - } - - case ROTATION_PITCH_90_ROLL_90: { - tmp = x; x = y; - y = -z; z = -tmp; + z = -tmp; return; } @@ -253,15 +249,9 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) float tmpx = x; float tmpy = y; float tmpz = z; - x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; - y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; - z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; - return; - } - - case ROTATION_PITCH_90_ROLL_270: { - tmp = x; x = -y; - y = z; z = -tmp; + x = -0.390731f * tmpy - 0.920505f * tmpz; + y = 0.374607f * tmpx - 0.853477f * tmpy + 0.362280f * tmpz; + z = -0.927184f * tmpx - 0.344827f * tmpy + 0.146371f * tmpz; return; } @@ -304,5 +294,106 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) z = -tmp; return; } + + case ROTATION_PITCH_180_YAW_90: { + tmp = x; + x = -y; + y = -tmp; + z = -z; + return; + } + + case ROTATION_PITCH_180_YAW_270: { + tmp = x; + x = y; + y = tmp; + z = -z; + return; + } + + case ROTATION_ROLL_90_PITCH_90: { + tmp = x; + x = y; + y = -z; + z = -tmp; + return; + } + + case ROTATION_ROLL_180_PITCH_90: { + tmp = x; + x = -z; + y = -y; + z = -tmp; + return; + } + + case ROTATION_ROLL_270_PITCH_90: { + tmp = x; + x = -y; + y = z; + z = -tmp; + return; + } + + case ROTATION_ROLL_90_PITCH_180: { + tmp = y; + x = -x; + y = -z; + z = -tmp; + return; + } + + case ROTATION_ROLL_270_PITCH_180: { + tmp = y; + x = -x; + y = z; + z = tmp; + return; + } + + case ROTATION_ROLL_90_PITCH_270: { + tmp = x; + x = -y; + y = -z; + z = tmp; + return; + } + + case ROTATION_ROLL_270_PITCH_270: { + tmp = x; + x = y; + y = z; + z = tmp; + return; + } + + case ROTATION_ROLL_90_PITCH_180_YAW_90: { + tmp = y; + y = -x; + x = z; + z = -tmp; + return; + } + + case ROTATION_ROLL_90_PITCH_68_YAW_293: { + float tmpx = x; + float tmpy = y; + float tmpz = z; + x = 0.146371f * tmpx + 0.362280f * tmpy - 0.920505f * tmpz; + y = -0.344827f * tmpx - 0.853477f * tmpy - 0.390731f * tmpz; + z = -0.927184f * tmpx + 0.374607f * tmpy; + return; + } + + case ROTATION_ROLL_90_PITCH_315: { + float tmpx = x; + float tmpy = y; + float tmpz = z; + x = 0.707107f * tmpx - 0.707107f * tmpy; + y = -tmpz; + z = 0.707107f * tmpx + 0.707107f * tmpy; + return; + } + } }