#include "Guidance.h" #include #include #include #include #include #include #include "string.h" #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} Guidance::~Guidance() {} void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], double refDirB[3], double quatBI[4], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to ECEF double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetE); // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, 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); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // transformation between ECEF and Body frame double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; QuaternionOperations::toDcm(quatBI, dcmBI); MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); // target Direction in the body frame double targetDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); double normRefDir = VectorOperations::norm(refDir, 3); double crossDir[3] = {0, 0, 0}; double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); targetQuat[0] = crossDir[0]; targetQuat[1] = crossDir[1]; targetQuat[2] = crossDir[2]; targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); VectorOperations::normalize(targetQuat, targetQuat, 4); //------------------------------------------------------------------------------------- // calculation of reference rotation rate //------------------------------------------------------------------------------------- double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3); MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); double normVelSatB = VectorOperations::norm(velSatB, 3); double normRefSatRate = normVelSatB / normTargetDirB; double satRateDir[3] = {0, 0, 0}; VectorOperations::cross(velSatB, targetDirB, satRateDir); VectorOperations::normalize(satRateDir, satRateDir, 3); VectorOperations::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); double exclAngle = acsParameters.strParameters.exclusionAngle, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; double sightAngleSun = VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); if (!(strBlindAvoidFlag)) { double critSightAngle = blindStart * exclAngle; if (sightAngleSun < critSightAngle) { strBlindAvoidFlag = true; } } else { if (sightAngleSun < blindEnd * exclAngle) { double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; double blindRefRate[3] = {0, 0, 0}; if (sunDirB[1] < 0) { blindRefRate[0] = normBlindRefRate; blindRefRate[1] = 0; blindRefRate[2] = 0; } else { blindRefRate[0] = -normBlindRefRate; blindRefRate[1] = 0; blindRefRate[2] = 0; } VectorOperations::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3); } else { strBlindAvoidFlag = false; } } } // revert calculated quaternion from qBX to qIX double quatIB[4] = {0, 0, 0, 1}; QuaternionOperations::inverse(quatBI, quatIB); QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, 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); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); MatrixOperations::multiply(*dcmIE, targetE, targetI, 3, 3, 1); VectorOperations::subtract(targetI, posSatI, targetDirI, 3); // x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(targetDirI, xAxis, 3); // transform velocity into inertial frame 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); // orbital normal vector of target and velocity vector double orbitalNormalI[3] = {0, 0, 0}; VectorOperations::cross(posSatI, velocityI, orbitalNormalI); VectorOperations::normalize(orbitalNormalI, orbitalNormalI, 3); // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture // resolution double yAxis[3] = {0, 0, 0}; VectorOperations::cross(orbitalNormalI, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); // z-axis completes RHS double zAxis[3] = {0, 0, 0}; VectorOperations::cross(xAxis, yAxis, zAxis); // join transformation matrix 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, targetQuat); int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } 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 //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double groundStationE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters.gsTargetModeControllerParameters.latitudeTgt, acsParameters.gsTargetModeControllerParameters.longitudeTgt, acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationE, posSatE, 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); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); MatrixOperations::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1); VectorOperations::subtract(groundStationI, posSatI, groundStationDirI, 3); // negative x-axis aligned with target direction // this aligns with the camera, E- and S-band antennas double xAxis[3] = {0, 0, 0}; VectorOperations::normalize(groundStationDirI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 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(xAxis, sunDirI); xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); double sunParallel[3], zAxis[3]; VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); VectorOperations::subtract(sunDirI, sunParallel, zAxis, 3); VectorOperations::normalize(zAxis, zAxis, 3); // y-axis completes RHS double yAxis[3]; VectorOperations::cross(zAxis, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); // 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]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- // positive z-Axis of EIVE in direction of sun double zAxis[3] = {0, 0, 0}; VectorOperations::normalize(sunDirI, zAxis, 3); // assign helper vector (north pole inertial) double helperVec[3] = {0, 0, 1}; // construct y-axis from helper vector and z-axis double yAxis[3] = {0, 0, 0}; VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); // x-axis completes RHS double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); VectorOperations::normalize(xAxis, xAxis, 3); // 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]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], double targetQuat[4], double refDirB[3], 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); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // transformation between ECEF and Body frame double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; QuaternionOperations::toDcm(quatBI, dcmBI); MatrixOperations::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3); // target Direction in the body frame double targetDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); double normRefDir = VectorOperations::norm(refDir, 3); double crossDir[3] = {0, 0, 0}; double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); targetQuat[0] = crossDir[0]; targetQuat[1] = crossDir[1]; targetQuat[2] = crossDir[2]; targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); VectorOperations::normalize(targetQuat, targetQuat, 4); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; // revert calculated quaternion from qBX to qIX double quatIB[4] = {0, 0, 0, 1}; QuaternionOperations::inverse(quatBI, quatIB); QuaternionOperations::multiply(quatIB, targetQuat, targetQuat); } void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- // 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); double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); // satellite position in inertial reference frame double posSatI[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); // 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(posSatI, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // make z-Axis parallel to major part of camera resolution double zAxis[3] = {0, 0, 0}; 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); // 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]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); // Keep scalar part of quaternion positive if (errorQuat[3] < 0) { VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); } // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); // Only give back error satellite rotational rate if orientation has already been aquired if (errorAngle < 2. / 180. * M_PI) { // First combine the target and reference satellite rotational rates double combinedRefSatRotRate[3] = {0, 0, 0}; VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); // Then substract the combined required satellite rotational rates from the actual rate VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3); } else { // If orientation has not been aquired yet set satellite rotational rate to zero errorSatRotRate = 0; } // target flag in matlab, importance, does look like it only gives feedback if pointing control is // under 150 arcsec ?? } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Keep scalar part of quaternion positive if (errorQuat[3] < 0) { VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); } // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); // Only give back error satellite rotational rate if orientation has already been aquired if (errorAngle < 2. / 180. * M_PI) { // Then substract the combined required satellite rotational rates from the actual rate VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); } else { // If orientation has not been aquired yet set satellite rotational rate to zero errorSatRotRate = 0; } // target flag in matlab, importance, does look like it only gives feedback if pointing control is // under 150 arcsec ?? } void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate) { //------------------------------------------------------------------------------------- // Calculation of target rotation rate //------------------------------------------------------------------------------------- double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); if (timeElapsed < timeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; VectorOperations::cross(quatInertialTarget, qDiff, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); omegaRefSaved[0] = omegaRefNew[0]; omegaRefSaved[1] = omegaRefNew[1]; omegaRefSaved[2] = omegaRefNew[2]; } else { refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } timeSavedQuaternion = now; savedQuaternion[0] = quatInertialTarget[0]; savedQuaternion[1] = quatInertialTarget[1]; savedQuaternion[2] = quatInertialTarget[2]; savedQuaternion[3] = quatInertialTarget[3]; } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()); bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()); bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); if (rw1valid && rw2valid && rw3valid && rw4valid) { std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { // @note: This one takes the normal pseudoInverse of all four raction wheels valid. // Does not make sense, but is implemented that way in MATLAB ?! // Thought: It does not really play a role, because in case there are more then one // reaction wheel invalid the pointing control is destined to fail. return returnvalue::FAILED; } } void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { // ToDo: if file does not exist anymore std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); } std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, 3 * sizeof(double)); } ReturnValue_t Guidance::solarArrayDeploymentComplete() { std::error_code e; if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) { std::remove(SD_0_SKEWED_PTG_FILE); if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) { return returnvalue::FAILED; } } if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { std::remove(SD_1_SKEWED_PTG_FILE); if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { return returnvalue::FAILED; } } return returnvalue::OK; }