rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Added conversion from rotation matrix to quaternions.

Browse Source
This commit is contained in:
Laura Sebesta 2012-10-08 15:17:47 +02:00
parent db8563f69f
commit 993cd0f8f6
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -257,5 +257,26 @@ void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3])
Rbe[2][2] = q0s - q1s - q2s + q3s;
}
//** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame **
void CoordinateConversions::R2Quaternion(float const Rbe[3][3], float q[4])
{
qreal w, x, y, z;
// w always >= 0
w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0;
x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0;
y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0;
z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0;
x = copysign(x, (Rbe[1][2] - Rbe[2][1]));
y = copysign(y, (Rbe[2][0] - Rbe[0][2]));
z = copysign(z, (Rbe[0][1] - Rbe[1][0]));
q[0]=w;
q[1]=x;
q[2]=y;
q[3]=z;
}
}

1
ground/openpilotgcs/src/libs/utils/coordinateconversions.h
View File

@ -50,6 +50,7 @@ public:
void Quaternion2RPY(const float q[4], float rpy[3]);
void RPY2Quaternion(const float rpy[3], float q[4]);
void Quaternion2R(const float q[4], float Rbe[3][3]);
void R2Quaternion(float const Rbe[3][3], float q[4]);
};
}