From 81a4112c45b5a4d0dba4714ff82edde374ad28c3 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 20 Feb 2023 16:20:54 +0100
Subject: [PATCH] nadir should work again

---
 mission/controller/AcsController.cpp | 12 +++++++-----
 mission/controller/acs/Guidance.cpp  | 11 +++--------
 mission/controller/acs/Guidance.h    |  3 ++-
 3 files changed, 12 insertions(+), 14 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 5df63342..ff8be5ad 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -311,11 +311,13 @@ void AcsController::performPointingCtrl() {
       break;
 
     case acs::PTG_NADIR:
-      guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
-                                           targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
+      guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+                                           gpsDataProcessed.gpsVelocity.value, targetQuat,
+                                           targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
+                          acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index a937f950..4f30e636 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -364,8 +364,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
   refSatRate[2] = 0;
 }
 
-void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
-                                     double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+                                           double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
   //-------------------------------------------------------------------------------------
@@ -409,12 +409,7 @@ void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velS
   QuaternionOperations::fromDcm(dcmTgt, targetQuat);
 
   int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
-  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
-
-  // Transform in system relative to satellite frame
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
+  targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
 }
 
 void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) {
diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h
index 4dc8ec86..7306d4f6 100644
--- a/mission/controller/acs/Guidance.h
+++ b/mission/controller/acs/Guidance.h
@@ -29,7 +29,8 @@ class Guidance {
   // Function to get the target quaternion and refence rotation rate from gps position for Nadir
   // pointing
   void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]);
-  void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double targetSatRotRate[3]);
+  void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+                                   double targetQuat[4], double refSatRate[3]);
 
   // Function to get the target quaternion and refence rotation rate from parameters for inertial
   // pointing