eive/fsfw
eive/fsfw
15
1
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases 1 Wiki Activity

Rotation from Quaternions #167

Merged
meggert merged 1 commits from rotation-from-quaternions into develop 2024-01-29 14:11:10 +01:00
Conversation 0 Commits 1 Files Changed 2 +26
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/fsfw/globalfunctions/math/QuaternionOperations.cpp
View File

@ -153,3 +153,26 @@ double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
}
}
}
void QuaternionOperations::rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta,
double rotRate[3]) {
double qOldInv[4] = {0, 0, 0, 0};
double qDelta[4] = {0, 0, 0, 0};
inverse(qOld, qOldInv);
multiply(qNew, qOldInv, qDelta);
if (VectorOperations<double>::norm(qDelta, 4) != 0.0) {
normalize(qDelta);
}
if (VectorOperations<double>::norm(qDelta, 3) == 0.0) {
rotRate[0] = 0.0;
rotRate[1] = 0.0;
rotRate[2] = 0.0;
return;
}
double rotVec[3] = {0, 0, 0};
double angle = getAngle(qDelta);
VectorOperations<double>::normalize(qDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotRate, 3);
}

3
src/fsfw/globalfunctions/math/QuaternionOperations.h
View File

@ -25,6 +25,9 @@ class QuaternionOperations {
static void slerp(const double q1[4], const double q2[4], const double weight, double q[4]);
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta, double rotRate[3]);
/**
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
*/