diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 3641e41e..ef21f7fc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -8,8 +8,8 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, 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); + double zAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(sunDirI, zAxisIX, 3); // 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}; @@ -17,20 +17,20 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, VectorOperations::normalize(posSatI, helperXI, 3); // 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); + double yAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(zAxisIX, helperXI, yAxisIX); + VectorOperations::normalize(yAxisIX, yAxisIX, 3); // x-axis completes RHS - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); - VectorOperations::normalize(xAxisXI, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(yAxisIX, zAxisIX, xAxisIX); + VectorOperations::normalize(xAxisIX, xAxisIX, 3); // join transformation matrix - double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; - QuaternionOperations::fromDcm(dcmXI, targetQuat); + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); // calculate of reference rotation rate targetRotationRate(timeDelta, targetQuat, targetSatRotRate); @@ -59,8 +59,8 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirI, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirI, xAxisIX, 3); // transform velocity into inertial frame double velSatI[3] = {0, 0, 0}; @@ -73,18 +73,18 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture // resolution - double yAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalI, xAxisXI, yAxisXI); - VectorOperations::normalize(yAxisXI, yAxisXI, 3); + double yAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalI, xAxisIX, yAxisIX); + VectorOperations::normalize(yAxisIX, yAxisIX, 3); // z-axis completes RHS - double zAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(xAxisXI, yAxisXI, zAxisXI); + double zAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(xAxisIX, yAxisIX, zAxisIX); // join transformation matrix - double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); @@ -111,32 +111,32 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou // negative x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(groundStationDirI, xAxisXI, 3); - VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(groundStationDirI, xAxisIX, 3); + VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); // get sun vector model in ECI VectorOperations::normalize(sunDirI, sunDirI, 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(xAxisXI, sunDirI); - xDotS /= pow(VectorOperations::norm(xAxisXI, 3), 2); - double sunParallel[3], zAxisXI[3]; - VectorOperations::mulScalar(xAxisXI, xDotS, sunParallel, 3); - VectorOperations::subtract(sunDirI, sunParallel, zAxisXI, 3); - VectorOperations::normalize(zAxisXI, zAxisXI, 3); + 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); + VectorOperations::normalize(zAxisIX, zAxisIX, 3); // y-axis completes RHS - double yAxisXI[3]; - VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); - VectorOperations::normalize(yAxisXI, yAxisXI, 3); + double yAxisIX[3]; + VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); + VectorOperations::normalize(yAxisIX, yAxisIX, 3); // join transformation matrix - double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; - QuaternionOperations::fromDcm(dcmXI, targetQuat); + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } @@ -153,26 +153,26 @@ void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, // negative x-axis aligned with position vector // this aligns with the camera, E- and S-band antennas - double xAxisXI[3] = {0, 0, 0}; - VectorOperations::normalize(posSatI, xAxisXI, 3); - VectorOperations::mulScalar(xAxisXI, -1, xAxisXI, 3); + double xAxisIX[3] = {0, 0, 0}; + VectorOperations::normalize(posSatI, xAxisIX, 3); + VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); // make z-Axis parallel to major part of camera resolution - double zAxisXI[3] = {0, 0, 0}; + double zAxisIX[3] = {0, 0, 0}; double velSatI[3] = {0, 0, 0}; CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute); - VectorOperations::cross(xAxisXI, velSatI, zAxisXI); - VectorOperations::normalize(zAxisXI, zAxisXI, 3); + VectorOperations::cross(xAxisIX, velSatI, zAxisIX); + VectorOperations::normalize(zAxisIX, zAxisIX, 3); // y-Axis completes RHS - double yAxisXI[3] = {0, 0, 0}; - VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); + double yAxisIX[3] = {0, 0, 0}; + VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); // join transformation matrix - double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]}, - {xAxisXI[1], yAxisXI[1], zAxisXI[1]}, - {xAxisXI[2], yAxisXI[2], zAxisXI[2]}}; - QuaternionOperations::fromDcm(dcmXI, targetQuat); + double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]}, + {xAxisIX[1], yAxisIX[1], zAxisIX[1]}, + {xAxisIX[2], yAxisIX[2], zAxisIX[2]}}; + QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, refSatRate); }