#include "Guidance.h" Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} 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 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}; CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute); VectorOperations::normalize(posSatI, helperXI, 3); // construct y-axis from helper vector and z-axis double yAxisIX[3] = {0, 0, 0}; VectorOperations::cross(zAxisIX, helperXI, yAxisIX); VectorOperations::normalize(yAxisIX, yAxisIX, 3); // x-axis completes RHS double xAxisIX[3] = {0, 0, 0}; VectorOperations::cross(yAxisIX, zAxisIX, xAxisIX); VectorOperations::normalize(xAxisIX, xAxisIX, 3); // join transformation matrix 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); } void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3], const 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}; CoordinateTransformations::cartesianFromLatLongAlt( acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.altitudeTgt, targetF); // 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 xAxisIX[3] = {0, 0, 0}; VectorOperations::normalize(targetDirI, xAxisIX, 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 yAxisIX[3] = {0, 0, 0}; VectorOperations::cross(orbitalNormalI, xAxisIX, yAxisIX); VectorOperations::normalize(yAxisIX, yAxisIX, 3); // z-axis completes RHS double zAxisIX[3] = {0, 0, 0}; VectorOperations::cross(xAxisIX, yAxisIX, zAxisIX); // join transformation matrix 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); } void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3], const 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}; CoordinateTransformations::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(posGroundStationF, 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 xAxisIX[3] = {0, 0, 0}; VectorOperations::normalize(groundStationDirI, xAxisIX, 3); VectorOperations::mulScalar(xAxisIX, -1, xAxisIX, 3); // get earth vector in ECI double earthDirI[3] = {0, 0, 0}; VectorOperations::normalize(posSatI, earthDirI, 3); VectorOperations::mulScalar(earthDirI, -1, earthDirI, 3); // sun avoidance calculations double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, VectorOperations::dot(xAxisIX, sunDirI), sunPerpendicularX, 3); VectorOperations::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3); VectorOperations::normalize(sunFloorYZ, sunFloorYZ, 3); VectorOperations::mulScalar(sunFloorYZ, -1, zAxisSun, 3); double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0}, strVecSunZ[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], strVecSunX, 3); VectorOperations::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2], strVecSunZ, 3); VectorOperations::add(strVecSunX, strVecSunZ, strVecSun, 3); VectorOperations::normalize(strVecSun, strVecSun, 3); sunWeight = VectorOperations::dot(strVecSun, sunDirI); // earth avoidance calculations double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, VectorOperations::dot(xAxisIX, earthDirI), earthPerpendicularX, 3); VectorOperations::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3); VectorOperations::normalize(earthFloorYZ, earthFloorYZ, 3); VectorOperations::mulScalar(earthFloorYZ, -1, zAxisEarth, 3); double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0}, strVecEarthZ[3] = {0, 0, 0}; VectorOperations::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0], strVecEarthX, 3); VectorOperations::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2], strVecEarthZ, 3); VectorOperations::add(strVecEarthX, strVecEarthZ, strVecEarth, 3); VectorOperations::normalize(strVecEarth, strVecEarth, 3); earthWeight = VectorOperations::dot(strVecEarth, earthDirI); if ((sunWeight == 0.0) and (earthWeight == 0.0)) { // if this actually ever happens i will eat a broom sunWeight = 0.5; earthWeight = 0.5; } // normalize weights for convenience double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight)); sunWeight *= normFactor; earthWeight *= normFactor; // calculate z-axis for str blinding avoidance double zAxisIX[3] = {0, 0, 0}; VectorOperations::mulScalar(zAxisSun, sunWeight, zAxisSun, 3); VectorOperations::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3); VectorOperations::add(zAxisSun, zAxisEarth, zAxisIX, 3); VectorOperations::mulScalar(zAxisIX, -1, zAxisIX, 3); VectorOperations::normalize(zAxisIX, zAxisIX, 3); // calculate y-axis double yAxisIX[3] = {0, 0, 0}; VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); VectorOperations::normalize(yAxisIX, yAxisIX, 3); // join transformation matrix 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); limitReferenceRotation(xAxisIX, targetQuat); targetRotationRate(timeDelta, targetQuat, targetSatRotRate); std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev)); } void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatE[3], const 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 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 zAxisIX[3] = {0, 0, 0}; double velSatI[3] = {0, 0, 0}; CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute); VectorOperations::cross(xAxisIX, velSatI, zAxisIX); VectorOperations::normalize(zAxisIX, zAxisIX, 3); // y-Axis completes RHS double yAxisIX[3] = {0, 0, 0}; VectorOperations::cross(zAxisIX, xAxisIX, yAxisIX); // join transformation matrix 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); } 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::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) { if ((VectorOperations::norm(quatIXprev, 4) == 0) or (VectorOperations::norm(xAxisIXprev, 3) == 0)) { return; } // check required rotation and return if below limit double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0}; QuaternionOperations::inverse(quatIXprev, quatXprevI); QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX); QuaternionOperations::normalize(quatXprevX); double phiMax = acsParameters->gsTargetModeControllerParameters.omMax * acsParameters->onBoardParams.sampleTime; if (2 * std::acos(quatXprevX[3]) < phiMax) { return; } // x-axis always needs full rotation double phiX = 0, phiXvec[3] = {0, 0, 0}; phiX = std::acos(VectorOperations::dot(xAxisIXprev, xAxisIX)); VectorOperations::cross(xAxisIXprev, xAxisIX, phiXvec); VectorOperations::normalize(phiXvec, phiXvec, 3); double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3); std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec)); quatXprevXtilde[3] = cos(phiX / 2.); QuaternionOperations::normalize(quatXprevXtilde); QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde); // use the residual rotation up to the maximum double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0}; QuaternionOperations::inverse(quatIX, quatXI); QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde); double phiResidual = 0, phiResidualVec[3] = {0, 0, 0}; if ((phiX * phiX) > (phiMax * phiMax)) { phiResidual = 0; } else { phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX)); } std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec)); VectorOperations::normalize(phiResidualVec, phiResidualVec, 3); double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec, 3); std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec)); quatXhatXTilde[3] = std::cos(phiResidual / 2.); QuaternionOperations::normalize(quatXhatXTilde); // calculate final quaternion QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat); QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX); QuaternionOperations::normalize(quatIX); } 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 without reference // quaternion double errorQuatWoRef[4] = {0, 0, 0, 0}; QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef); // Then add rotation from reference quaternion QuaternionOperations::multiply(refQuat, errorQuatWoRef, 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, acsctrl::RwAvail *rwAvail) { rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and not rwAvail->rw4avail) { 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)); std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev)); } 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; }