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

added Antistiction, added Nadir Pointing, added performSafe()
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Marquardt
2022-11-08 13:48:50 +01:00
parent 75ab11fc35
commit 20936faec6
9 changed files with 213 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -234,6 +234,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
refSatRate[2] = 0;
}
void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// 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);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
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 dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
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 quatBJ[4] = {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);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
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};