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

fixes

Browse Source
This commit is contained in:
Marius Eggert 2023-12-05 15:02:17 +01:00
parent a05fd75828
commit dab10596f6
1 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,9 +23,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
fusedRotRateData->rotRateOrthogonal.setValid(false); fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false); fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true); fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} }
@ -37,9 +37,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
fusedRotRateData->rotRateOrthogonal.setValid(false); fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false); fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true); fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} }
@ -52,9 +52,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid( fusedRotRateData->rotRateParallel.setValid(
fusedRotRateSourcesData->rotRateParallelSusMgm.isValid()); fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateTotal.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true); fusedRotRateData->rotRateTotal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
fusedRotRateData->rotRateSource.setValid(true); fusedRotRateData->rotRateSource.setValid(true);
} else { } else {
@ -62,7 +62,7 @@ void FusedRotationEstimation::estimateFusedRotationRate(
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.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)); std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
fusedRotRateData->setValidity(false, true); fusedRotRateData->setValidity(false, true);
} }
@ -82,11 +82,13 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
QuaternionOperations::inverse(quatOldStr, quatOldInv); QuaternionOperations::inverse(quatOldStr, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
QuaternionOperations::normalize(quatDelta); if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0}; double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta); double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) { if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -95,6 +97,8 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
fusedRotRateSourcesData->rotRateTotalStr.setValid(true); fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
} }
} }
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
} }
VectorOperations<double>::normalize(quatDelta, rotVec, 3); VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3); VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
@ -139,11 +143,13 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
QuaternionOperations::inverse(quatOldQuest, quatOldInv); QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv,
quatDelta); quatDelta);
QuaternionOperations::normalize(quatDelta); if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0}; double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta); double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) { if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -152,6 +158,8 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
} }
} }
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
} }
VectorOperations<double>::normalize(quatDelta, rotVec, 3); VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3); VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);