From 58a8c5869c694527188406ec1cc9a6f825145ef2 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 22 Feb 2024 16:59:26 +0100
Subject: [PATCH] some fixes and debug stuff

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

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index 079b627d..c70c37e3 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -102,9 +102,12 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
       acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
 
   // target direction in the ECI frame
-  double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
-  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
-  CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
+  double posSatI[3] = {2030378.44284257, -56784.9332372798, 6567201.63882364},
+         posGroundStationI[3] = {3693004.40032203, -2049075.29966484, 4764086.46806924},
+         groundStationDirI[3] = {0, 0, 0};
+  // CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
+  // CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI,
+  // &timeAbsolute);
   VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
 
   // negative x-axis aligned with target direction
@@ -119,32 +122,34 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
   VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
 
   // sun avoidance calculations
-  double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
+  double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
                                       sunPerpendicularX, 3);
   VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
   VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
+  VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
   double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
          strVecSunZ[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
                                       strVecSunX, 3);
-  VectorOperations<double>::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2],
+  VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
                                       strVecSunZ, 3);
   VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
   VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
   sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
 
   // earth avoidance calculations
-  double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0};
+  double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
                                       earthPerpendicularX, 3);
   VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
   VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
+  VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
   double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
          strVecEarthZ[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
                                       strVecEarthX, 3);
-  VectorOperations<double>::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2],
+  VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
                                       strVecEarthZ, 3);
   VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
   VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
@@ -162,9 +167,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
   earthWeight *= normFactor;
 
   // calculate z-axis for str blinding avoidance
-  double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3);
-  VectorOperations<double>::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3);
+  double zAxisIX[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
+  VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
   VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
   VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
   VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
@@ -249,6 +254,8 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
     return;
   }
 
+  sif::debug << "Reduced Quaternion Rotation Required" << std::endl;
+
   // x-axis always needs full rotation
   double phiX = 0, phiXvec[3] = {0, 0, 0};
   phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));