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 +17 -10
1 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 58a8c5869c - Show all commits

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

@ -102,9 +102,12 @@ 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] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; double posSatI[3] = {2030378.44284257, -56784.9332372798, 6567201.63882364},
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); posGroundStationI[3] = {3693004.40032203, -2049075.29966484, 4764086.46806924},
CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute); groundStationDirI[3] = {0, 0, 0};
// 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
@ -119,32 +122,34 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3); VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
// sun avoidance calculations // 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), VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
sunPerpendicularX, 3); sunPerpendicularX, 3);
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3); VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
VectorOperations<double>::normalize(sunFloorYZ, 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}, double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
strVecSunZ[3] = {0, 0, 0}; strVecSunZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
strVecSunX, 3); strVecSunX, 3);
VectorOperations<double>::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2], VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
strVecSunZ, 3); strVecSunZ, 3);
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3); VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
VectorOperations<double>::normalize(strVecSun, strVecSun, 3); VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI); sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
// earth avoidance calculations // 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), VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
earthPerpendicularX, 3); earthPerpendicularX, 3);
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3); VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
VectorOperations<double>::normalize(earthFloorYZ, 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}, double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
strVecEarthZ[3] = {0, 0, 0}; strVecEarthZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
strVecEarthX, 3); strVecEarthX, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2], VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
strVecEarthZ, 3); strVecEarthZ, 3);
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3); VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
@ -162,9 +167,9 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
earthWeight *= normFactor; earthWeight *= normFactor;
// calculate z-axis for str blinding avoidance // calculate z-axis for str blinding avoidance
double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0}; double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3); VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3); VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3); VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3); VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3); VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
@ -249,6 +254,8 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
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));