401 lines
19 KiB
C++
401 lines
19 KiB
C++
#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<double>::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<double>::normalize(posSatI, helperXI, 3);
|
|
|
|
// construct y-axis from helper vector and z-axis
|
|
double yAxisIX[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
|
|
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
|
|
|
// x-axis completes RHS
|
|
double xAxisIX[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
|
|
VectorOperations<double>::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<double>::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<double>::normalize(targetDirI, xAxisIX, 3);
|
|
VectorOperations<double>::mulScalar(xAxisIX, -1, 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<double>::cross(posSatI, velSatI, orbitalNormalI);
|
|
VectorOperations<double>::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<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
|
|
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
|
|
|
// z-axis completes RHS
|
|
double zAxisIX[3] = {0, 0, 0};
|
|
VectorOperations<double>::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<double>::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<double>::normalize(groundStationDirI, xAxisIX, 3);
|
|
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
|
|
|
|
// get earth vector in ECI
|
|
double earthDirI[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(posSatI, earthDirI, 3);
|
|
VectorOperations<double>::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<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
|
|
sunPerpendicularX, 3);
|
|
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
|
|
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
|
|
VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
|
|
double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
|
|
strVecSunZ[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
|
|
strVecSunX, 3);
|
|
VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
|
|
strVecSunZ, 3);
|
|
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
|
|
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
|
|
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
|
|
|
|
// earth avoidance calculations
|
|
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
|
|
earthPerpendicularX, 3);
|
|
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
|
|
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
|
|
VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
|
|
double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
|
|
strVecEarthZ[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
|
|
strVecEarthX, 3);
|
|
VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
|
|
strVecEarthZ, 3);
|
|
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
|
|
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
|
|
earthWeight = VectorOperations<double>::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<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
|
|
VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
|
|
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
|
|
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
|
|
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
|
|
|
|
// calculate y-axis
|
|
double yAxisIX[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
|
|
VectorOperations<double>::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<double>::normalize(posSatI, xAxisIX, 3);
|
|
VectorOperations<double>::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<double>::cross(xAxisIX, velSatI, zAxisIX);
|
|
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
|
|
|
|
// y-Axis completes RHS
|
|
double yAxisIX[3] = {0, 0, 0};
|
|
VectorOperations<double>::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<double>::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<double>::norm(quatIXprev, 4) == 0) or
|
|
(VectorOperations<double>::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<double>::dot(xAxisIXprev, xAxisIX));
|
|
VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
|
|
VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
|
|
|
|
double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
|
|
VectorOperations<double>::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<double>::normalize(phiResidualVec, phiResidualVec, 3);
|
|
|
|
double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
|
|
VectorOperations<double>::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<double>::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<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
|
|
// Then subtract the combined required satellite rotational rates from the actual rate
|
|
VectorOperations<double>::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;
|
|
}
|