Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

cleanup unneccessary complicated formulations

Browse Source
This commit is contained in:
baumanta
2020-04-29 09:38:35 +02:00
committed by Daniel Agar
parent c758da2391
commit 38a6304d53
1 changed files with 20 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/conversion/rotation.cpp
+20 -16
View File
@@ -202,8 +202,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_270_YAW_90: {
tmp = z; z = -y; y = tmp;
tmp = x; x = -y; y = tmp;
tmp = x;
x = -z;
z = -y;
y = tmp;
return;
}
@@ -216,8 +218,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_270_YAW_270: {
tmp = z; z = -y; y = tmp;
tmp = x; x = y; y = -tmp;
tmp = x;
x = z;
z = -y;
y = -tmp;
return;
}
@@ -246,9 +250,9 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_YAW_293_PITCH_68_ROLL_90: {
float tmpx = x;
float tmpy = y;
float tmpz = z;
const float tmpx = x;
const float tmpy = y;
const float tmpz = z;
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;
@@ -256,9 +260,9 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_PITCH_9_YAW_180: {
float tmpx = x;
float tmpy = y;
float tmpz = z;
const float tmpx = x;
const float tmpy = y;
const float tmpz = z;
x = -0.987688f * tmpx + 0.000000f * tmpy + -0.156434f * tmpz;
y = 0.000000f * tmpx + -1.000000f * tmpy + 0.000000f * tmpz;
z = -0.156434f * tmpx + 0.000000f * tmpy + 0.987688f * tmpz;
@@ -376,9 +380,9 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_90_PITCH_68_YAW_293: {
float tmpx = x;
float tmpy = y;
float tmpz = z;
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;
@@ -386,9 +390,9 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_90_PITCH_315: {
float tmpx = x;
float tmpy = y;
float tmpz = z;
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;