From 7c10f4b1cdd345733671a1ffee6b148637096f55 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 29 Apr 2024 10:24:53 +0200
Subject: [PATCH] fix calculation of target rotation

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

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index 2c1b3c63..1700d265 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -232,6 +232,7 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
   }
   if (timeDelta != 0.0) {
     QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
+    VectorOperations<double>::mulScalar(refSatRate, -1, refSatRate, 3);
   } else {
     std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
   }
@@ -315,9 +316,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
 
   // Calculate error satellite rotational rate
   // Convert target rotational rate into body RF
-  double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
-  QuaternionOperations::inverse(errorQuat, errorQuatInv);
-  QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
+  double targetSatRotRateB[3] = {0, 0, 0};
+  QuaternionOperations::multiplyVector(currentQuat, targetSatRotRate, targetSatRotRateB);
+  VectorOperations<double>::copy(targetSatRotRateB, targetSatRotRateB, 3);
   // Combine the target and reference satellite rotational rates
   double combinedRefSatRotRate[3] = {0, 0, 0};
   VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);