meggert
0dae3b04be
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
200 lines
8.8 KiB
C++
200 lines
8.8 KiB
C++
/*
|
|
* PtgCtrl.cpp
|
|
*
|
|
* Created on: 17 Jul 2022
|
|
* Author: Robin Marquardt
|
|
*/
|
|
|
|
#include "PtgCtrl.h"
|
|
|
|
#include <fsfw/globalfunctions/constants.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <fsfw/globalfunctions/sign.h>
|
|
#include <math.h>
|
|
|
|
#include "../util/MathOperations.h"
|
|
|
|
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(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) {
|
|
//------------------------------------------------------------------------------------------------
|
|
// Compute gain matrix K and P matrix
|
|
//------------------------------------------------------------------------------------------------
|
|
double om = pointingLawParameters->om;
|
|
double zeta = pointingLawParameters->zeta;
|
|
double qErrorMin = pointingLawParameters->qiMin;
|
|
double omMax = pointingLawParameters->omMax;
|
|
|
|
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
|
|
|
|
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<double>::norm(qErrorLaw, 3);
|
|
|
|
double gain1 = cInt * omMax / qErrorLawNorm;
|
|
double gainVector[3] = {0, 0, 0};
|
|
VectorOperations<double>::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<double>::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<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
|
|
3, 3);
|
|
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
|
|
|
|
//------------------------------------------------------------------------------------------------
|
|
// Torque Calculations for the reaction wheels
|
|
//------------------------------------------------------------------------------------------------
|
|
|
|
double pError[3] = {0, 0, 0};
|
|
MatrixOperations<double>::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<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
|
|
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
|
|
|
|
// Torque for rate error
|
|
double torqueRate[3] = {0, 0, 0};
|
|
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
|
|
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
|
|
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
|
|
|
|
// Final commanded Torque for every reaction wheel
|
|
double torque[3] = {0, 0, 0};
|
|
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
|
|
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
|
|
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
|
|
}
|
|
|
|
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
|
|
double *magFieldEst, bool magFieldEstValid, double *satRate,
|
|
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
|
|
int32_t *speedRw3, double *mgtDpDes) {
|
|
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
|
|
mgtDpDes[0] = 0;
|
|
mgtDpDes[1] = 0;
|
|
mgtDpDes[2] = 0;
|
|
return;
|
|
}
|
|
|
|
// 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<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
|
|
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
|
|
1);
|
|
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
|
|
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
|
|
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
|
|
// calculating momentum error
|
|
double deltaMomentum[3] = {0, 0, 0};
|
|
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
|
|
deltaMomentum, 3);
|
|
// resulting magnetic dipole command
|
|
double crossMomentumMagField[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
|
|
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
|
|
factor = (pointingLawParameters->deSatGainFactor) / normMag;
|
|
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
|
|
}
|
|
|
|
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
|
const int32_t *speedRw0, const int32_t *speedRw1,
|
|
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
|
|
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*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<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
|
|
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
|
|
double diffRwSpeed[4] = {0, 0, 0, 0};
|
|
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
|
VectorOperations<double>::mulScalar(diffRwSpeed, 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<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
|
|
*nullSpaceMatrix, 4);
|
|
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
|
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
|
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
|
}
|
|
|
|
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
|
|
bool rwAvailable[4] = {
|
|
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
|
|
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
|
|
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
|
|
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
|
|
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
|
|
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
|
|
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];
|
|
}
|
|
}
|