diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 20f12d05..6653f4d1 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -22,6 +22,10 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( } if (not susDataProcessed->susVecTot.isValid()) { estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } return; } @@ -42,6 +46,10 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( fusedRotRateParallel, 3); } else { estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } return; }