when did i push this last

mission/controller/AcsController.cpp

@@ -249,26 +249,21 @@ void AcsController::performPointingCtrl() {
          satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0};
   switch (submode) {
     case acs::PTG_IDLE:
-      guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
+      guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
-                                targetQuat, refSatRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
-                     *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
-          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET:
@@ -296,17 +291,15 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET_GS:
-      guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
+      guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
-                               targetQuat, refSatRate);
+                               susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-                  4 * sizeof(double));
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -319,16 +312,16 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_NADIR:
       guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
                                            targetQuat, refSatRate);
       std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
-      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
       guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
                           deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
+      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -341,13 +334,13 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
       guidance.targetQuatPtgInertial(targetQuat, refSatRate);
       std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
                   4 * sizeof(double));
-      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
                           deltaRate);
       ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
@@ -363,6 +356,7 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       break;
   }
 

mission/controller/acs/AcsParameters.h

@@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF {
     uint8_t enableAntiStiction = true;
   } pointingLawParameters;
 
+  struct IdleModeControllerParameters : PointingLawParameters {
+  } idleModeControllerParameters;
+
   struct TargetModeControllerParameters : PointingLawParameters {
     double refDirection[3] = {-1, 0, 0};  // Antenna
-    double refRotRate[3] = {0, 0, 0};     // Not used atm, do we want an option to
+    double refRotRate[3] = {0, 0, 0};
-    // give this as an input- currently en calculation is done
     double quatRef[4] = {0, 0, 0, 1};
     int8_t timeElapsedMax = 10;  // rot rate calculations
 
@@ -860,6 +862,16 @@ class AcsParameters : public HasParametersIF {
     double blindRotRate = 1 * M_PI / 180;
   } targetModeControllerParameters;
 
+  struct GsTargetModeControllerParameters : PointingLawParameters {
+    double refDirection[3] = {-1, 0, 0};  // Antenna
+    int8_t timeElapsedMax = 10;           // rot rate calculations
+
+    // Default is Stuttgart GS
+    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
+    double altitudeTgt = 500;                     // [m]
+  } gsTargetModeControllerParameters;
+
   struct NadirModeControllerParameters : PointingLawParameters {
     double refDirection[3] = {-1, 0, 0};  // Antenna
     double quatRef[4] = {0, 0, 0, 1};

mission/controller/acs/Guidance.cpp

@@ -17,11 +17,11 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters
 Guidance::~Guidance() {}
 
 void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4],
-                                       double refSatRate[3]) {
+                                       double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
   //-------------------------------------------------------------------------------------
-  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
+  //  transform longitude, latitude and altitude to cartesian coordiantes (earth
   //  fixed/centered frame)
   double targetCart[3] = {0, 0, 0};
 
@@ -30,19 +30,19 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
       acsParameters.targetModeControllerParameters.longitudeTgt,
       acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
 
-  //	Target direction in the ECEF frame
+  //  target direction in the ECEF frame
   double targetDirE[3] = {0, 0, 0};
   VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
 
-  //	Transformation between ECEF and ECI frame
+  //  transformation between ECEF and ECI frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEI[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 dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[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, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
   //	Transformation between ECEF and Body frame
   // double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -52,20 +52,16 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
   // QuaternionOperations::toDcm(quatBJ, dcmBJ);
   // MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
 
-  //	Target Direction in the body frame
+  //  target Direction in the body frame
   double targetDirB[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
 
   //	rotation quaternion from two vectors
-  double refDir[3] = {0, 0, 0};
+  double refDirB[3] = {0, 0, 0};
-  refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
+  std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection,
-  refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
+              3 * sizeof(double));
-  refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
-  double noramlizedTargetDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
-  VectorOperations<double>::normalize(refDir, refDir, 3);
   double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
-  double normRefDir = VectorOperations<double>::norm(refDir, 3);
+  double normRefDirB = VectorOperations<double>::norm(refDir, 3);
   double crossDir[3] = {0, 0, 0};
   double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
   VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
@@ -213,133 +209,96 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
   targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
+                               double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for ground station pointing
   //-------------------------------------------------------------------------------------
-  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
+  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
-  // fixed/centered frame)
+  double groundStationE[3] = {0, 0, 0};
-  double groundStationCart[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters.gsTargetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.gsTargetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
+      acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
   double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
+  VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
 
-  //	Transformation between ECEF and ECI frame
+  // transformation between ECEF and ECI frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEI[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 dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[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, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
-  //	Target Direction and position vector in the inertial frame
+  // target direction and position vector in the inertial frame
-  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
+  double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
 
-  // negative x-Axis aligned with target (Camera/E-band transmitter position)
+  // negative x-axis aligned with target
+  // this aligns with the camera, E- and S-band antennas
   double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
+  VectorOperations<double>::normalize(targetDirI, xAxis, 3);
   VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
 
-  // get Sun Vector Model in ECI
+  // get sun vector model in ECI
-  double sunJ[3];
+  VectorOperations<double>::normalize(sunDirI, sunDirI, 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);
+  double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
   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>::subtract(sunDirI, sunParallel, zAxis, 3);
   VectorOperations<double>::normalize(zAxis, zAxis, 3);
 
-  // calculate y-axis
+  // y-axis completes RHS
   double yAxis[3];
   VectorOperations<double>::cross(zAxis, xAxis, yAxis);
   VectorOperations<double>::normalize(yAxis, yAxis, 3);
 
-  // Complete transformation matrix
+  // join transformation matrix
   double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                          {xAxis[1], yAxis[1], zAxis[1]},
                          {xAxis[2], yAxis[2], zAxis[2]}};
   double quatInertialTarget[4] = {0, 0, 0, 0};
   QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
 
-  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
-  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
+  targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate);
-
-  // 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);
 }
 
-void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to sun
   //-------------------------------------------------------------------------------------
-  double quatBJ[4] = {0, 0, 0, 0};
-  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::toDcm(quatBJ, dcmBJ);
-
-  double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
-  if (susDataProcessed->sunIjkModel.isValid()) {
-    std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
-    MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
-  } else if (susDataProcessed->susVecTot.isValid()) {
-    std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
-  } else {
-    return;
-  }
-
-  //	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 dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
-
   //  positive z-Axis of EIVE in direction of sun
   double zAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(sunDirB, zAxis, 3);
+  VectorOperations<double>::normalize(sunDirI, zAxis, 3);
 
-  // Assign helper vector (north pole inertial)
+  //  assign helper vector (north pole inertial)
   double helperVec[3] = {0, 0, 1};
 
-  // Construct y-axis from helper vector and z-axis
+  //  construct y-axis from helper vector and z-axis
   double yAxis[3] = {0, 0, 0};
   VectorOperations<double>::cross(zAxis, helperVec, yAxis);
   VectorOperations<double>::normalize(yAxis, yAxis, 3);
 
-  // Construct x-axis from y- and z-axes
+  //  construct x-axis from y- and z-axes
   double xAxis[3] = {0, 0, 0};
   VectorOperations<double>::cross(yAxis, zAxis, xAxis);
   VectorOperations<double>::normalize(xAxis, xAxis, 3);
 
-  // Transformation matrix to Sun, no further transforamtions, reference is already
+  // join transformation matrix
-  // the EIVE body frame
   double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
                          {xAxis[1], yAxis[1], zAxis[1]},
                          {xAxis[2], yAxis[2], zAxis[2]}};
-  double quatSun[4] = {0, 0, 0, 0};
+  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
-  QuaternionOperations::fromDcm(dcmTgt, quatSun);
-
-  targetQuat[0] = quatSun[0];
-  targetQuat[1] = quatSun[1];
-  targetQuat[2] = quatSun[2];
-  targetQuat[3] = quatSun[3];
 
   //----------------------------------------------------------------------------
   //	Calculation of reference rotation rate
@@ -501,6 +460,31 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   // under 150 arcsec ??
 }
 
+void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
+                          double targetSatRotRate[3], double errorQuat[4],
+                          double errorSatRotRate[3], double errorAngle) {
+  // First calculate error quaternion between current and target orientation
+  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
+  // Keep scalar part of quaternion positive
+  if (errorQuat[3] < 0) {
+    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
+  }
+  // Calculate error angle
+  errorAngle = QuaternionOperations::getAngle(errorQuat, true);
+
+  // Only give back error satellite rotational rate if orientation has already been aquired
+  if (errorAngle < 2. / 180. * M_PI) {
+    // Then substract the combined required satellite rotational rates from the actual rate
+    VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
+  } else {
+    // If orientation has not been aquired yet set satellite rotational rate to zero
+    errorSatRotRate = 0;
+  }
+
+  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
+  // under 150 arcsec ??
+}
+
 void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                                   double *refSatRate) {
   //-------------------------------------------------------------------------------------

mission/controller/acs/Guidance.h

@@ -19,11 +19,12 @@ class Guidance {
   void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]);
   void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
                               double targetSatRotRate[3]);
-  void targetQuatPtgGs(timeval now, double targetQuat[4], double targetSatRotRate[3]);
+  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
+                       double targetSatRotRate[3]);
 
   // Function to get the target quaternion and refence rotation rate for sun pointing after ground
   // station
-  void targetQuatPtgSun(timeval now, double targetQuat[4], double targetSatRotRate[3]);
+  void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
 
   // Function to get the target quaternion and refence rotation rate from gps position for Nadir
   // pointing
@@ -41,6 +42,9 @@ class Guidance {
   void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                   double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                   double errorQuat[4], double errorSatRotRate[3], double errorAngle);
+  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
+                  double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
+                  double errorAngle);
 
   void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                           double *targetSatRotRate);

mission/controller/acs/SensorProcessing.cpp

@@ -1,10 +1,3 @@
-/*
- * SensorProcessing.cpp
- *
- *  Created on: 7 Mar 2022
- *      Author: Robin Marquardt
- */
-
 #include "SensorProcessing.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>