limit rotation

mission/controller/acs/Guidance.cpp

@@ -180,6 +180,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
                         {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
   QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
+  limitReferenceRotation(xAxisIX, targetQuat);
   targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
 }
 
@@ -232,6 +233,54 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
   std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
 }
 
+void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
+  if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
+      (VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
+    return;
+  }
+
+  // check required rotation and return if below limit
+  double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
+  QuaternionOperations::inverse(quatIXprev, quatXprevI);
+  QuaternionOperations::multiply(quatXprevI, quatIX, quatXprevX);
+  double phiMax = acsParameters->gsTargetModeControllerParameters.omMax *
+                  acsParameters->onBoardParams.sampleTime;
+  if (std::acos(quatXprevX[3]) < phiMax) {
+    return;
+  }
+
+  // x-axis always needs full rotation
+  double phiX = 0, phiXvec[3] = {0, 0, 0};
+  phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));
+  VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
+  VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
+
+  double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(phiXvec, std::cos(phiX / 2.), phiXvec, 3);
+  std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
+  quatXprevXtilde[3] = cos(phiX / 2.);
+  QuaternionOperations::multiply(quatIXprev, quatXprevXtilde, quatIXtilde);
+
+  // use the residual rotation up to the maximum
+  double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
+  QuaternionOperations::multiply(quatXI, quatIXtilde, quatXXtilde);
+
+  double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
+  phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
+  std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
+  VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
+
+  double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(phiResidualVec, std::cos(phiResidual / 2.), phiResidualVec,
+                                      3);
+  std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
+  quatXhatXTilde[3] = cos(phiResidual / 2.);
+
+  // calculate final quaternion
+  QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
+  QuaternionOperations::multiply(quatIXtilde, quatXTildeXhat, quatIX);
+}
+
 void Guidance::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) {
@@ -298,7 +347,10 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
   return acsctrl::MULTIPLE_RW_UNAVAILABLE;
 }
 
-void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
+void Guidance::resetValues() {
+  std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev));
+  std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev));
+}
 
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
   std::error_code e;

mission/controller/acs/Guidance.h

@@ -35,6 +35,8 @@ class Guidance {
   void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
                           double *targetSatRotRate);
 
+  void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]);
+
   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);
@@ -52,6 +54,7 @@ class Guidance {
 
   bool strBlindAvoidFlag = false;
   double quatIXprev[4] = {0, 0, 0, 0};
+  double xAxisIXprev[3] = {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";