diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp new file mode 100644 index 00000000..eaffd909 --- /dev/null +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -0,0 +1,165 @@ +/* + * PtgCtrl.cpp + * + * Created on: 17 Jul 2022 + * Author: Robin Marquardt + */ + + + +#include "PtgCtrl.h" +#include +#include +#include +#include +#include +#include +#include + +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ + loadAcsParameters(acsParameters_); +} + +PtgCtrl::~PtgCtrl(){ + +} + +void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){ + pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); + rwHandlingParameters = &(acsParameters_->rwHandlingParameters); + rwMatrices =&(acsParameters_->rwMatrices); +} + +void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ + + //------------------------------------------------------------------------------------------------ + // Compute gain matrix K and P matrix + //------------------------------------------------------------------------------------------------ + double om = pointingModeControllerParameters->om; + double zeta = pointingModeControllerParameters->zeta; + double qErrorMin = pointingModeControllerParameters->qiMin; + double omMax = pointingModeControllerParameters->omMax; + + double cInt = 2 * om * zeta; + double kInt = 2 * pow(om,2); + + double qErrorLaw[3] = {0, 0, 0}; + + for (int i = 0; i < 3; i++) { + if (abs(qError[i]) < qErrorMin) { + qErrorLaw[i] = qErrorMin; + } + else { + qErrorLaw[i] = abs(qError[i]); + } + } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); + + double gain1 = cInt * omMax / qErrorLawNorm; + double gainVector[3] = {0, 0, 0}; + VectorOperations::mulScalar(qErrorLaw, gain1, gainVector, 3); + + double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixDiagonal[0][0] = gainVector[0]; + gainMatrixDiagonal[1][1] = gainVector[1]; + gainMatrixDiagonal[2][2] = gainVector[2]; + MatrixOperations::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3); + + // Inverse of gainMatrix + double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; + 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::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); + + //------------------------------------------------------------------------------------------------ + // Torque Calculations for the reaction wheels + //------------------------------------------------------------------------------------------------ + + double pError[3] = {0, 0, 0}; + MatrixOperations::multiply(*pMatrix, qError, pError, 3, 3, 1); + double pErrorSign[3] = {0, 0, 0}; + + for (int i = 0; i < 3; i++) { + + if (abs(pError[i]) > 1) { + pErrorSign[i] = sign(pError[i]); + } + else { + pErrorSign[i] = pError[i]; + } + } +// Torque for quaternion error + double torqueQuat[3] = {0, 0, 0}; + MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); + VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); + +// Torque for rate error + double torqueRate[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); + VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); + +// Final commanded Torque for every reaction wheel + double torque[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueQuat, torque, 3); + MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); + +} + +void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, + double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes) { + if ( !(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { + + mgtDpDes[0] = 0; + mgtDpDes[1] = 0; + mgtDpDes[2] = 0; + return; + + } + +// calculating momentum of satellite and momentum of reaction wheels + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *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); + double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); +// calculating momentum error + double deltaMomentum[3] = {0, 0, 0}; + VectorOperations::subtract(momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); +// resulting magnetic dipole command + double crossMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); + double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; + factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; + VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); + +} + +void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs) { + + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double wheelMomentum[4] = {0, 0, 0, 0}; + double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; + //Conversion to [rad/s] for further calculations + VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); + 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, wheelMomentum, 4); + double gainNs = pointingModeControllerParameters->gainNullspace; + double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, *nullSpaceMatrix, 4); + MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); + VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); + + +} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h new file mode 100644 index 00000000..5bd7f0da --- /dev/null +++ b/mission/controller/acs/control/PtgCtrl.h @@ -0,0 +1,60 @@ +/* + * PtgCtrl.h + * + * Created on: 17 Jul 2022 + * Author: Robin Marquardt + * + * @brief: This class handles the pointing control mechanism. Calculation of an commanded torque + * for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * + * @note: A description of the used algorithms can be found in + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 + */ + +#ifndef PTGCTRL_H_ +#define PTGCTRL_H_ + +#include +#include +#include +#include +#include +#include "acs/config/classIds.h" +#include +#include + +class PtgCtrl : public HasReturnvaluesIF { + +public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + PtgCtrl(AcsParameters *acsParameters_); + virtual ~PtgCtrl(); + + static const uint8_t INTERFACE_ID = CLASS_ID::PTG; + static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + + /* @brief: Load AcsParameters für 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 ptgGroundstation(const double mode,const double *qError,const double *deltaRate,const double *rwPseudoInv, double *torqueRws); + + void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, + double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes); + + void ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs); + +private: + AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters; + AcsParameters::RwHandlingParameters* rwHandlingParameters; + AcsParameters::InertiaEIVE* inertiaEIVE; + AcsParameters::RWMatrices* rwMatrices; +}; + +#endif /* ACS_CONTROL_PTGCTRL_H_ */