From 27c8a97d45ca631fade540daf30b3645a36a003e Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 13 Mar 2024 16:59:14 +0100
Subject: [PATCH] added function to prevent quaternion sign jump

---
 src/fsfw/globalfunctions/math/QuaternionOperations.cpp | 9 +++++++++
 src/fsfw/globalfunctions/math/QuaternionOperations.h   | 2 ++
 2 files changed, 11 insertions(+)

diff --git a/src/fsfw/globalfunctions/math/QuaternionOperations.cpp b/src/fsfw/globalfunctions/math/QuaternionOperations.cpp
index 780a629f..1e5283fe 100644
--- a/src/fsfw/globalfunctions/math/QuaternionOperations.cpp
+++ b/src/fsfw/globalfunctions/math/QuaternionOperations.cpp
@@ -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) {
diff --git a/src/fsfw/globalfunctions/math/QuaternionOperations.h b/src/fsfw/globalfunctions/math/QuaternionOperations.h
index 2ad842be..d6f1c38f 100644
--- a/src/fsfw/globalfunctions/math/QuaternionOperations.h
+++ b/src/fsfw/globalfunctions/math/QuaternionOperations.h
@@ -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
    */