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

cleanup
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-02-23 11:10:52 +01:00
parent 75654277b2
commit 2f43f58792
2 changed files with 6 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
fsfw

@ -1 +1 @@
Subproject commit 90f3f8ef1f67dc6d07c242b68bc59c805bb407dd Subproject commit 7d4e77843b64e812e537e2abd1213c632b6eeb48

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

@ -102,12 +102,9 @@ 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] = {3808618.68633079, -1163140.41530084, 5612341.3560814}, double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
posGroundStationI[3] = {3737093.20736689, -1967773.50625331, 4763980.76688758}, CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
groundStationDirI[3] = {0, 0, 0}; CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
// CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
// CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI,
// &timeAbsolute);
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3); VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
// negative x-axis aligned with target direction // negative x-axis aligned with target direction
@ -241,10 +238,6 @@ 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;
@ -255,14 +248,12 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
QuaternionOperations::inverse(quatIXprev, quatXprevI); QuaternionOperations::inverse(quatIXprev, quatXprevI);
QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX); QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
QuaternionOperations::normalize(quatXprevX); QuaternionOperations::normalize(quatXprevX);
double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * 1; double phiMax = acsParameters->gsTargetModeControllerParameters.omMax *
// acsParameters->onBoardParams.sampleTime; acsParameters->onBoardParams.sampleTime;
if (2 * std::acos(quatXprevX[3]) < phiMax) { if (2 * std::acos(quatXprevX[3]) < phiMax) {
return; return;
} }
sif::debug << "Reduced Quaternion Rotation Required" << std::endl;
// x-axis always needs full rotation // x-axis always needs full rotation
double phiX = 0, phiXvec[3] = {0, 0, 0}; double phiX = 0, phiXvec[3] = {0, 0, 0};
phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX)); phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));