fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 23:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib delete Matrix, Quaternion, Vector

Browse Source
This commit is contained in:
Daniel Agar
2018-03-28 17:05:42 -04:00
parent 0d7b5c4a4e
commit 222a91c6be
32 changed files with 160 additions and 1746 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/conversion/rotation.cpp
+28 -40
View File
@@ -40,16 +40,6 @@
#include "math.h"
#include "rotation.h"
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix)
{
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
rot_matrix->from_euler(roll, pitch, yaw);
}
__EXPORT matrix::Dcmf
get_rot_matrix(enum Rotation rot)
{
@@ -59,8 +49,6 @@ get_rot_matrix(enum Rotation rot)
math::radians((float)rot_lookup[rot].yaw)}};
}
#define HALF_SQRT_2 0.70710678118654757f
__EXPORT void
rotate_3f(enum Rotation rot, float &x, float &y, float &z)
{
@@ -72,8 +60,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
case ROTATION_YAW_45: {
tmp = HALF_SQRT_2 * (x - y);
y = HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@@ -84,8 +72,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_YAW_135: {
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
@@ -95,8 +83,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
case ROTATION_YAW_225: {
tmp = HALF_SQRT_2 * (y - x);
y = -HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (y - x);
y = -M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@@ -107,8 +95,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_YAW_315: {
tmp = HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (y - x);
tmp = M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (y - x);
x = tmp;
return;
}
@@ -119,8 +107,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_45: {
tmp = HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp; z = -z;
return;
}
@@ -131,8 +119,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_135: {
tmp = HALF_SQRT_2 * (y - x);
y = HALF_SQRT_2 * (y + x);
tmp = M_SQRT1_2_F * (y - x);
y = M_SQRT1_2_F * (y + x);
x = tmp; z = -z;
return;
}
@@ -143,8 +131,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_225: {
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (y - x);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (y - x);
x = tmp; z = -z;
return;
}
@@ -155,8 +143,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_315: {
tmp = HALF_SQRT_2 * (x - y);
y = -HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = -M_SQRT1_2_F * (x + y);
x = tmp; z = -z;
return;
}
@@ -168,8 +156,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_90_YAW_45: {
tmp = z; z = y; y = -tmp;
tmp = HALF_SQRT_2 * (x - y);
y = HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@@ -182,8 +170,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_90_YAW_135: {
tmp = z; z = y; y = -tmp;
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
@@ -195,8 +183,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_270_YAW_45: {
tmp = z; z = -y; y = tmp;
tmp = HALF_SQRT_2 * (x - y);
y = HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@@ -209,8 +197,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_270_YAW_135: {
tmp = z; z = -y; y = tmp;
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
@@ -276,15 +264,15 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_PITCH_45: {
tmp = HALF_SQRT_2 * x + HALF_SQRT_2 * z;
z = HALF_SQRT_2 * z - HALF_SQRT_2 * x;
tmp = M_SQRT1_2_F * x + M_SQRT1_2_F * z;
z = M_SQRT1_2_F * z - M_SQRT1_2_F * x;
x = tmp;
return;
}
case ROTATION_PITCH_315: {
tmp = HALF_SQRT_2 * x - HALF_SQRT_2 * z;
z = HALF_SQRT_2 * z + HALF_SQRT_2 * x;
tmp = M_SQRT1_2_F * x - M_SQRT1_2_F * z;
z = M_SQRT1_2_F * z + M_SQRT1_2_F * x;
x = tmp;
return;
}