mission/controller/acs/Guidance.cpp

@@ -1,15 +1,5 @@
 #include "Guidance.h"
 
-#include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <mission/controller/acs/util/MathOperations.h>
-
-#include <cmath>
-#include <filesystem>
-#include <string>
-
 Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 Guidance::~Guidance() {}
@@ -455,44 +445,16 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
 }
 
 void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
-                                  double quatInertialTarget[4], double *refSatRate) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target rotation rate
-  //-------------------------------------------------------------------------------------
-  if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
-    std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
+                                  double quatIX[4], double *refSatRate) {
+  if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
+    std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
   }
-  if (timeDelta < timeElapsedMax and timeDelta != 0.0) {
-    double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
-    QuaternionOperations::inverse(quatInertialTarget, q);
-    QuaternionOperations::inverse(savedQuaternion, qS);
-    double qDiff[4] = {0, 0, 0, 0};
-    VectorOperations<double>::subtract(q, qS, qDiff, 4);
-    VectorOperations<double>::mulScalar(qDiff, 1. / timeDelta, qDiff, 4);
-
-    double tgtQuatVec[3] = {q[0], q[1], q[2]};
-    double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
-    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
-    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
-    VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
-    VectorOperations<double>::add(sum1, sum2, sum, 3);
-    VectorOperations<double>::subtract(sum, sum3, sum, 3);
-    double omegaRefNew[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
-
-    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
-    VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
-    omegaRefSaved[0] = omegaRefNew[0];
-    omegaRefSaved[1] = omegaRefNew[1];
-    omegaRefSaved[2] = omegaRefNew[2];
+  if (not timeDelta != 0.0) {
+    QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
   } else {
-    refSatRate[0] = 0;
-    refSatRate[1] = 0;
-    refSatRate[2] = 0;
+    std::memcpy(refSatRate, ZERO_VEC3, sizeof(refSatRate));
   }
-
-  std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
+  std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
 }
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
@@ -506,22 +468,27 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
   } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
-    return returnvalue::OK;
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
+                12 * sizeof(double));
+    return SINGLE_RW_UNAVAILABLE;
   } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
-    return returnvalue::OK;
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
+                12 * sizeof(double));
+    return SINGLE_RW_UNAVAILABLE;
   } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
-    return returnvalue::OK;
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
+                12 * sizeof(double));
+    return SINGLE_RW_UNAVAILABLE;
   } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
-    return returnvalue::OK;
-  } else {
-    return returnvalue::FAILED;
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
+                12 * sizeof(double));
+    return SINGLE_RW_UNAVAILABLE;
   }
+  return MULTIPLE_RW_UNAVAILABLE;
 }
 
+void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
+
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
   std::error_code e;
   if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or

mission/controller/acs/Guidance.h

@@ -1,9 +1,18 @@
 #ifndef GUIDANCE_H_
 #define GUIDANCE_H_
 
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/util/MathOperations.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 #include <time.h>
 
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
+#include <cmath>
+#include <filesystem>
+#include <string>
+
 #include "AcsParameters.h"
 #include "SensorValues.h"
 
@@ -14,6 +23,7 @@ class Guidance {
 
   void getTargetParamsSafe(double sunTargetSafe[3]);
   ReturnValue_t solarArrayDeploymentComplete();
+  void resetValues();
 
   // Function to get the target quaternion and reference rotation rate from gps position and
   // position of the ground station
@@ -56,12 +66,21 @@ class Guidance {
   // reation wheel 	maybe can be done in "commanding.h"
   ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
 
+  //! [EXPORT] : [COMMENT] A single RW has failed.
+  static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE =
+      returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 3);
+  //! [EXPORT] : [COMMENT] Multiple RWs have failed.
+  static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE =
+      returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 4);
+
  private:
   const AcsParameters *acsParameters;
 
+  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC4[3] = {0, 0, 0, 0};
+
   bool strBlindAvoidFlag = false;
-  double savedQuaternion[4] = {0, 0, 0, 0};
-  double omegaRefSaved[3] = {0, 0, 0};
+  double quatIXprev[4] = {0, 0, 0, 0};
 
   static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
   static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";