From 1d5856b4d42e209480daee682df9c91ec0bbd580 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 21 Feb 2023 10:44:02 +0100
Subject: [PATCH] same for nadir single axis

---
 mission/controller/acs/Guidance.cpp | 30 ++++++++++++++++-------------
 mission/controller/acs/Guidance.h   |  3 ++-
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index 1213df7a..2346d18f 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -302,7 +302,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
   refSatRate[2] = 0;
 }
 
-void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refDirB[3],
+void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
+                                            double targetQuat[4], double refDirB[3],
                                             double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
@@ -311,22 +312,20 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], d
   VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
 
   // transformation between ECEF and ECI frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
   // transformation between ECEF and Body frame
-  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::toDcm(quatBJ, dcmBJ);
-  MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
+  QuaternionOperations::toDcm(quatBI, dcmBI);
+  MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
 
   // target Direction in the body frame
   double targetDirB[3] = {0, 0, 0};
@@ -357,6 +356,11 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], d
   refSatRate[0] = 0;
   refSatRate[1] = 0;
   refSatRate[2] = 0;
+
+  // revert calculated quaternion from qBX to qIX
+  double quatIB[4] = {0, 0, 0, 1};
+  QuaternionOperations::inverse(quatBI, quatIB);
+  QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
 }
 
 void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h
index b494c7a9..65c9aa12 100644
--- a/mission/controller/acs/Guidance.h
+++ b/mission/controller/acs/Guidance.h
@@ -30,7 +30,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 targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
+                                    double targetQuat[4], double refDirB[3], double refSatRate[3]);
   void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
                                    double targetQuat[4], double refSatRate[3]);