diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index 3c4a3475..a8b0e9a6 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -2,6 +2,7 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp ActuatorCmd.cpp + FusedRotationEstimation.cpp Guidance.cpp Igrf13Model.cpp MultiplicativeKalmanFilter.cpp diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp new file mode 100644 index 00000000..1b4f87f8 --- /dev/null +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -0,0 +1,68 @@ +#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::norm(fusedRateOldB, 3)) or + ((not VectorOperations::norm(sunRateB, 3) and + not VectorOperations::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::cross(magB, sunB, magSunCross); + double magSunCrossNorm = VectorOperations::norm(magSunCross, 3); + double magNorm = VectorOperations::norm(magB, 3); + if (magSunCrossNorm > + (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) { + double omegaParallel = + VectorOperations::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2); + double omegaParallelVec[3] = {0, 0, 0}; + VectorOperations::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::cross(sunRateB, sunB, fusedRateOrthogonalB); + VectorOperations::mulScalar(fusedRateOrthogonalB, + pow(VectorOperations::norm(sunB, 2), 2), + fusedRateOrthogonalB, 3); + + // calculate total rotation rate + VectorOperations::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::norm(fusedRateOldB, 3)) { + return; + } + double angAccelB[3] = {0, 0, 0}; + VectorOperations::subtract(rotRateB, rotRateOldB, angAccelB, 3); + double omegaTotVec[3] = {0, 0, 0}; + VectorOperations::add(fusedRateOldB, angAccelB, fusedRateB, 3); +} diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h new file mode 100644 index 00000000..f49748ed --- /dev/null +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -0,0 +1,26 @@ +#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ +#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ + +#include +#include + +class FusedRotationEstimation { + public: + FusedRotationEstimation(AcsParameters *acsParameters_); + + void 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]); + void estimateFusedRotationRateEclipse(bool rotRateValid, double rotRateB[3], double fusedRateB[3], + double fusedRateParallelB[3], + double fusedRateOrthogonalB[3]); + + protected: + private: + AcsParameters *acsParameters; + double rotRateOldB[3] = {0, 0, 0}; + double fusedRateOldB[3] = {0, 0, 0}; +}; + +#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */