dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

rotation: combine duplicates

This commit is contained in:
Daniel Agar 2020-08-25 16:00:32 -04:00
parent 08cfea0b6f
commit 08ad7850fc
1 changed files with 31 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
src/lib/conversion/rotation.cpp
View File

@ -123,8 +123,14 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_180_YAW_90: {
tmp = x; x = y; y = tmp; z = -z;
case ROTATION_ROLL_180_YAW_90:
// FALLTHROUGH
case ROTATION_PITCH_180_YAW_270: {
tmp = x;
x = y;
y = tmp;
z = -z;
return;
}
@ -147,8 +153,14 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_180_YAW_270: {
tmp = x; x = -y; y = -tmp; z = -z;
case ROTATION_ROLL_180_YAW_270:
// FALLTHROUGH
case ROTATION_PITCH_180_YAW_90: {
tmp = x;
x = -y;
y = -tmp;
z = -z;
return;
}
@ -217,14 +229,6 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_270_YAW_270: {
tmp = x;
x = z;
z = -y;
y = -tmp;
return;
}
case ROTATION_PITCH_90: {
tmp = z; z = -x; x = tmp;
return;
@ -241,14 +245,6 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_PITCH_90_YAW_180: {
tmp = x;
x = -z;
y = -y;
z = -tmp;
return;
}
case ROTATION_PITCH_9_YAW_180: {
const float tmpx = x;
const float tmpy = y;
@ -281,30 +277,6 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_270_YAW_180: {
x = -x;
tmp = y;
y = -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;
@ -313,7 +285,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_180_PITCH_90: {
case ROTATION_ROLL_180_PITCH_90:
// FALLTHROUGH
case ROTATION_PITCH_90_YAW_180: {
tmp = x;
x = -z;
y = -y;
@ -329,7 +304,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_90_PITCH_180: {
case ROTATION_ROLL_90_PITCH_180:
// FALLTHROUGH
case ROTATION_ROLL_270_YAW_180: {
tmp = y;
x = -x;
y = -z;
@ -361,11 +339,14 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
tmp = y;
y = -x;
case ROTATION_ROLL_90_PITCH_180_YAW_90:
// FALLTHROUGH
case ROTATION_ROLL_270_YAW_270: {
tmp = x;
x = z;
z = -tmp;
z = -y;
y = -tmp;
return;
}