diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index b167573a..7a630eef 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -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::normalize(groundStationDirI, xAxisIX, 3); VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); - // get sun vector model in ECI - VectorOperations::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::normalize(posSatI, earthDirNormalizedI, 3); + VectorOperations::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::dot(xAxisIX, sunDirI); - xDotS /= pow(VectorOperations::norm(xAxisIX, 3), 2); - double sunParallel[3], zAxisIX[3]; - VectorOperations::mulScalar(xAxisIX, xDotS, sunParallel, 3); - VectorOperations::subtract(sunDirI, sunParallel, zAxisIX, 3); + // sun avoidance calculations + double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}; + VectorOperations::mulScalar( + xAxisIX, VectorOperations::dot(xAxisIX, sunDirNormalizedI), sunPerpendicularX, 3); + VectorOperations::subtract(sunDirNormalizedI, sunPerpendicularX, sunFloorYZ, 3); + VectorOperations::normalize(sunFloorYZ, sunFloorYZ, 3); + double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0}, + strVecSunZ[3] = {0, 0, 0}; + VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], + strVecSunX, 3); + VectorOperations::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2], + strVecSunZ, 3); + VectorOperations::add(strVecSunX, strVecSunZ, strVecSun, 3); + VectorOperations::normalize(strVecSun, strVecSun, 3); + sunWeight = VectorOperations::dot(strVecSun, sunDirNormalizedI); + + // earth avoidance calculations + double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}; + VectorOperations::mulScalar( + xAxisIX, VectorOperations::dot(xAxisIX, earthDirNormalizedI), earthPerpendicularX, 3); + VectorOperations::subtract(earthDirNormalizedI, earthPerpendicularX, earthFloorYZ, 3); + VectorOperations::normalize(earthFloorYZ, earthFloorYZ, 3); + double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0}, + strVecEarthZ[3] = {0, 0, 0}; + VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], + strVecEarthX, 3); + VectorOperations::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2], + strVecEarthZ, 3); + VectorOperations::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); + VectorOperations::normalize(strVecEarth, strVecEarth, 3); + earthWeight = VectorOperations::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::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3); + VectorOperations::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3); + VectorOperations::add(zAxisSun, zAxisEarth, zAxisIX, 3); + VectorOperations::mulScalar(zAxisIX, -1, zAxisIX, 3); VectorOperations::normalize(zAxisIX, zAxisIX, 3); - // y-axis completes RHS - double yAxisIX[3]; + // calculate y-axis + double yAxisIX[3] = {0, 0, 0}; VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); VectorOperations::normalize(yAxisIX, yAxisIX, 3); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index c9ae1b07..6fbce0a4 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -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]);