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

same for nadir single axis
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-02-21 10:44:02 +01:00
parent 35ad0a40d3
commit 1d5856b4d4
2 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -302,7 +302,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
refSatRate[2] = 0; refSatRate[2] = 0;
} }
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double refDirB[3], void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3],
double refSatRate[3]) { double refSatRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing // Calculation of target quaternion for Nadir pointing
@ -311,22 +312,20 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], d
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3); VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// transformation between ECEF and ECI 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);
// 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 dcmBI[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}; QuaternionOperations::toDcm(quatBI, dcmBI);
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
QuaternionOperations::toDcm(quatBJ, dcmBJ);
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};
@ -357,6 +356,11 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], d
refSatRate[0] = 0; refSatRate[0] = 0;
refSatRate[1] = 0; refSatRate[1] = 0;
refSatRate[2] = 0; refSatRate[2] = 0;
// revert calculated quaternion from qBX to qIX
double quatIB[4] = {0, 0, 0, 1};
QuaternionOperations::inverse(quatBI, quatIB);
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
} }
void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],

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

@ -30,7 +30,8 @@ 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 targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
double targetQuat[4], double refDirB[3], double refSatRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3], void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]); double targetQuat[4], double refSatRate[3]);