diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index ee245519..a74b6082 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -12,113 +12,194 @@ void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::FusedRotRateData *fusedRotRateData) { estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); + estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateSourcesData); - // Fused Rotation Estimation of STR + if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and + acsParameters->onBoardParams.fusedRateFromStr) { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(true); + } + } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and + acsParameters->onBoardParams.fusedRateFromQuest) { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(true); + } + } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid( + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid()); + std::memcpy(fusedRotRateData->rotRateParallel.value, + fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid( + fusedRotRateSourcesData->rotRateParallelSusMgm.isValid()); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(true); + } else { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->setValidity(false, true); + } + } +} + +void FusedRotationEstimation::estimateFusedRotationRateStr( + ACS::SensorValues *sensorValues, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { - estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData); - } else { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldStr, quatOldInv); + QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); + QuaternionOperations::normalize(quatDelta); + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (angle == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } + } + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } + } + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(false); } } std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); - } else { - std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); - { - PoolReadGuard pg(fusedRotRateSourcesData); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(false); } } + std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); +} - // Fused Rotation Estimation of QUEST - if ((attitudeEstimationData->quatQuest.isValid())) { +void FusedRotationEstimation::estimateFusedRotationRateQuest( + acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { + if (attitudeEstimationData->quatQuest.isValid()) { if (VectorOperations::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { - estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true, - fusedRotRateSourcesData); - } else { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldQuest, quatOldInv); + QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, + quatDelta); + QuaternionOperations::normalize(quatDelta); + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (angle == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); } } std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); - } else { - std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); - { - PoolReadGuard pg(fusedRotRateSourcesData); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); } } - - // Fused Rotation Estimation of SUS&MGM + std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); } -void FusedRotationEstimation::estimateFusedRotationRateQuat( - double *quatNew, const double timeDelta, const bool str, - acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { - double quatOldInv[4] = {0, 0, 0, 0}; - double quatDelta[4] = {0, 0, 0, 0}; - - if (str) { - QuaternionOperations::inverse(quatOldStr, quatOldInv); - } else { - QuaternionOperations::inverse(quatOldQuest, quatOldInv); - } - QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); - QuaternionOperations::normalize(quatDelta); - - double rotVec[3] = {0, 0, 0}; - double angle = QuaternionOperations::getAngle(quatDelta); - if (angle == 0.0) { - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (str) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalStr.setValid(true); - } else { - std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, - 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); - } - } - } - VectorOperations::normalize(quatDelta, rotVec, 3); - VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); - { - PoolReadGuard pg(fusedRotRateSourcesData); - if (str) { - std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalStr.setValid(true); - } else { - std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); - fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); - } - } -} - -void FusedRotationEstimation::estimateFusedRotationRateSafe( +void FusedRotationEstimation::estimateFusedRotationRateSusMgm( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and - not fusedRotRateData->rotRateTotal.isValid()) or + not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or (not susDataProcessed->susVecTotDerivative.isValid() and not mgmDataProcessed->mgmVecTotDerivative.isValid())) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false); + } } // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { @@ -127,7 +208,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( return; } if (not susDataProcessed->susVecTot.isValid()) { - estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData); // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); @@ -151,7 +232,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( VectorOperations::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, fusedRotRateParallel, 3); } else { - estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData); // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); @@ -173,12 +254,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); { - 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); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true); + } } // store for calculation of angular acceleration @@ -188,31 +275,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( } void FusedRotationEstimation::estimateFusedRotationRateEclipse( - acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or not gyrDataProcessed->gyrVecTot.isValid() or - VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { + VectorOperations::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false); + } } return; } double angAccelB[3] = {0, 0, 0}; VectorOperations::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); double fusedRotRateTotal[3] = {0, 0, 0}; - VectorOperations::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal, - 3); + VectorOperations::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB, + fusedRotRateTotal, 3); { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true); + } } } diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 2c4c1588..2fc2ab8f 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -36,7 +36,7 @@ class FusedRotationEstimation { acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::FusedRotRateData *fusedRotRateData); + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);