diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 2972dfed..079b627d 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/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::norm(quatIXprev, 4) == 0) or + (VectorOperations::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::dot(xAxisIXprev, xAxisIX)); + VectorOperations::cross(xAxisIXprev, xAxisIX, phiXvec); + VectorOperations::normalize(phiXvec, phiXvec, 3); + + double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0}; + VectorOperations::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::normalize(phiResidualVec, phiResidualVec, 3); + + double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0}; + VectorOperations::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; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 5284ba01..a914ecfe 100644 --- a/mission/controller/acs/Guidance.h +++ b/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";