diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 9e9c04f9..cdfc6d11 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -204,7 +204,6 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]) { - sif::debug << acsParameters->gsTargetModeControllerParameters.altitudeTgt << std::endl; //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 74a32a4a..c7c8d2f0 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,10 +1,3 @@ -/* - * PtgCtrl.cpp - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - */ - #include "PtgCtrl.h" #include @@ -16,16 +9,10 @@ #include "../util/MathOperations.h" -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} -void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - rwHandlingParameters = &(acsParameters_->rwHandlingParameters); - rwMatrices = &(acsParameters_->rwMatrices); -} - void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { @@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[2][2] = gainVector[2]; - MatrixOperations::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), - *gainMatrix, 3, 3, 3); + MatrixOperations::multiply( + *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3); // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, - 3, 3); + MatrixOperations::multiply( + *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3); MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); //------------------------------------------------------------------------------------------------ @@ -98,7 +85,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters // Torque for rate error double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate, + torqueRate, 3, 3, 1); VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); @@ -123,11 +111,13 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP // calculating momentum of satellite and momentum of reaction wheels double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); - MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, - 1); + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + momentumRwU, 4); + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, + momentumRw, 3, 4, 1); double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate, + momentumSat, 3, 3, 1); VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; @@ -152,11 +142,12 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); double diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, + VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, wheelMomentum, 4); double gainNs = pointingLawParameters->gainNullspace; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, + MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, + acsParameters->rwMatrices.nullspace, *nullSpaceMatrix, 4); MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); @@ -174,21 +165,22 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueComm for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { + if ((omegaRw[i] * torqueMemory[i]) > + acsParameters->rwHandlingParameters.stictionReleaseSpeed) { torqueMemory[i] = 0; } else { - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque; } } else { - if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && - (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if ((omegaRw[i] < acsParameters->rwHandlingParameters.stictionSpeed) && + (omegaRw[i] > -acsParameters->rwHandlingParameters.stictionSpeed)) { if (omegaRw[i] < omegaMemory[i]) { torqueMemory[i] = -1; } else { torqueMemory[i] = 1; } - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque; } } } else { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fb27fc6d..87185612 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -33,13 +33,7 @@ class PtgCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG; static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - /* @brief: Calculates the needed torque for the pointing control mechanism - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); @@ -54,15 +48,12 @@ class PtgCtrl { const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low - * @param: sensorValues class containing all RW related values * torqueCommand modified torque after antistiction */ void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); private: - AcsParameters::RwHandlingParameters *rwHandlingParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::RwMatrices *rwMatrices; + const AcsParameters *acsParameters; double torqueMemory[4] = {0, 0, 0, 0}; double omegaMemory[4] = {0, 0, 0, 0};