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

x-axis now points away of equator plane for sunPtgMode
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-01-12 11:48:57 +01:00
parent 66e2d782b2
commit 27d6e95062
1 changed files with 20 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,7 +23,11 @@ Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (false) { // ToDo: if file does not exist anymore
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
} else {
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDirLeop[i];
}
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
} }
@ -230,10 +234,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsParameters.ptgTargetParameters.altitudeTgt, targetCart); acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
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);
@ -406,34 +407,28 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
double zAxis[3] = {0, 0, 0}; double zAxis[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirB, zAxis, 3); VectorOperations<double>::normalize(sunDirB, zAxis, 3);
// Position of the satellite in the earth/fixed frame via GPS and body // Position of satellite in ECEF
// velocity
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
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);
// Normal to velocity and sunDir // Check whether Sat is above or below equator
double crossVelSun[3] = {0, 0, 0}; // Assign helper vector (north pole inertial) accordingly
VectorOperations<double>::cross(velocityB, sunDirB, crossVelSun); double helperVec[3] = {0, 0, 0};
if (posSatE[2] > 0) {
double helperVec[2] = 1;
} else {
double helperVec[2] = -1;
}
// y- Axis as cross of normal velSun and zAxis //
double yAxis[3] = {0, 0, 0}; double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(crossVelSun, sunDirB, yAxis); VectorOperations<double>::cross(zAxis, helperVec, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3); VectorOperations<double>::normalize(yAxis, yAxis, 3);
// complete RHS for x-Axis //
double xAxis[3] = {0, 0, 0}; double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis); VectorOperations<double>::cross(yAxis, zAxis, xAxis);
VectorOperations<double>::normalize(xAxis, xAxis, 3);
// Transformation matrix to Sun, no further transforamtions, reference is already // Transformation matrix to Sun, no further transforamtions, reference is already
// the EIVE body frame // the EIVE body frame