From 6f3876d20420515598d4e64cd70fbf5f6efb923d Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 5 Feb 2024 09:24:41 +0100
Subject: [PATCH] fixed quaternion multiplication bug

---
 mission/controller/acs/Guidance.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index 89f3d958..e4e9ccab 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -192,10 +192,12 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                           double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
-  // First calculate error quaternion between current and target orientation
-  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
-  // Last calculate add rotation from reference quaternion
-  QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
+  // First calculate error quaternion between current and target orientation without reference
+  // quaternion
+  double errorQuatWoRef[4] = {0, 0, 0, 0};
+  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
+  // Then add rotation from reference quaternion
+  QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
   // Keep scalar part of quaternion positive
   if (errorQuat[3] < 0) {
     VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);