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

Function to prevent Quaternion Sign Jump #171

Merged
meggert merged 1 commits from quat-no-sign-jump into develop 2024-03-18 10:59:20 +01:00
Conversation 0 Commits 1 Files Changed 2 +11
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -72,6 +72,15 @@ void QuaternionOperations::slerp(const double q1[4], const double q2[4], const d
normalize(q);
}
void QuaternionOperations::preventSignJump(double qNew[4], const double qOld[4]) {
double qDiff[4] = {0, 0, 0, 0}, qSum[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(qOld, qNew, qDiff, 4);
VectorOperations<double>::add(qOld, qNew, qSum, 4);
if (VectorOperations<double>::norm(qDiff, 4) > VectorOperations<double>::norm(qSum, 4)) {
VectorOperations<double>::mulScalar(qNew, -1, qNew, 4);
}
}
QuaternionOperations::QuaternionOperations() {}
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {

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

@ -28,6 +28,8 @@ class QuaternionOperations {
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta, double rotRate[3]);
static void preventSignJump(double qNew[4], const double qOld[4]);
/**
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
*/