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

some things also broken some things fixed again

Browse Source
This commit is contained in:
Marius Eggert 2023-02-17 14:46:41 +01:00
parent f12fa77644
commit 6352b65f46
3 changed files with 123 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
mission/controller/AcsController.cpp
View File

@ -227,10 +227,7 @@ void AcsController::performPointingCtrl() {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf); &mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 0};
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -248,6 +245,7 @@ void AcsController::performPointingCtrl() {
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0}; double mgtDpDes[3] = {0, 0, 0};
double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0};
switch (submode) { switch (submode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
@ -273,11 +271,14 @@ void AcsController::performPointingCtrl() {
break; break;
case acs::PTG_TARGET: case acs::PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, if (!gpsDataProcessed.gpsPosition.isValid() || !gpsDataProcessed.gpsVelocity.isValid()) {
refSatRate); // ToDo: triggerEvent
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, return;
4 * sizeof(double)); }
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate); deltaRate);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,

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

@ -12,27 +12,12 @@
#include "util/CholeskyDecomposition.h" #include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h" #include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
Guidance::~Guidance() {} Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4],
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or double refSatRate[3]) {
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}
void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
timeval now,
double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude // Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
@ -49,7 +34,7 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3); VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame // Transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -60,13 +45,12 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame // Transformation between ECEF and Body frame
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0}; // double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ); // QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); // MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame // Target Direction in the body frame
double targetDirB[3] = {0, 0, 0}; double targetDirB[3] = {0, 0, 0};
@ -163,119 +147,70 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
} }
} }
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double *refSatRate) { double quatIX[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
(timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
if (timeElapsed < timeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[2] = omegaRefNew[2];
} else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
}
void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now,
double targetQuat[4], double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing // Calculation of target quaternion for target pointing
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
// fixed/centered frame) double targetE[3] = {0, 0, 0};
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3); VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame // transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[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 dcmEJDot[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(now, *dcmEJ, *dcmEJDot); MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE); MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// Target Direction and position vector in the inertial frame // target direction and position vector in the inertial frame
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
// negative x-Axis aligned with target (Camera/E-band transmitter position) // negative x-axis aligned with target
// this aligns with the camera, E- and S-band antennas
double xAxis[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirJ, xAxis, 3); VectorOperations<double>::normalize(targetDirI, xAxis, 3);
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3); VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// Transform velocity into inertial frame // transform velocity into inertial frame
double velocityE[3]; double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
// orbital normal vector // orbital normal vector of target and velocity vector
double orbitalNormalJ[3] = {0, 0, 0}; double orbitalNormalI[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ); VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3); VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution // 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}; double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis); VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3); VectorOperations<double>::normalize(yAxis, yAxis, 3);
// z-Axis completes RHS // z-axis completes RHS
double zAxis[3] = {0, 0, 0}; double zAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxis, yAxis, zAxis); VectorOperations<double>::cross(xAxis, yAxis, zAxis);
// Complete transformation matrix // join transformation matrix
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
{xAxis[1], yAxis[1], zAxis[1]}, {xAxis[1], yAxis[1], zAxis[1]},
{xAxis[2], yAxis[2], zAxis[2]}}; {xAxis[2], yAxis[2], zAxis[2]}};
double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmIX, quatIX);
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); refRotationRate(timeElapsedMax, now, quatIX, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
} }
void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) { void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
@ -293,7 +228,7 @@ void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatR
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3); VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
// Transformation between ECEF and IJK frame // Transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -367,7 +302,7 @@ void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSat
return; return;
} }
// Transformation between ECEF and IJK frame // Transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -422,7 +357,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3); VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame // Transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -479,7 +414,7 @@ void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], tim
double targetDirE[3] = {0, 0, 0}; double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3); VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame // Transformation between ECEF and ECI frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -560,6 +495,48 @@ void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSat
// under 150 arcsec ?? // under 150 arcsec ??
} }
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
double *refSatRate) {
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
(timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
if (timeElapsed < timeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
VectorOperations<double>::add(sum1, sum2, sum, 3);
VectorOperations<double>::subtract(sum, sum3, sum, 3);
double omegaRefNew[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[2] = omegaRefNew[2];
} else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) { double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
@ -590,3 +567,16 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
return returnvalue::FAILED; return returnvalue::FAILED;
} }
} }
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,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}

11
mission/controller/acs/Guidance.h
View File

@ -16,9 +16,10 @@ class Guidance {
// Function to get the target quaternion and refence rotation rate from gps position and // Function to get the target quaternion and refence rotation rate from gps position and
// position of the ground station // position of the ground station
void targetQuatPtgThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
double refSatRate[3]);
void targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground // Function to get the target quaternion and refence rotation rate for sun pointing after ground
// station // station
@ -26,15 +27,15 @@ class Guidance {
// Function to get the target quaternion and refence rotation rate from gps position for Nadir // Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing // pointing
void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]); void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial // Function to get the target quaternion and refence rotation rate from parameters for inertial
// pointing // pointing
void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]); void targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and // @note: compares target Quaternion and reference quaternion, also actual and desired satellite
// desired // rate
void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3], void comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]); double quatErrorComplete[4], double quatError[3], double deltaRate[3]);