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

fixed merge conflict
All checks were successful
EIVE/eive-obsw/pipeline/pr-eggert/acs This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt
2023-02-01 15:27:01 +01:00
parent adef468c0b e5b297a513
commit 60e75612fd
54 changed files with 942 additions and 581 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,6 +13,8 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <filesystem>
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
@ -22,12 +24,16 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete
Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) {
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
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));
}
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}
void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
@ -232,10 +238,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
@ -411,34 +414,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
// Position of the satellite in the earth/fixed frame via GPS and body
// velocity
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double velocityE[3];
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
double velocityB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1);
// Assign helper vector (north pole inertial)
double helperVec[3] = {0, 0, 1};
// Normal to velocity and sunDir
double crossVelSun[3] = {0, 0, 0};
VectorOperations<double>::cross(velocityB, sunDirB, crossVelSun);
// y- Axis as cross of normal velSun and zAxis
//
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(crossVelSun, sunDirB, yAxis);
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// complete RHS for x-Axis
//
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
// Transformation matrix to Sun, no further transforamtions, reference is already
// the EIVE body frame