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

gs pointing with sun/earth avoidance
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-13 15:58:44 +01:00
parent d32f7b31c3
commit 16255a91dc
2 changed files with 62 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -90,9 +90,9 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
targetRotationRate(timeDelta, targetQuat, targetSatRotRate); targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
} }
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
double sunDirI[3], double targetQuat[4], const double posSatF[3], const double sunDirI[3],
double targetSatRotRate[3]) { double targetQuat[4], double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing // Calculation of target quaternion for ground station pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -115,20 +115,66 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou
VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3); VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3); VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
// get sun vector model in ECI // normalize sun vector in ECI
VectorOperations<double>::normalize(sunDirI, sunDirI, 3); double sunDirNormalizedI[3] = {0, 0, 0};
// get earth vector in ECI
double earthDirNormalizedI[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, earthDirNormalizedI, 3);
VectorOperations<double>::mulScalar(earthDirNormalizedI, -1, earthDirNormalizedI, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector // sun avoidance calculations
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
double xDotS = VectorOperations<double>::dot(xAxisIX, sunDirI); VectorOperations<double>::mulScalar(
xDotS /= pow(VectorOperations<double>::norm(xAxisIX, 3), 2); xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3);
double sunParallel[3], zAxisIX[3]; VectorOperations<double>::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3);
VectorOperations<double>::mulScalar(xAxisIX, xDotS, sunParallel, 3); VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisIX, 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],
strVecSunZ, 3);
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirNormalizedI);
// earth avoidance calculations
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(
xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirNormalizedI), earthPerpendicularX, 3);
VectorOperations<double>::subtract(earthDirNormalizedI, earthPerpendicularX, earthFloorYZ, 3);
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 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],
strVecEarthZ, 3);
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirNormalizedI);
if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
// if this actually ever happens i will eat a broom
sunWeight = 0.5;
earthWeight = 0.5;
}
// normalize weights for convenience
double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight));
sunWeight *= normFactor;
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);
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3); VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
// y-axis completes RHS // calculate y-axis
double yAxisIX[3]; double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX); VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3); VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);

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

@ -28,8 +28,8 @@ class Guidance {
const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]); const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatE[3], double quatIX[4], double targetSatRotRate[3]); double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3], void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3], void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatF[3], double targetQuat[4], double refSatRate[3]); double velSatF[3], double targetQuat[4], double refSatRate[3]);