#include "Guidance.h" Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} 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); // 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 double yAxisXI[3] = {0, 0, 0}; VectorOperations::cross(zAxisXI, helperXI, yAxisXI); VectorOperations::normalize(yAxisXI, yAxisXI, 3); // x-axis completes RHS double xAxisXI[3] = {0, 0, 0}; VectorOperations::cross(yAxisXI, zAxisXI, xAxisXI); VectorOperations::normalize(xAxisXI, xAxisXI, 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); // calculate of reference rotation rate targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3], double velSatF[3], double targetQuat[4], double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetF[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.altitudeTgt, targetF); double targetDirF[3] = {0, 0, 0}; VectorOperations::subtract(targetF, posSatF, targetDirF, 3); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute); VectorOperations::subtract(targetI, posSatI, targetDirI, 3); // 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); // transform velocity into inertial frame double velSatI[3] = {0, 0, 0}; CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute); // orbital normal vector of target and velocity vector double orbitalNormalI[3] = {0, 0, 0}; VectorOperations::cross(posSatI, velSatI, 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 yAxisXI[3] = {0, 0, 0}; VectorOperations::cross(orbitalNormalI, xAxisXI, yAxisXI); VectorOperations::normalize(yAxisXI, yAxisXI, 3); // z-axis completes RHS double zAxisXI[3] = {0, 0, 0}; VectorOperations::cross(xAxisXI, yAxisXI, zAxisXI); // 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]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[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 posGroundStationF[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( acsParameters->gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt, acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); // target direction in the ECI frame double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0}; CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute); VectorOperations::subtract(posGroundStationI, posSatI, groundStationDirI, 3); // 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); // 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); // y-axis completes RHS double yAxisXI[3]; VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); VectorOperations::normalize(yAxisXI, yAxisXI, 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); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3], double velSatE[3], double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- // satellite position in inertial reference frame double posSatI[3] = {0, 0, 0}; CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute); // 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); // make z-Axis parallel to major part of camera resolution double zAxisXI[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); // y-Axis completes RHS double yAxisXI[3] = {0, 0, 0}; VectorOperations::cross(zAxisXI, xAxisXI, yAxisXI); // 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); targetRotationRate(timeDelta, targetQuat, refSatRate); } void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) { if (VectorOperations::norm(quatIXprev, 4) == 0) { std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } if (timeDelta != 0.0) { QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); } else { std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); } std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } 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); // Calculate error satellite rotational rate // Convert target rotational rate into body RF double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0}; QuaternionOperations::inverse(errorQuat, errorQuatInv); QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB); // Combine the target and reference satellite rotational rates double combinedRefSatRotRate[3] = {0, 0, 0}; VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); // Then subtract the combined required satellite rotational rates from the actual rate VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0}; comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate, errorQuat, errorSatRotRate, errorAngle); } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); if (rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } return acsctrl::MULTIPLE_RW_UNAVAILABLE; } void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); } void Guidance::getTargetParamsSafe(double sunTargetSafe[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)) { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, 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; }