/* * PtgCtrl.cpp * * Created on: 17 Jul 2022 * Author: Robin Marquardt */ #include "PtgCtrl.h" #include "../util/MathOperations.h" #include #include #include #include #include #include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} { loadAcsParameters(acsParameters_); } PtgCtrl::~PtgCtrl(){ } void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){ pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); inertiaEIVE = &(acsParameters_->inertiaEIVE); rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwMatrices =&(acsParameters_->rwMatrices); } void PtgCtrl::ptgLaw(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); VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *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 int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *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); } void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, double* torqueCommand) { for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { if (torqueMemory[i] != 0) { if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { torqueMemory[i] = 0; } else { torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; } } else { if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { if (omegaRw[i] < omegaMemory[i]) { torqueMemory[i] = -1; } else { torqueMemory[i] = 1; } torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; } } } else { torqueMemory[i] = 0; } omegaMemory[i] = omegaRw[i]; } }