From 300a6c5ff24d4136d6c1ca2736e57c176fe66a6f Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 11:09:34 +0200 Subject: [PATCH] use datasets --- .../acs/FusedRotationEstimation.cpp | 104 ++++++++++++------ .../controller/acs/FusedRotationEstimation.h | 20 ++-- 2 files changed, 82 insertions(+), 42 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index f64ad716..501d1913 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -5,64 +5,100 @@ FusedRotationEstimation::FusedRotationEstimation(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)))) { + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + if ((not mgmDataProcessed->mgmVecTot.isValid()) or + (not susDataProcessed->susVecTot.isValid() and + VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3)) == 0 or + ((VectorOperations::norm(susDataProcessed->susVecTotDerivative.value, 3) == 0 and + VectorOperations::norm(mgmDataProcessed->mgmVecTotDerivative.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; } - if (not sunValid) { - estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, - fusedRateOrthogonalB); + if (not susDataProcessed->susVecTot.isValid()) { + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); return; } // calculate rotation around the sun double magSunCross[3] = {0, 0, 0}; - VectorOperations::cross(magB, sunB, magSunCross); + VectorOperations::cross(mgmDataProcessed->mgmVecTot.value, + susDataProcessed->susVecTot.value, magSunCross); double magSunCrossNorm = VectorOperations::norm(magSunCross, 3); - double magNorm = VectorOperations::norm(magB, 3); + double magNorm = VectorOperations::norm(mgmDataProcessed->mgmVecTot.value, 3); + double fusedRotRateParallel[3] = {0, 0, 0}; 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 + VectorOperations::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) * + pow(magSunCrossNorm, -2); + VectorOperations::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, + fusedRotRateParallel, 3); } else { - estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, - fusedRateOrthogonalB); // ToDo + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); return; } // calculate rotation orthogonal to the sun - VectorOperations::cross(sunRateB, sunB, fusedRateOrthogonalB); - VectorOperations::mulScalar(fusedRateOrthogonalB, - pow(VectorOperations::norm(sunB, 2), 2), - fusedRateOrthogonalB, 3); + double fusedRotRateOrthogonal[3] = {0, 0, 0}; + VectorOperations::cross(susDataProcessed->susVecTotDerivative.value, + susDataProcessed->susVecTot.value, fusedRotRateOrthogonal); + VectorOperations::mulScalar( + fusedRotRateOrthogonal, + pow(VectorOperations::norm(susDataProcessed->susVecTot.value, 2), 2), + fusedRotRateOrthogonal, 3); // calculate total rotation rate - VectorOperations::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB); + double fusedRotRateTotal[3] = {0, 0, 0}; + VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); - std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double)); - if (rotRateValid) { - std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double)); + // 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(bool rotRateValid, - double rotRateB[3], - double fusedRateB[3], - double fusedRateParallelB[3], - double fusedRateOrthogonalB[3]) { - if (not rotRateValid or not VectorOperations::norm(fusedRateOldB, 3)) { +void FusedRotationEstimation::estimateFusedRotationRateEclipse( + acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + if (not gyrDataProcessed->gyrVecTot.isValid() or + VectorOperations::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::subtract(rotRateB, rotRateOldB, angAccelB, 3); - double omegaTotVec[3] = {0, 0, 0}; - VectorOperations::add(fusedRateOldB, angAccelB, fusedRateB, 3); + VectorOperations::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); + double fusedRotRateTotal[3] = {0, 0, 0}; + VectorOperations::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); + } } diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index f49748ed..dd538cc6 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -1,26 +1,30 @@ #ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ +#include #include #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]); + void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateData *fusedRotRateData); protected: private: + double constexpr ZERO_VEC[3] = {0, 0, 0}; + AcsParameters *acsParameters; double rotRateOldB[3] = {0, 0, 0}; - double fusedRateOldB[3] = {0, 0, 0}; + // double fusedRateOldB[3] = {0, 0, 0}; + + void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateData *fusedRotRateData); }; #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */