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

gs pointing with sun/earth avoidance
EIVE/eive-obsw/pipeline/pr-main This commit looks good Details

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);
}
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
const double posSatF[3], const double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// 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>::mulScalar(xAxisIX, -1, xAxisIX, 3);
// get sun vector model in ECI
VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
// normalize sun vector in ECI
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
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxisIX, sunDirI);
xDotS /= pow(VectorOperations<double>::norm(xAxisIX, 3), 2);
double sunParallel[3], zAxisIX[3];
VectorOperations<double>::mulScalar(xAxisIX, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisIX, 3);
// sun avoidance calculations
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(
xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3);
VectorOperations<double>::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3);
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 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);
// y-axis completes RHS
double yAxisIX[3];
// calculate y-axis
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
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]);
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatF[3], double targetQuat[4], double refSatRate[3]);