From 70e503cb919928493ee6acefaf22ce0a529e2cdb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 10 Jan 2021 18:08:41 -0500 Subject: [PATCH] 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 | 7 +- src/lib/conversion/rotation.cpp | 243 ++++++++++-------------------- src/lib/conversion/rotation.h | 14 +- 3 files changed, 90 insertions(+), 174 deletions(-) diff --git a/src/lib/conversion/CMakeLists.txt b/src/lib/conversion/CMakeLists.txt index d2b87413f1..a3876dad88 100644 --- a/src/lib/conversion/CMakeLists.txt +++ b/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) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 454cc280ae..a19842c915 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -37,8 +37,6 @@ * Vector rotation library */ -#include -#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; + float tmp = x; + x = -y; + y = tmp; } + break; - case ROTATION_YAW_135: { - tmp = -M_SQRT1_2_F * (x + y); - y = M_SQRT1_2_F * (x - y); - x = tmp; - return; - } - - 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; - } - - 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; - } - - 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; + } + break; + + default: + + // 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; } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 2a67ca584e..decb9f4b6a 100644 --- a/src/lib/conversion/rotation.h +++ b/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 +#include + #include #include +#include /** * 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 };