eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

some fixes and debug stuff
EIVE/eive-obsw/pipeline/pr-main This commit looks good Details

This commit is contained in:
Marius Eggert 2024-02-22 16:59:26 +01:00
parent 5e6b0f5ea2
commit 58a8c5869c
1 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -102,9 +102,12 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
double posSatI[3] = {2030378.44284257, -56784.9332372798, 6567201.63882364},
posGroundStationI[3] = {3693004.40032203, -2049075.29966484, 4764086.46806924},
groundStationDirI[3] = {0, 0, 0};
// CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
// CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI,
// &timeAbsolute);
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
// negative x-axis aligned with target direction
@ -119,32 +122,34 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
// sun avoidance calculations
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
sunPerpendicularX, 3);
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
strVecSunZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
strVecSunX, 3);
VectorOperations<double>::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2],
VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
strVecSunZ, 3);
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
// earth avoidance calculations
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0};
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
earthPerpendicularX, 3);
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
strVecEarthZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
strVecEarthX, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2],
VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
strVecEarthZ, 3);
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
@ -162,9 +167,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
earthWeight *= normFactor;
// calculate z-axis for str blinding avoidance
double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3);
double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
@ -249,6 +254,8 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
return;
}
sif::debug << "Reduced Quaternion Rotation Required" << std::endl;
// x-axis always needs full rotation
double phiX = 0, phiXvec[3] = {0, 0, 0};
phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));