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

enabled possibility to disable safe controller during eclipse
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-04-06 13:36:46 +02:00
parent 13844bce65
commit 2af1735cfd
5 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -49,8 +49,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(
*gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
MatrixOperations<double>::multiply(*gainMatrixDiagonal,
*(acsParameters->inertiaEIVE.inertiaMatrixDeployed),
*gainMatrix, 3, 3, 3);
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -60,7 +61,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrixDeployed), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
@ -85,7 +86,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
// torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate,
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), deltaRate,
torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
@ -116,7 +117,7 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
momentumRw, 3, 4, 1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate,
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error