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

Prevent STR Blinding #859

Merged
meggert merged 35 commits from prevent-str-blinding into main 2024-02-27 13:48:20 +01:00
Conversation 0 Commits 35 Files Changed 15 +24 -13
2 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 75654277b2 - Show all commits

2
mission/controller/acs/AcsParameters.h
View File

@ -915,7 +915,7 @@ class AcsParameters : public HasParametersIF {
struct StrParameters { struct StrParameters {
double exclusionAngle = 20. * DEG2RAD; double exclusionAngle = 20. * DEG2RAD;
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // body rf
} strParameters; } strParameters;
struct GpsParameters { struct GpsParameters {

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

@ -102,8 +102,8 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
// target direction in the ECI frame // target direction in the ECI frame
double posSatI[3] = {2030378.44284257, -56784.9332372798, 6567201.63882364}, double posSatI[3] = {3808618.68633079, -1163140.41530084, 5612341.3560814},
posGroundStationI[3] = {3693004.40032203, -2049075.29966484, 4764086.46806924}, posGroundStationI[3] = {3737093.20736689, -1967773.50625331, 4763980.76688758},
groundStationDirI[3] = {0, 0, 0}; groundStationDirI[3] = {0, 0, 0};
// CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); // CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
// CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, // CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI,
@ -187,6 +187,8 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
limitReferenceRotation(xAxisIX, targetQuat); limitReferenceRotation(xAxisIX, targetQuat);
targetRotationRate(timeDelta, targetQuat, targetSatRotRate); targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev));
} }
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
@ -239,6 +241,10 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
} }
void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) { void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
sif::debug << "xAxisIX = [" << xAxisIX[0] << " " << xAxisIX[1] << " " << xAxisIX[2] << "]"
<< std::endl;
sif::debug << "quatIX = [" << quatIX[3] << " " << quatIX[0] << " " << quatIX[1] << " "
<< quatIX[2] << "]" << std::endl;
if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
(VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) { (VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
return; return;
@ -247,10 +253,11 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
// check required rotation and return if below limit // check required rotation and return if below limit
double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0}; double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatIXprev, quatXprevI); QuaternionOperations::inverse(quatIXprev, quatXprevI);
QuaternionOperations::multiply(quatXprevI, quatIX, quatXprevX); QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * QuaternionOperations::normalize(quatXprevX);
acsParameters->onBoardParams.sampleTime; double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * 1;
if (std::acos(quatXprevX[3]) < phiMax) { // acsParameters->onBoardParams.sampleTime;
if (2 * std::acos(quatXprevX[3]) < phiMax) {
return; return;
} }
@ -263,14 +270,16 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
VectorOperations<double>::normalize(phiXvec, phiXvec, 3); VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0}; double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(phiXvec, std::cos(phiX / 2.), phiXvec, 3); VectorOperations<double>::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3);
std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec)); std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
quatXprevXtilde[3] = cos(phiX / 2.); quatXprevXtilde[3] = cos(phiX / 2.);
QuaternionOperations::multiply(quatIXprev, quatXprevXtilde, quatIXtilde); QuaternionOperations::normalize(quatXprevXtilde);
QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde);
// use the residual rotation up to the maximum // use the residual rotation up to the maximum
double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0}; double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
QuaternionOperations::multiply(quatXI, quatIXtilde, quatXXtilde); QuaternionOperations::inverse(quatIX, quatXI);
QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
double phiResidual = 0, phiResidualVec[3] = {0, 0, 0}; double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX)); phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
@ -278,14 +287,16 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3); VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0}; double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(phiResidualVec, std::cos(phiResidual / 2.), phiResidualVec, VectorOperations<double>::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec,
3); 3);
std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec)); std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
quatXhatXTilde[3] = cos(phiResidual / 2.); quatXhatXTilde[3] = std::cos(phiResidual / 2.);
QuaternionOperations::normalize(quatXhatXTilde);
// calculate final quaternion // calculate final quaternion
QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat); QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
QuaternionOperations::multiply(quatIXtilde, quatXTildeXhat, quatIX); QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX);
QuaternionOperations::normalize(quatIX);
} }
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],