eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

this is a mess
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-23 16:56:36 +01:00
parent 886dd17e4a
commit 4a67f9ffe5
14 changed files with 234 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -7,32 +7,73 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) {
if (acsParameters->strParameters.useStrForRotRate and
(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
acsctrl::FusedRotRateData *fusedRotRateData) {
estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
// Fused Rotation Estimation of STR
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<double>::norm(quatOld, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData);
if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData);
} else {
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData);
{
PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOld, quatNew, sizeof(quatOld));
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
} else {
std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld));
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData);
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
{
PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
// Fused Rotation Estimation of QUEST
if ((attitudeEstimationData->quatQuest.isValid())) {
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true,
fusedRotRateSourcesData);
} else {
{
PoolReadGuard pg(fusedRotRateSourcesData);
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);
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
// Fused Rotation Estimation of SUS&MGM
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) {
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};
QuaternionOperations::inverse(quatOld, quatOldInv);
if (str) {
QuaternionOperations::inverse(quatOldStr, quatOldInv);
} else {
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
}
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
QuaternionOperations::normalize(quatDelta);
@ -40,25 +81,28 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
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);
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<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::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);
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);
}
}
}