eive-obsw/mission/controller/acs/control/PtgCtrl.cpp

211 lines
9.9 KiB
C++
Raw Normal View History

2022-09-20 13:43:26 +02:00
#include "PtgCtrl.h"
2022-12-01 15:56:55 +01:00
2022-09-27 11:06:11 +02:00
#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>
2022-09-20 13:43:26 +02:00
2023-02-28 10:47:37 +01:00
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
2022-09-20 13:43:26 +02:00
2022-12-01 15:56:55 +01:00
PtgCtrl::~PtgCtrl() {}
2022-09-20 13:43:26 +02:00
2023-01-23 16:34:52 +01:00
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
2023-02-17 15:57:07 +01:00
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
2023-01-23 16:34:52 +01:00
double *torqueRws) {
2022-12-01 15:56:55 +01:00
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
2023-01-12 15:19:21 +01:00
double om = pointingLawParameters->om;
double zeta = pointingLawParameters->zeta;
double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingLawParameters->omMax;
2022-12-01 15:56:55 +01:00
2023-02-17 15:57:07 +01:00
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
2022-12-01 15:56:55 +01:00
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);
double qErrorLaw[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
2023-06-02 09:33:40 +02:00
if (std::abs(qError[i]) < qErrorMin) {
2022-12-01 15:56:55 +01:00
qErrorLaw[i] = qErrorMin;
} else {
2023-06-02 09:33:40 +02:00
qErrorLaw[i] = std::abs(qError[i]);
2022-12-01 15:56:55 +01:00
}
}
2023-06-02 13:29:25 +02:00
2022-12-01 15:56:55 +01:00
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,
*(acsParameters->inertiaEIVE.inertiaMatrixDeployed),
*gainMatrix, 3, 3, 3);
2022-12-01 15:56:55 +01:00
// 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}};
2023-02-28 10:47:37 +01:00
MatrixOperations<double>::multiply(
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrixDeployed), *pMatrix, 3, 3, 3);
2022-12-01 15:56:55 +01:00
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++) {
2023-06-02 09:33:40 +02:00
if (std::abs(pError[i]) > 1) {
2022-12-13 11:51:03 +01:00
pErrorSign[i] = sign(pError[i]);
} else {
pErrorSign[i] = pError[i];
}
}
2023-03-10 11:39:31 +01:00
// torque for quaternion error
2022-12-13 11:51:03 +01:00
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
2023-03-10 11:39:31 +01:00
// torque for rate error
2022-12-13 11:51:03 +01:00
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), deltaRate,
2023-02-28 10:47:37 +01:00
torqueRate, 3, 3, 1);
2022-12-13 11:51:03 +01:00
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
2023-03-10 11:39:31 +01:00
// final commanded Torque for every reaction wheel
2022-12-13 11:51:03 +01:00
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);
2022-09-20 13:43:26 +02:00
}
2023-01-23 16:34:52 +01:00
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs) {
2023-04-27 11:21:02 +02:00
// concentrate RW speeds as vector and convert to double
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
2023-06-02 14:22:20 +02:00
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
2023-04-27 13:52:41 +02:00
// calculate RPM offset utilizing the nullspace
double rpmOffset[4] = {0, 0, 0, 0};
double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
rpmOffset, 4);
// calculate resulting angular momentum
double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
2022-10-12 15:06:24 +02:00
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
2023-02-28 10:47:37 +01:00
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
2023-04-27 13:52:41 +02:00
rwAngMomentum, 4);
// calculate resulting torque
double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
1, 4);
MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
4);
2022-09-20 13:43:26 +02:00
}
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid,
const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
if (not magFieldBValid or not pointingLawParameters->desatOn) {
return;
}
// concentrate RW speeds as vector and convert to double
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
// convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// calculate angular momentum of the satellite
double angMomentumSat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
angMomentumSat, 3, 3, 1);
// calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
// relocate RW speed zero to nullspace RW speed
double refSpeedRws[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
// convert speed from 10 RPM to 1 RPM
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
// convert to rad/s
VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
// calculate angular momentum of each RW
double angMomentumRwU[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
angMomentumRwU, 4);
// convert RW angular momentum to body RF
double angMomentumRw[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU,
angMomentumRw, 3, 4, 1);
// calculate total angular momentum
double angMomentumTotal[3] = {0, 0, 0};
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
// calculating momentum error
double deltaAngMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
deltaAngMomentum, 3);
// resulting magnetic dipole command
double crossAngMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
double factor =
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
}
2023-03-10 17:21:52 +01:00
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
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())};
2023-03-10 17:21:52 +01:00
int32_t currRwSpeed[4] = {
sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
2022-12-13 11:51:03 +01:00
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
2023-03-10 17:21:52 +01:00
if (rwCmdSpeeds[i] != 0) {
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
2023-04-27 10:47:20 +02:00
if (rwCmdSpeeds[i] > currRwSpeed[i]) {
2023-03-10 17:21:52 +01:00
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
2023-04-27 10:47:20 +02:00
} else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
2023-03-10 17:21:52 +01:00
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
2022-12-13 11:51:03 +01:00
}
}
}
}
}
}