diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index a68418d2..1b2df526 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -7,13 +7,13 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, - acsctrl::FusedRotRateData *fusedRotRateData) { + const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { 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(quatOld, 4) != 0) { - estimateFusedRotationRateStr(quatNew, fusedRotRateData); + if (VectorOperations::norm(quatOld, 4) != 0 and timeDelta != 0) { + estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData); } else { estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, fusedRotRateData); @@ -27,7 +27,39 @@ void FusedRotationEstimation::estimateFusedRotationRate( } void FusedRotationEstimation::estimateFusedRotationRateStr( - double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {} + double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOld, 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(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, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + } + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 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, rotVec, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + } +} void FusedRotationEstimation::estimateFusedRotationRateSafe( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 989f1f7a..490fe126 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -15,7 +15,7 @@ class FusedRotationEstimation { void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, - ACS::SensorValues *sensorValues, + ACS::SensorValues *sensorValues, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData); void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, @@ -23,7 +23,8 @@ class FusedRotationEstimation { acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData); - void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateStr(double *quatNew, const double timeDelta, + acsctrl::FusedRotRateData *fusedRotRateData); protected: private: