fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 22:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
- increase optimization to ${MAX_CUSTOM_OPT_LEVEL} (max per board)
This commit is contained in:
Daniel Agar
2021-01-10 18:08:41 -05:00
committed by GitHub
parent 06b733bb86
commit 70e503cb91
3 changed files with 90 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/conversion/rotation.cpp
+76 -167
View File
@@ -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;
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;
}
}