rotation: use Dcmf for all rotations that aren't direct swaps

- increase optimization to ${MAX_CUSTOM_OPT_LEVEL} (max per board)

src/lib/conversion/CMakeLists.txt

@@ -31,6 +31,11 @@
 #
 ############################################################################
 
-px4_add_library(conversion rotation.cpp)
+px4_add_library(conversion
+	rotation.cpp
+	rotation.h
+)
+
+target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 
 px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion)

src/lib/conversion/rotation.cpp

@@ -37,8 +37,6 @@
  * Vector rotation library
  */
 
-#include <px4_platform_common/defines.h>
-#include "math.h"
 #include "rotation.h"
 
 __EXPORT matrix::Dcmf
@@ -62,287 +60,198 @@ get_rot_quaternion(enum Rotation rot)
 __EXPORT void
 rotate_3f(enum Rotation rot, float &x, float &y, float &z)
 {
-	float tmp;
-
 	switch (rot) {
 	case ROTATION_NONE:
-	case ROTATION_MAX:
-		return;
-
-	case ROTATION_YAW_45: {
-			tmp = M_SQRT1_2_F * (x - y);
-			y   = M_SQRT1_2_F * (x + y);
-			x = tmp;
-			return;
-		}
+		break;
 
 	case ROTATION_YAW_90: {
-			tmp = x; x = -y; y = tmp;
-			return;
-		}
-
-	case ROTATION_YAW_135: {
-			tmp = -M_SQRT1_2_F * (x + y);
-			y   =  M_SQRT1_2_F * (x - y);
-			x = tmp;
-			return;
+			float tmp = x;
+			x = -y;
+			y = tmp;
 		}
+		break;
 
-	case ROTATION_YAW_180:
-		x = -x; y = -y;
-		return;
-
-	case ROTATION_YAW_225: {
-			tmp = M_SQRT1_2_F * (y - x);
-			y   = -M_SQRT1_2_F * (x + y);
-			x = tmp;
-			return;
+	case ROTATION_YAW_180: {
+			x = -x;
+			y = -y;
 		}
+		break;
 
 	case ROTATION_YAW_270: {
-			tmp = x; x = y; y = -tmp;
-			return;
-		}
-
-	case ROTATION_YAW_315: {
-			tmp = M_SQRT1_2_F * (x + y);
-			y   = M_SQRT1_2_F * (y - x);
-			x = tmp;
-			return;
+			float tmp = x;
+			x = y;
+			y = -tmp;
 		}
+		break;
 
 	case ROTATION_ROLL_180: {
-			y = -y; z = -z;
-			return;
-		}
-
-	case ROTATION_ROLL_180_YAW_45: {
-			tmp = M_SQRT1_2_F * (x + y);
-			y   = M_SQRT1_2_F * (x - y);
-			x = tmp; z = -z;
-			return;
+			y = -y;
+			z = -z;
 		}
+		break;
 
 	case ROTATION_ROLL_180_YAW_90:
 
 	// FALLTHROUGH
 	case ROTATION_PITCH_180_YAW_270: {
-			tmp = x;
+			float tmp = x;
 			x = y;
 			y = tmp;
 			z = -z;
-			return;
-		}
-
-	case ROTATION_ROLL_180_YAW_135: {
-			tmp = M_SQRT1_2_F * (y - x);
-			y   = M_SQRT1_2_F * (y + x);
-			x = tmp; z = -z;
-			return;
 		}
+		break;
 
 	case ROTATION_PITCH_180: {
-			x = -x; z = -z;
-			return;
-		}
-
-	case ROTATION_ROLL_180_YAW_225: {
-			tmp = -M_SQRT1_2_F * (x + y);
-			y   =  M_SQRT1_2_F * (y - x);
-			x = tmp; z = -z;
-			return;
+			x = -x;
+			z = -z;
 		}
+		break;
 
 	case ROTATION_ROLL_180_YAW_270:
 
 	// FALLTHROUGH
 	case ROTATION_PITCH_180_YAW_90: {
-			tmp = x;
+			float tmp = x;
 			x = -y;
 			y = -tmp;
 			z = -z;
-			return;
-		}
-
-	case ROTATION_ROLL_180_YAW_315: {
-			tmp =  M_SQRT1_2_F * (x - y);
-			y   = -M_SQRT1_2_F * (x + y);
-			x = tmp; z = -z;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_90: {
-			tmp = z; z = y; y = -tmp;
-			return;
-		}
-
-	case ROTATION_ROLL_90_YAW_45: {
-			tmp = z; z = y; y = -tmp;
-			tmp = M_SQRT1_2_F * (x - y);
-			y   = M_SQRT1_2_F * (x + y);
-			x = tmp;
-			return;
+			float tmp = z;
+			z = y;
+			y = -tmp;
 		}
+		break;
 
 	case ROTATION_ROLL_90_YAW_90: {
-			tmp = x;
+			float tmp = x;
 			x = z;
 			z = y;
 			y = tmp;
-			return;
-		}
-
-	case ROTATION_ROLL_90_YAW_135: {
-			tmp = z; z = y; y = -tmp;
-			tmp = -M_SQRT1_2_F * (x + y);
-			y   =  M_SQRT1_2_F * (x - y);
-			x = tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_270: {
-			tmp = z; z = -y; y = tmp;
-			return;
-		}
-
-	case ROTATION_ROLL_270_YAW_45: {
-			tmp = z; z = -y; y = tmp;
-			tmp = M_SQRT1_2_F * (x - y);
-			y   = M_SQRT1_2_F * (x + y);
-			x = tmp;
-			return;
+			float tmp = z;
+			z = -y;
+			y = tmp;
 		}
+		break;
 
 	case ROTATION_ROLL_270_YAW_90: {
-			tmp = x;
+			float tmp = x;
 			x = -z;
 			z = -y;
 			y = tmp;
-			return;
-		}
-
-	case ROTATION_ROLL_270_YAW_135: {
-			tmp = z; z = -y; y = tmp;
-			tmp = -M_SQRT1_2_F * (x + y);
-			y   =  M_SQRT1_2_F * (x - y);
-			x = tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_PITCH_90: {
-			tmp = z; z = -x; x = tmp;
-			return;
+			float tmp = z;
+			z = -x;
+			x = tmp;
 		}
+		break;
 
 	case ROTATION_PITCH_270: {
-			tmp = z; z = x; x = -tmp;
-			return;
+			float tmp = z;
+			z = x;
+			x = -tmp;
 		}
+		break;
 
 	case ROTATION_ROLL_180_PITCH_270: {
-			tmp = z; z = x; x = tmp;
-			y = -y;
-			return;
-		}
-
-	case ROTATION_PITCH_315: {
-			tmp = M_SQRT1_2_F * x - M_SQRT1_2_F * z;
-			z = M_SQRT1_2_F * z + M_SQRT1_2_F * x;
+			float tmp = z;
+			z = x;
 			x = tmp;
-			return;
+			y = -y;
 		}
+		break;
 
 	case ROTATION_ROLL_90_YAW_270: {
-			tmp = x;
+			float tmp = x;
 			x = -z;
 			z = y;
 			y = -tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_90_PITCH_90: {
-			tmp = x;
+			float tmp = x;
 			x = y;
 			y = -z;
 			z = -tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_180_PITCH_90: {
-			tmp = x;
+			float tmp = x;
 			x = -z;
 			y = -y;
 			z = -tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_270_PITCH_90: {
-			tmp = x;
+			float tmp = x;
 			x = -y;
 			y = z;
 			z = -tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_90_PITCH_180: {
-			tmp = y;
+			float tmp = y;
 			x = -x;
 			y = -z;
 			z = -tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_270_PITCH_180: {
-			tmp = y;
+			float tmp = y;
 			x = -x;
 			y = z;
 			z = tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_90_PITCH_270: {
-			tmp = x;
+			float tmp = x;
 			x = -y;
 			y = -z;
 			z = tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_270_PITCH_270: {
-			tmp = x;
+			float tmp = x;
 			x = y;
 			y = z;
 			z = tmp;
-			return;
 		}
+		break;
 
 	case ROTATION_ROLL_90_PITCH_180_YAW_90: {
-			tmp = x;
+			float tmp = x;
 			x = z;
 			z = -y;
 			y = -tmp;
-			return;
 		}
+		break;
 
-	case ROTATION_ROLL_90_PITCH_68_YAW_293: {
-			const float tmpx = x;
-			const float tmpy = y;
-			const 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;
-		}
+	default:
 
-	case ROTATION_ROLL_90_PITCH_315: {
-			const float tmpx = x;
-			const float tmpy = y;
-			const float tmpz = z;
-			x = 0.707107f * tmpx - 0.707107f * tmpy;
-			y = -tmpz;
-			z = 0.707107f * tmpx + 0.707107f * tmpy;
-			return;
+		// otherwise use full rotation matrix for valid rotations
+		if (rot < ROTATION_MAX) {
+			const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
+			x = r(0);
+			y = r(1);
+			z = r(2);
 		}
 
+		break;
 	}
 }

src/lib/conversion/rotation.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-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
@@ -40,15 +40,17 @@
 #ifndef ROTATION_H_
 #define ROTATION_H_
 
-#include <unistd.h>
+#include <stdint.h>
+
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
+#include <px4_platform_common/defines.h>
 
 /**
  * Enum for board and external compass rotations.
  * This enum maps from board attitude to airframe attitude.
  */
-enum Rotation {
+enum Rotation : uint8_t {
 	ROTATION_NONE                = 0,
 	ROTATION_YAW_45              = 1,
 	ROTATION_YAW_90              = 2,
@@ -86,10 +88,10 @@ enum Rotation {
 	ROTATION_ROLL_180_PITCH_270  = 34,
 	ROTATION_ROLL_270_PITCH_270  = 35,
 	ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
-	ROTATION_ROLL_90_YAW_270     = 37,
+	ROTATION_ROLL_90_YAW_270          = 37,
 	ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
-	ROTATION_PITCH_315           = 39,
-	ROTATION_ROLL_90_PITCH_315   = 40,
+	ROTATION_PITCH_315                = 39,
+	ROTATION_ROLL_90_PITCH_315        = 40,
 
 	ROTATION_MAX
 };