rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Add helper function for converting an Euler rotation vector to a rotation matrix

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3150 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
jonathan 2011-04-10 18:33:30 +00:00 committed by jonathan
parent 55cba70bd7
commit c70e134cf8
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/Libraries/CoordinateConversions.c
View File

@ -334,6 +334,34 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
return 1;
}
void Rv2Rot(float Rv[3], float R[3][3])
{
// Compute rotation matrix from a rotation vector
// To save .text space, uses Quaternion2R()
float q[4];
float angle = VectorMagnitude(Rv);
if (angle <= 0.00048828125f) {
// angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f
q[0] = 1.0f;
// and flush sin(x/2)/x to 0.5
q[1] = 0.5f*Rv[0];
q[2] = 0.5f*Rv[1];
q[3] = 0.5f*Rv[2];
// This prevents division by zero, while retaining full accuracy
}
else {
q[0] = cosf(angle*0.5f);
float scale = sinf(angle*0.5f) / angle;
q[1] = scale*Rv[0];
q[2] = scale*Rv[1];
q[3] = scale*Rv[2];
}
Quaternion2R(q, R);
}
// ****** Vector Cross Product ********
void CrossProduct(const float v1[3], const float v2[3], float result[3])
{

3
flight/Libraries/inc/CoordinateConversions.h
View File

@ -38,6 +38,9 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3]);
void RneFromLLA(double LLA[3], float Rne[3][3]);
// ****** find rotation matrix from rotation vector
void Rv2Rot(float Rv[3], float R[3][3]);
// ****** find roll, pitch, yaw from quaternion ********
void Quaternion2RPY(const float q[4], float rpy[3]);