From 7275454f8a10b760b10fa9b105433eed90949bfa Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 20 Feb 2023 16:59:05 +0100
Subject: [PATCH] actually calculate the vector from position to target

---
 mission/controller/acs/Guidance.cpp | 49 +++++++++++------------------
 1 file changed, 19 insertions(+), 30 deletions(-)

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index 1c5dca92..fb64f4a4 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -21,18 +21,13 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
   //-------------------------------------------------------------------------------------
-  //  transform longitude, latitude and altitude to cartesian coordiantes (earth
-  //  fixed/centered frame)
-  double targetCart[3] = {0, 0, 0};
+  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
+  double targetE[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
       acsParameters.targetModeControllerParameters.latitudeTgt,
       acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
-
-  //  target direction in the ECEF frame
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
 
   //  transformation between ECEF and ECI frame
   double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -44,17 +39,11 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
   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 dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  // double quatBJ[4] = {0, 0, 0, 0};
-
-  // QuaternionOperations::toDcm(quatBJ, dcmBJ);
-  // MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
-
-  //  target Direction in the body frame
-  double targetDirB[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
+  //  target direction in the ECI frame
+  double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
+  VectorOperations<double>::subtract(targetI, posSatI, &targetDirI, 3);
 
   //	rotation quaternion from two vectors
   double refDirB[3] = {0, 0, 0};
@@ -167,16 +156,16 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
-  // target direction and position vector in the inertial frame
-  double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
+  // target direction in the ECI frame
+  double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
+  VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
 
-  // negative x-axis aligned with target
+  // x-axis aligned with target direction
   // this aligns with the camera, E- and S-band antennas
   double xAxis[3] = {0, 0, 0};
   VectorOperations<double>::normalize(targetDirI, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
 
   // transform velocity into inertial frame
   double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
@@ -234,16 +223,16 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
   double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
-  // target direction and position vector in the inertial frame
-  double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
+  // target direction in the ECI frame
+  double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
+  VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
 
-  // negative x-axis aligned with target
+  // x-axis aligned with target direction
   // this aligns with the camera, E- and S-band antennas
   double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirI, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+  VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
 
   // get sun vector model in ECI
   VectorOperations<double>::normalize(sunDirI, sunDirI, 3);