meggert
beaacc251b
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
69 lines
3.1 KiB
C++
69 lines
3.1 KiB
C++
#include "FusedRotationEstimation.h"
|
|
|
|
FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) {
|
|
acsParameters = acsParameters_;
|
|
}
|
|
|
|
void FusedRotationEstimation::estimateFusedRotationRateSafe(
|
|
bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3],
|
|
double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3],
|
|
double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) {
|
|
if ((not magValid) or (not sunValid and not VectorOperations<double>::norm(fusedRateOldB, 3)) or
|
|
((not VectorOperations<double>::norm(sunRateB, 3) and
|
|
not VectorOperations<double>::norm(magRateB, 3)))) {
|
|
return;
|
|
}
|
|
if (not sunValid) {
|
|
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
|
|
fusedRateOrthogonalB); // ToDo
|
|
return;
|
|
}
|
|
|
|
// calculate rotation around the sun
|
|
double magSunCross[3] = {0, 0, 0};
|
|
|
|
VectorOperations<double>::cross(magB, sunB, magSunCross);
|
|
double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
|
|
double magNorm = VectorOperations<double>::norm(magB, 3);
|
|
if (magSunCrossNorm >
|
|
(acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
|
|
double omegaParallel =
|
|
VectorOperations<double>::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2);
|
|
double omegaParallelVec[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(sunB, omegaParallel, omegaParallelVec, 3);
|
|
std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double)); // to dataset
|
|
} else {
|
|
estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
|
|
fusedRateOrthogonalB); // ToDo
|
|
return;
|
|
}
|
|
|
|
// calculate rotation orthogonal to the sun
|
|
VectorOperations<double>::cross(sunRateB, sunB, fusedRateOrthogonalB);
|
|
VectorOperations<double>::mulScalar(fusedRateOrthogonalB,
|
|
pow(VectorOperations<double>::norm(sunB, 2), 2),
|
|
fusedRateOrthogonalB, 3);
|
|
|
|
// calculate total rotation rate
|
|
VectorOperations<double>::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB);
|
|
|
|
std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double));
|
|
if (rotRateValid) {
|
|
std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double));
|
|
}
|
|
}
|
|
|
|
void FusedRotationEstimation::estimateFusedRotationRateEclipse(bool rotRateValid,
|
|
double rotRateB[3],
|
|
double fusedRateB[3],
|
|
double fusedRateParallelB[3],
|
|
double fusedRateOrthogonalB[3]) {
|
|
if (not rotRateValid or not VectorOperations<double>::norm(fusedRateOldB, 3)) {
|
|
return;
|
|
}
|
|
double angAccelB[3] = {0, 0, 0};
|
|
VectorOperations<double>::subtract(rotRateB, rotRateOldB, angAccelB, 3);
|
|
double omegaTotVec[3] = {0, 0, 0};
|
|
VectorOperations<double>::add(fusedRateOldB, angAccelB, fusedRateB, 3);
|
|
}
|