eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

limit rotation

Browse Source
This commit is contained in:
Marius Eggert 2024-02-22 15:04:30 +01:00
parent 7d4b97d977
commit e445f8942a
2 changed files with 56 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
mission/controller/acs/Guidance.cpp
View File

@ -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;

3
mission/controller/acs/Guidance.h
View File

@ -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";