PTG_TARGET should work now

mission/controller/AcsController.cpp

@@ -228,8 +228,6 @@ void AcsController::performPointingCtrl() {
                      &mekfData, &validMekf);
 
   uint8_t enableAntiStiction = true;
-  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
-         deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
@@ -245,8 +243,10 @@ void AcsController::performPointingCtrl() {
   double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
   double mgtDpDes[3] = {0, 0, 0};
 
-  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
+  // Variables required for guidance
-         errorAngle = 0;
+  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0},
+         errorQuatInterim[4] = {0, 0, 0, 1}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0,
+         satRotRate[3] = {0, 0, 0}, satRotRateError[3] = {0, 0, 0};
   switch (submode) {
     case acs::PTG_IDLE:
       guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@@ -279,10 +279,17 @@ void AcsController::performPointingCtrl() {
       guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
                                       gpsDataProcessed.gpsVelocity.value, targetQuat,
                                       targetSatRotRate);
-      guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuat, errorAngle);
+      guidance.calculateErrorQuat(targetQuat, mekfData.quatMekf.value, errorQuatInterim,
-      guidance.comparePtg(errorQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                                  errorAngle);
-                          deltaRate);
+      if (mekfData.satRotRateMekf.isValid()) {
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+        std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double));
+      } else {
+        std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double));
+      }
+      guidance.comparePtg(errorQuatInterim, acsParameters.targetModeControllerParameters.quatRef,
+                          errorQuat, satRotRate, targetSatRotRate,
+                          acsParameters.targetModeControllerParameters.refRotRate, satRotRateError);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, satRotRateError,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),

mission/controller/acs/Guidance.cpp

@@ -470,27 +470,23 @@ void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3])
               3 * sizeof(double));
 }
 
-void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
+void Guidance::comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4],
-                          double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
+                          double satRotRate[3], double satRotRateGuidance[3],
-  double satRate[3] = {0, 0, 0};
+                          double satRotRateRef[3], double satRotRateError[3]) {
-  std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
+  double combinedRefSatRate[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
+  VectorOperations<double>::add(satRotRateGuidance, satRotRateRef, combinedRefSatRate, 3);
-  // valid checks ?
+  VectorOperations<double>::subtract(satRotRate, combinedRefSatRate, satRotRateError, 3);
+
   double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
                                {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
                                {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
                                {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
 
-  MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
+  MatrixOperations<double>::multiply(*quatErrorMtx, oldErrorQuat, newErrorQuatComplete, 4, 4, 1);
 
-  if (quatErrorComplete[3] < 0) {
+  if (newErrorQuatComplete[3] < 0) {
-    quatErrorComplete[3] *= -1;
+    VectorOperations<double>::mulScalar(newErrorQuatComplete, -1, newErrorQuatComplete, 4);
   }
-
-  quatError[0] = quatErrorComplete[0];
-  quatError[1] = quatErrorComplete[1];
-  quatError[2] = quatErrorComplete[2];
-
   // target flag in matlab, importance, does look like it only gives feedback if pointing control is
   // under 150 arcsec ??
 }
@@ -537,6 +533,12 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn
   savedQuaternion[3] = quatInertialTarget[3];
 }
 
+void Guidance::calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4],
+                                  double errorAngle) {
+  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
+  errorAngle = 2 * acos(errorQuat[3]);
+}
+
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
                                                 double *rwPseudoInv) {
   bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());

mission/controller/acs/Guidance.h

@@ -34,10 +34,16 @@ class Guidance {
   // pointing
   void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]);
 
+  // @note: compares the target Quaternion in the ECI to the current orientation in ECI to compute
+  // the error quaternion and error angle
+  void calculateErrorQuat(double targetQuat[4], double currentQuat[4], double errorQuat[4],
+                          double errorAngle);
+
   // @note: compares target Quaternion and reference quaternion, also actual and desired satellite
   // rate
-  void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
+  void comparePtg(double oldErrorQuat[4], double quatRef[4], double newErrorQuatComplete[4],
-                  double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
+                  double satRotRate[3], double satRotRateGuidance[3], double satRotRateRef[3],
+                  double satRotRateError[3]);
 
   void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                        double *refSatRate);

mission/controller/acs/control/PtgCtrl.cpp

@@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
 }
 
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
-                     const double *qError, const double *deltaRate, const double *rwPseudoInv,
+                     const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
                      double *torqueRws) {
   //------------------------------------------------------------------------------------------------
   // Compute gain matrix K and P matrix
@@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double qErrorMin = pointingLawParameters->qiMin;
   double omMax = pointingLawParameters->omMax;
 
+  double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
+
   double cInt = 2 * om * zeta;
   double kInt = 2 * pow(om, 2);