eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

changed AcsParameters instance in guidance to const pointer
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-02-27 17:08:48 +01:00
parent 309358f447
commit 5c30adb9bb
4 changed files with 51 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
mission/controller/acs/Guidance.cpp
View File

@ -12,7 +12,7 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {}
@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
// target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
// 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];
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);
@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate in case of star tracker blinding
//-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
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 exclAngle = acsParameters->strParameters.exclusionAngle,
blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle;
@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
}
} else {
if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate;
@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
// 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);
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
@ -198,12 +198,13 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
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]) {
sif::debug << acsParameters->gsTargetModeControllerParameters.altitudeTgt << std::endl;
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
@ -211,9 +212,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
double groundStationE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.gsTargetModeControllerParameters.latitudeTgt,
acsParameters.gsTargetModeControllerParameters.longitudeTgt,
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
@ -262,7 +263,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
@ -332,9 +333,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
// 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];
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);
@ -406,7 +407,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
}
@ -516,19 +517,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
if (rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
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));
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));
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));
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));
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.
@ -542,13 +543,13 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}