From 25867e76b196fa4cddf3a00773df34323e6175e9 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 11 Jan 2023 16:00:30 +0100
Subject: [PATCH] fixed solar panels -> sun vector direction

---
 mission/controller/acs/Guidance.cpp | 24 +++++++++++++++---------
 1 file changed, 15 insertions(+), 9 deletions(-)

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index cd3a7b95..14ea5412 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -337,21 +337,25 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
   VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
   VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
 
-  // get Sun Vector Model in ECI as helper vector (element of x-z plane)
+  // get Sun Vector Model in ECI
   double sunJ[3];
   std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
   VectorOperations<double>::normalize(sunJ, sunJ, 3);
 
+  // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
+  // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
+  double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
+  xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
+  double sunParallel[3], zAxis[3];
+  VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
+  VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
+  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
   // calculate y-axis
   double yAxis[3];
-  VectorOperations<double>::cross(sunJ, xAxis, yAxis);
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
   VectorOperations<double>::normalize(yAxis, yAxis, 3);
 
-  // calculate z-axis
-  double zAxis[3];
-  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
-  VectorOperations<double>::normalize(zAxis, zAxis, 3);
-
   // Complete transformation matrix
   double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                          {xAxis[1], yAxis[1], zAxis[1]},
@@ -383,8 +387,10 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
   if (susDataProcessed->sunIjkModel.isValid()) {
     std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
     MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
-  } else {
+  } else if (susDataProcessed->susVecTot.isValid()) {
     std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
+  } else {
+    return;
   }
 
   //	Transformation between ECEF and IJK frame
@@ -416,7 +422,7 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
   double velocityB[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1);
 
-  // Normale to velocity and sunDir
+  // Normal to velocity and sunDir
   double crossVelSun[3] = {0, 0, 0};
   VectorOperations<double>::cross(velocityB, sunDirB, crossVelSun);