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

updated AcsParams, added class function to get quaternion for sun pointing (guidance)
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt
2022-10-28 18:18:28 +02:00
parent e87221a8a3
commit ba541300ca
9 changed files with 133 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -45,8 +45,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
sensorValues->gpsSet.longitude.value,
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);
// Target direction in the ECEF frame
@ -190,6 +191,59 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
}
}
void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
double quatBJ[4] = {0, 0, 0, 0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::toDcm(quatBJ, dcmBJ);
double sunDirJ[3] = {0, 0, 0}, sunDir[3] = {0, 0, 0};
if (outputValues->sunDirModelValid) {
sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDir, 3, 3, 1);
}
else {
sunDir[0] = outputValues->sunDirEst[0];
sunDir[1] = outputValues->sunDirEst[1];
sunDir[2] = outputValues->sunDirEst[2];
}
double sunRef[3] = {0, 0, 0};
sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0];
sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1];
sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2];
double sunCross[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDir, sunRef, sunCross);
double normSunDir = VectorOperations<double>::norm(sunDir, 3);
double normSunRef = VectorOperations<double>::norm(sunRef, 3);
double dotSun = VectorOperations<double>::dot(sunDir, sunRef);
targetQuat[0] = sunCross[0];
targetQuat[1] = sunCross[1];
targetQuat[2] = sunCross[2];
targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
@ -310,7 +364,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one
// reaction wheel the pointing control is destined to fail.
// reaction wheel invalid the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];