add test for rotations, add new rotations, fix old rotations

src/lib/conversion/CMakeLists.txt

@@ -32,3 +32,5 @@
 ############################################################################
 
 px4_add_library(conversion rotation.cpp)
+
+px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion)

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 <gtest/gtest.h>
+
+#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<Rotation>(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);
+	}
+}

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;
+		}
+
 	}
 }