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

rotation from quaternions

Browse Source
This commit is contained in:
Marius Eggert 2024-01-26 11:06:05 +01:00
parent e64e8b274d
commit 5879eb7f7b
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
*/