eive-obsw/mission/controller/acs/FusedRotationEstimation.cpp
meggert f736489430
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
bugfix
2023-07-21 11:10:09 +02:00

104 lines
4.8 KiB
C++

#include "FusedRotationEstimation.h"
FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) {
acsParameters = acsParameters_;
}
void FusedRotationEstimation::estimateFusedRotationRateSafe(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
not fusedRotRateData->rotRateTotal.isValid()) or
(not susDataProcessed->susVecTotDerivative.isValid() and
not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return;
}
if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
return;
}
// calculate rotation around the sun
double magSunCross[3] = {0, 0, 0};
VectorOperations<double>::cross(mgmDataProcessed->mgmVecTot.value,
susDataProcessed->susVecTot.value, magSunCross);
double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
double magNorm = VectorOperations<double>::norm(mgmDataProcessed->mgmVecTot.value, 3);
double fusedRotRateParallel[3] = {0, 0, 0};
if (magSunCrossNorm >
(acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
double omegaParallel =
VectorOperations<double>::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
pow(magSunCrossNorm, -2);
VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
fusedRotRateParallel, 3);
} else {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
return;
}
// calculate rotation orthogonal to the sun
double fusedRotRateOrthogonal[3] = {0, 0, 0};
VectorOperations<double>::cross(susDataProcessed->susVecTotDerivative.value,
susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
VectorOperations<double>::mulScalar(
fusedRotRateOrthogonal,
pow(VectorOperations<double>::norm(susDataProcessed->susVecTot.value, 3), -2),
fusedRotRateOrthogonal, 3);
// calculate total rotation rate
double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
// store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
}
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->setValidity(true, true);
}
}
void FusedRotationEstimation::estimateFusedRotationRateEclipse(
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
if (not gyrDataProcessed->gyrVecTot.isValid() or
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return;
}
double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);
}
}