diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index bca5f190..4f1dad45 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -70,11 +70,6 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( double fusedRotRateTotal[3] = {0, 0, 0}; VectorOperations::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, @@ -83,6 +78,11 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); fusedRotRateData->setValidity(true, true); } + + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } } void FusedRotationEstimation::estimateFusedRotationRateEclipse(