diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f2d3cceb..a937f950 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -144,7 +144,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta } void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], - double quatIX[4], double targetSatRotRate[3]) { + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -203,14 +203,14 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmIX, quatIX); + QuaternionOperations::fromDcm(dcmIX, targetQuat); int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate); + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], - double targetSatRotRate[3]) { +void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], + double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -266,11 +266,10 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); + QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; - targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate); + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { @@ -289,7 +288,7 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // construct x-axis from y- and z-axes + // x-axis completes RHS double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); @@ -365,53 +364,49 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], refSatRate[2] = 0; } -void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now, +void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + // transformation between ECEF and ECI frame + double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot); + MathOperations::inverseMatrixDimThree(*dcmEI, *dcmIE); - // Transformation between ECEF and ECI frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + // satellite position in inertial reference frame + double posSatI[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); - // Target Direction in the body frame - double targetDirJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - - // negative x-Axis aligned with target (Camera position) + // negative x-axis aligned with position vector + // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::normalize(posSatI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // z-Axis parallel to long side of picture resolution + // make z-Axis parallel to major part of camera resolution double zAxis[3] = {0, 0, 0}; - std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); - VectorOperations::cross(xAxis, velocityJ, zAxis); + double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityI, 3); + VectorOperations::cross(xAxis, velocityI, zAxis); VectorOperations::normalize(zAxis, zAxis, 3); // y-Axis completes RHS double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, xAxis, yAxis); - // Complete transformation matrix + // join transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); + QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);