eive-obsw/mission/controller/acs/Guidance.cpp
meggert 64d105cf87
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
fix
2024-01-29 11:11:47 +01:00

524 lines
25 KiB
C++

#include "Guidance.h"
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {}
[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, 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<double>::cartesianFromLatLongAlt(
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
// target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::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<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::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<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
// target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::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<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::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<double>::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<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
double normRefSatRate = normVelSatB / normTargetDirB;
double satRateDir[3] = {0, 0, 0};
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
VectorOperations<double>::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<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
double exclAngle = acsParameters->strParameters.exclusionAngle,
blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
double sightAngleSun =
VectorOperations<double>::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<double>::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(const timeval timeAbsolute, const double timeDelta,
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<double>::cartesianFromLatLongAlt(
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::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<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::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<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
VectorOperations<double>::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<double>::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<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
// orbital normal vector of target and velocity vector
double orbitalNormalI[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatI, velocityI, 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 yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-axis completes RHS
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::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, timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
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<double>::cartesianFromLatLongAlt(
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::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<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::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<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
VectorOperations<double>::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<double>::normalize(groundStationDirI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// get sun vector model in ECI
VectorOperations<double>::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<double>::dot(xAxis, sunDirI);
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
double sunParallel[3], zAxis[3];
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-axis completes RHS
double yAxis[3];
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
VectorOperations<double>::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, timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
// positive z-Axis of EIVE in direction of sun
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::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<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// x-axis completes RHS
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::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
//----------------------------------------------------------------------------
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
}
[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
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<double>::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<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::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<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
// target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::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<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::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<double>::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(const timeval timeAbsolute, const double timeDelta,
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<double>::ecfToEciWithNutPre(timeAbsolute, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// satellite position in inertial reference frame
double posSatI[3] = {0, 0, 0};
MatrixOperations<double>::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<double>::normalize(posSatI, xAxis, 3);
VectorOperations<double>::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<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
VectorOperations<double>::cross(xAxis, velocityI, zAxis);
VectorOperations<double>::normalize(zAxis, zAxis, 3);
// y-Axis completes RHS
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::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, timeDelta, 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
double invTargetQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(targetQuat, invTargetQuat);
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
// Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, 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) {
// 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<double>::mulScalar(errorQuat, -1, errorQuat, 4);
}
// Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
// Calculate error satellite rotational rate
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
}
void Guidance::targetRotationRate(const int8_t timeElapsedMax, 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));
}
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;
}