diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 46d98599..89f3d958 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -4,24 +4,24 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter Guidance::~Guidance() {} -void Guidance::targetQuatPtgIdle(double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]) { - //------------------------------------------------------------------------------------- - // Calculation of target quaternion to sun - //------------------------------------------------------------------------------------- - // positive z-Axis of EIVE in direction of sun +void Guidance::targetQuatPtgIdle(const double timeDelta, timeval timeAbsolute, + const double sunDirI[3], const double posSatF[4], + double targetQuat[4], double targetSatRotRate[3]) { + // positive z-Axis of EIVE in direction of sun double zAxisXI[3] = {0, 0, 0}; VectorOperations::normalize(sunDirI, zAxisXI, 3); - // assign helper vector (north pole inertial) - double helperXI[3] = {0, 0, 1}; + // determine helper vector to point x-Axis and therefore the STR away from Earth + double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0}; + CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); + VectorOperations::normalize(posSatI, helperXI, 3); - // construct y-axis from helper vector and z-axis + // construct y-axis from helper vector and z-axis double yAxisXI[3] = {0, 0, 0}; VectorOperations::cross(zAxisXI, helperXI, yAxisXI); VectorOperations::normalize(yAxisXI, yAxisXI, 3); - // x-axis completes RHS + // x-axis completes RHS double xAxisXI[3] = {0, 0, 0}; VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); VectorOperations::normalize(xAxisXI, xAxisXI, 3); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 15e88cdc..973c0bd3 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -25,8 +25,8 @@ class Guidance { ReturnValue_t solarArrayDeploymentComplete(); void resetValues(); - void targetQuatPtgIdle(const double timeDelta, double sunDirI[3], double targetQuat[4], - double targetSatRotRate[3]); + void targetQuatPtgIdle(const double timeDelta, timeval timeAbsolute, const double sunDirI[3], + 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],